continue to work on replacing SAP broad-phase by dynamic AABB tree
This commit is contained in:
Daniel Chappuis
parent
76cb11a74f
commit
643ca41922
|
|
@ -25,32 +25,129 @@
|
|||
|
||||
// Libraries
|
||||
#include "CollisionBody.h"
|
||||
#include "../engine/CollisionWorld.h"
|
||||
#include "../engine/ContactManifold.h"
|
||||
|
||||
// We want to use the ReactPhysics3D namespace
|
||||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor
|
||||
CollisionBody::CollisionBody(const Transform& transform, CollisionShape *collisionShape,
|
||||
bodyindex id)
|
||||
: Body(id), mType(DYNAMIC), mCollisionShape(collisionShape), mTransform(transform),
|
||||
mHasMoved(false), mContactManifoldsList(NULL) {
|
||||
|
||||
assert(collisionShape);
|
||||
CollisionBody::CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id)
|
||||
: Body(id), mType(DYNAMIC), mTransform(transform), mProxyCollisionShapes(NULL),
|
||||
mNbCollisionShapes(0), mContactManifoldsList(NULL), mWorld(world) {
|
||||
|
||||
mIsCollisionEnabled = true;
|
||||
mInterpolationFactor = 0.0;
|
||||
|
||||
// Initialize the old transform
|
||||
mOldTransform = transform;
|
||||
|
||||
// Initialize the AABB for broad-phase collision detection
|
||||
mCollisionShape->updateAABB(mAabb, transform);
|
||||
}
|
||||
|
||||
// Destructor
|
||||
CollisionBody::~CollisionBody() {
|
||||
assert(mContactManifoldsList == NULL);
|
||||
|
||||
// Remove all the proxy collision shapes of the body
|
||||
removeAllCollisionShapes();
|
||||
}
|
||||
|
||||
// Add a collision shape to the body.
|
||||
/// This methods will create a copy of the collision shape you provided inside the world and
|
||||
/// return a pointer to the actual collision shape in the world. You can use this pointer to
|
||||
/// remove the collision from the body. Note that when the body is destroyed, all the collision
|
||||
/// shapes will also be destroyed automatically. Because an internal copy of the collision shape
|
||||
/// you provided is performed, you can delete it right after calling this method. The second
|
||||
/// parameter is the transformation that transform the local-space of the collision shape into
|
||||
/// the local-space of the body. By default, the second parameter is the identity transform.
|
||||
/// This method will return a pointer to the proxy collision shape that links the body with
|
||||
/// the collision shape you have added.
|
||||
const ProxyShape* CollisionBody::addCollisionShape(const CollisionShape& collisionShape,
|
||||
const Transform& transform) {
|
||||
|
||||
// Create an internal copy of the collision shape into the world (if it does not exist yet)
|
||||
CollisionShape* newCollisionShape = mWorld.createCollisionShape(collisionShape);
|
||||
|
||||
// Create a new proxy collision shape to attach the collision shape to the body
|
||||
ProxyShape* proxyShape = newCollisionShape->createProxyShape(mWorld.mMemoryAllocator,
|
||||
this, transform, decimal(1.0));
|
||||
|
||||
// Add it to the list of proxy collision shapes of the body
|
||||
if (mProxyCollisionShapes == NULL) {
|
||||
mProxyCollisionShapes = proxyShape;
|
||||
}
|
||||
else {
|
||||
proxyShape->mNext = mProxyCollisionShapes;
|
||||
mProxyCollisionShapes = proxyShape;
|
||||
}
|
||||
|
||||
// Notify the collision detection about this new collision shape
|
||||
mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape);
|
||||
|
||||
mNbCollisionShapes++;
|
||||
|
||||
// Return a pointer to the collision shape
|
||||
return proxyShape;
|
||||
}
|
||||
|
||||
// Remove a collision shape from the body
|
||||
void CollisionBody::removeCollisionShape(const CollisionShape* collisionShape) {
|
||||
|
||||
ProxyShape* current = mProxyCollisionShapes;
|
||||
|
||||
// If the the first proxy shape is the one to remove
|
||||
if (current->getCollisionShape() == collisionShape) {
|
||||
mProxyCollisionShapes = current->mNext;
|
||||
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
|
||||
size_t sizeBytes = current->getSizeInBytes();
|
||||
current->ProxyShape::~ProxyShape();
|
||||
mWorld.mMemoryAllocator.release(current, sizeBytes);
|
||||
mNbCollisionShapes--;
|
||||
return;
|
||||
}
|
||||
|
||||
// Look for the proxy shape that contains the collision shape in parameter
|
||||
while(current->mNext != NULL) {
|
||||
|
||||
// If we have found the collision shape to remove
|
||||
if (current->mNext->getCollisionShape() == collisionShape) {
|
||||
|
||||
// Remove the proxy collision shape
|
||||
ProxyShape* elementToRemove = current->mNext;
|
||||
current->mNext = elementToRemove->mNext;
|
||||
mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove);
|
||||
size_t sizeBytes = elementToRemove->getSizeInBytes();
|
||||
elementToRemove->ProxyShape::~ProxyShape();
|
||||
mWorld.mMemoryAllocator.release(elementToRemove, sizeBytes);
|
||||
mNbCollisionShapes--;
|
||||
return;
|
||||
}
|
||||
|
||||
// Get the next element in the list
|
||||
current = current->mNext;
|
||||
}
|
||||
|
||||
assert(mNbCollisionShapes >= 0);
|
||||
}
|
||||
|
||||
// Remove all the collision shapes
|
||||
void CollisionBody::removeAllCollisionShapes() {
|
||||
|
||||
ProxyShape* current = mProxyCollisionShapes;
|
||||
|
||||
// Look for the proxy shape that contains the collision shape in parameter
|
||||
while(current != NULL) {
|
||||
|
||||
// Remove the proxy collision shape
|
||||
ProxyShape* nextElement = current->mNext;
|
||||
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
|
||||
current->ProxyShape::~ProxyShape();
|
||||
mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
|
||||
|
||||
// Get the next element in the list
|
||||
current = nextElement;
|
||||
}
|
||||
|
||||
mProxyCollisionShapes = NULL;
|
||||
}
|
||||
|
||||
// Reset the contact manifold lists
|
||||
|
|
@ -71,3 +168,18 @@ void CollisionBody::resetContactManifoldsList(MemoryAllocator& memoryAllocator)
|
|||
|
||||
assert(mContactManifoldsList == NULL);
|
||||
}
|
||||
|
||||
// Update the broad-phase state for this body (because it has moved for instance)
|
||||
void CollisionBody::updateBroadPhaseState() const {
|
||||
|
||||
// For all the proxy collision shapes of the body
|
||||
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
|
||||
|
||||
// Recompute the world-space AABB of the collision shape
|
||||
AABB aabb;
|
||||
shape->getCollisionShape()->computeAABB(aabb, mTransform);
|
||||
|
||||
// Update the broad-phase state for the proxy collision shape
|
||||
mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -41,6 +41,8 @@ namespace reactphysics3d {
|
|||
|
||||
// Class declarations
|
||||
struct ContactManifoldListElement;
|
||||
class ProxyShape;
|
||||
class CollisionWorld;
|
||||
|
||||
/// Enumeration for the type of a body
|
||||
/// STATIC : A static body has infinite mass, zero velocity but the position can be
|
||||
|
|
@ -67,9 +69,6 @@ class CollisionBody : public Body {
|
|||
/// Type of body (static, kinematic or dynamic)
|
||||
BodyType mType;
|
||||
|
||||
/// Collision shape of the body
|
||||
CollisionShape* mCollisionShape;
|
||||
|
||||
/// Position and orientation of the body
|
||||
Transform mTransform;
|
||||
|
||||
|
|
@ -82,15 +81,18 @@ class CollisionBody : public Body {
|
|||
/// True if the body can collide with others bodies
|
||||
bool mIsCollisionEnabled;
|
||||
|
||||
/// AABB for Broad-Phase collision detection
|
||||
AABB mAabb;
|
||||
/// First element of the linked list of proxy collision shapes of this body
|
||||
ProxyShape* mProxyCollisionShapes;
|
||||
|
||||
/// True if the body has moved during the last frame
|
||||
bool mHasMoved;
|
||||
/// Number of collision shapes
|
||||
uint mNbCollisionShapes;
|
||||
|
||||
/// First element of the linked list of contact manifolds involving this body
|
||||
ContactManifoldListElement* mContactManifoldsList;
|
||||
|
||||
/// Reference to the world the body belongs to
|
||||
CollisionWorld& mWorld;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Private copy-constructor
|
||||
|
|
@ -102,18 +104,21 @@ class CollisionBody : public Body {
|
|||
/// Reset the contact manifold lists
|
||||
void resetContactManifoldsList(MemoryAllocator& memoryAllocator);
|
||||
|
||||
/// Remove all the collision shapes
|
||||
void removeAllCollisionShapes();
|
||||
|
||||
/// Update the old transform with the current one.
|
||||
void updateOldTransform();
|
||||
|
||||
/// Update the Axis-Aligned Bounding Box coordinates
|
||||
void updateAABB();
|
||||
/// Update the broad-phase state for this body (because it has moved for instance)
|
||||
void updateBroadPhaseState() const;
|
||||
|
||||
public :
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
CollisionBody(const Transform& transform, CollisionShape* collisionShape, bodyindex id);
|
||||
CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id);
|
||||
|
||||
/// Destructor
|
||||
virtual ~CollisionBody();
|
||||
|
|
@ -124,20 +129,18 @@ class CollisionBody : public Body {
|
|||
/// Set the type of the body
|
||||
void setType(BodyType type);
|
||||
|
||||
/// Return the collision shape
|
||||
CollisionShape* getCollisionShape() const;
|
||||
|
||||
/// Set the collision shape
|
||||
void setCollisionShape(CollisionShape* collisionShape);
|
||||
|
||||
/// Return the current position and orientation
|
||||
const Transform& getTransform() const;
|
||||
|
||||
/// Set the current position and orientation
|
||||
void setTransform(const Transform& transform);
|
||||
|
||||
/// Return the AAABB of the body
|
||||
const AABB& getAABB() const;
|
||||
/// Add a collision shape to the body.
|
||||
const ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
|
||||
const Transform& transform = Transform::identity());
|
||||
|
||||
/// Remove a collision shape from the body
|
||||
virtual void removeCollisionShape(const CollisionShape* collisionShape);
|
||||
|
||||
/// Return the interpolated transform for rendering
|
||||
Transform getInterpolatedTransform() const;
|
||||
|
|
@ -158,6 +161,7 @@ class CollisionBody : public Body {
|
|||
|
||||
friend class DynamicsWorld;
|
||||
friend class CollisionDetection;
|
||||
friend class BroadPhaseAlgorithm;
|
||||
};
|
||||
|
||||
// Return the type of the body
|
||||
|
|
@ -168,18 +172,12 @@ inline BodyType CollisionBody::getType() const {
|
|||
// Set the type of the body
|
||||
inline void CollisionBody::setType(BodyType type) {
|
||||
mType = type;
|
||||
}
|
||||
|
||||
// Return the collision shape
|
||||
inline CollisionShape* CollisionBody::getCollisionShape() const {
|
||||
assert(mCollisionShape);
|
||||
return mCollisionShape;
|
||||
}
|
||||
if (mType == STATIC) {
|
||||
|
||||
// Set the collision shape
|
||||
inline void CollisionBody::setCollisionShape(CollisionShape* collisionShape) {
|
||||
assert(collisionShape);
|
||||
mCollisionShape = collisionShape;
|
||||
// Update the broad-phase state of the body
|
||||
updateBroadPhaseState();
|
||||
}
|
||||
}
|
||||
|
||||
// Return the interpolated transform for rendering
|
||||
|
|
@ -201,20 +199,14 @@ inline const Transform& CollisionBody::getTransform() const {
|
|||
// Set the current position and orientation
|
||||
inline void CollisionBody::setTransform(const Transform& transform) {
|
||||
|
||||
// Check if the body has moved
|
||||
if (mTransform != transform) {
|
||||
mHasMoved = true;
|
||||
}
|
||||
|
||||
// Update the transform of the body
|
||||
mTransform = transform;
|
||||
|
||||
// Update the broad-phase state of the body
|
||||
updateBroadPhaseState();
|
||||
}
|
||||
|
||||
// Return the AAABB of the body
|
||||
inline const AABB& CollisionBody::getAABB() const {
|
||||
return mAabb;
|
||||
}
|
||||
|
||||
// Return true if the body can collide with others bodies
|
||||
// Return true if the body can collide with others bodies
|
||||
inline bool CollisionBody::isCollisionEnabled() const {
|
||||
return mIsCollisionEnabled;
|
||||
}
|
||||
|
|
@ -230,13 +222,6 @@ inline void CollisionBody::updateOldTransform() {
|
|||
mOldTransform = mTransform;
|
||||
}
|
||||
|
||||
// Update the rigid body in order to reflect a change in the body state
|
||||
inline void CollisionBody::updateAABB() {
|
||||
|
||||
// Update the AABB
|
||||
mCollisionShape->updateAABB(mAabb, mTransform);
|
||||
}
|
||||
|
||||
// Return the first element of the linked list of contact manifolds involving this body
|
||||
inline const ContactManifoldListElement* CollisionBody::getContactManifoldsLists() const {
|
||||
return mContactManifoldsList;
|
||||
|
|
|
|||
|
|
@ -27,29 +27,20 @@
|
|||
#include "RigidBody.h"
|
||||
#include "constraint/Joint.h"
|
||||
#include "../collision/shapes/CollisionShape.h"
|
||||
#include "../engine/CollisionWorld.h"
|
||||
|
||||
// We want to use the ReactPhysics3D namespace
|
||||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor
|
||||
RigidBody::RigidBody(const Transform& transform, decimal mass, CollisionShape *collisionShape,
|
||||
bodyindex id)
|
||||
: CollisionBody(transform, collisionShape, id), mInitMass(mass), mIsGravityEnabled(true),
|
||||
mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), mJointsList(NULL) {
|
||||
|
||||
assert(collisionShape);
|
||||
|
||||
// If the mass is not positive, set it to one
|
||||
if (mInitMass <= decimal(0.0)) {
|
||||
mInitMass = decimal(1.0);
|
||||
}
|
||||
|
||||
// Compute the inertia tensor using the collision shape of the body
|
||||
mCollisionShape->computeLocalInertiaTensor(mInertiaTensorLocal, mInitMass);
|
||||
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
|
||||
RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id)
|
||||
: CollisionBody(transform, world, id), mInitMass(decimal(1.0)),
|
||||
mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
|
||||
mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)),
|
||||
mJointsList(NULL) {
|
||||
|
||||
// Compute the inverse mass
|
||||
mMassInverse = decimal(1.0) / mass;
|
||||
mMassInverse = decimal(1.0) / mInitMass;
|
||||
}
|
||||
|
||||
// Destructor
|
||||
|
|
@ -64,6 +55,9 @@ void RigidBody::setType(BodyType type) {
|
|||
|
||||
CollisionBody::setType(type);
|
||||
|
||||
// Recompute the total mass, center of mass and inertia tensor
|
||||
recomputeMassInformation();
|
||||
|
||||
// If it is a static body
|
||||
if (mType == STATIC) {
|
||||
|
||||
|
|
@ -77,7 +71,8 @@ void RigidBody::setType(BodyType type) {
|
|||
|
||||
// Reset the inverse mass and inverse inertia tensor to zero
|
||||
mMassInverse = decimal(0.0);
|
||||
mInertiaTensorLocalInverse = Matrix3x3::zero();
|
||||
mInertiaTensorLocal.setToZero();
|
||||
mInertiaTensorLocalInverse.setToZero();
|
||||
|
||||
}
|
||||
else { // If it is a dynamic body
|
||||
|
|
@ -89,24 +84,6 @@ void RigidBody::setType(BodyType type) {
|
|||
setIsSleeping(false);
|
||||
}
|
||||
|
||||
// Method that set the mass of the body
|
||||
void RigidBody::setMass(decimal mass) {
|
||||
mInitMass = mass;
|
||||
|
||||
// If the mass is negative, set it to one
|
||||
if (mInitMass <= decimal(0.0)) {
|
||||
mInitMass = decimal(1.0);
|
||||
}
|
||||
|
||||
// Recompute the inverse mass
|
||||
if (mType == DYNAMIC) {
|
||||
mMassInverse = decimal(1.0) / mInitMass;
|
||||
}
|
||||
else {
|
||||
mMassInverse = decimal(0.0);
|
||||
}
|
||||
}
|
||||
|
||||
// Set the local inertia tensor of the body (in body coordinates)
|
||||
void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
|
||||
mInertiaTensorLocal = inertiaTensorLocal;
|
||||
|
|
@ -148,4 +125,131 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons
|
|||
}
|
||||
}
|
||||
|
||||
// Add a collision shape to the body.
|
||||
/// This methods will create a copy of the collision shape you provided inside the world and
|
||||
/// return a pointer to the actual collision shape in the world. You can use this pointer to
|
||||
/// remove the collision from the body. Note that when the body is destroyed, all the collision
|
||||
/// shapes will also be destroyed automatically. Because an internal copy of the collision shape
|
||||
/// you provided is performed, you can delete it right after calling this method. The second
|
||||
/// parameter is the transformation that transform the local-space of the collision shape into
|
||||
/// the local-space of the body. By default, the second parameter is the identity transform.
|
||||
/// The third parameter is the mass of the collision shape (this will used to compute the
|
||||
/// total mass of the rigid body and its inertia tensor). The mass must be positive.
|
||||
const CollisionShape* RigidBody::addCollisionShape(const CollisionShape& collisionShape,
|
||||
decimal mass,
|
||||
const Transform& transform
|
||||
) {
|
||||
|
||||
assert(mass > decimal(0.0));
|
||||
|
||||
// Create an internal copy of the collision shape into the world if it is not there yet
|
||||
CollisionShape* newCollisionShape = mWorld.createCollisionShape(collisionShape);
|
||||
|
||||
// Create a new proxy collision shape to attach the collision shape to the body
|
||||
ProxyShape* proxyShape = newCollisionShape->createProxyShape(mWorld.mMemoryAllocator,
|
||||
this, transform, mass);
|
||||
|
||||
// Add it to the list of proxy collision shapes of the body
|
||||
if (mProxyCollisionShapes == NULL) {
|
||||
mProxyCollisionShapes = proxyShape;
|
||||
}
|
||||
else {
|
||||
proxyShape->mNext = mProxyCollisionShapes;
|
||||
mProxyCollisionShapes = proxyShape;
|
||||
}
|
||||
|
||||
// Notify the collision detection about this new collision shape
|
||||
mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape);
|
||||
|
||||
mNbCollisionShapes++;
|
||||
|
||||
// Recompute the center of mass, total mass and inertia tensor of the body with the new
|
||||
// collision shape
|
||||
recomputeMassInformation();
|
||||
|
||||
// Return a pointer to the collision shape
|
||||
return newCollisionShape;
|
||||
}
|
||||
|
||||
// Remove a collision shape from the body
|
||||
void RigidBody::removeCollisionShape(const CollisionShape* collisionShape) {
|
||||
|
||||
// Remove the collision shape
|
||||
CollisionBody::removeCollisionShape(collisionShape);
|
||||
|
||||
// Recompute the total mass, center of mass and inertia tensor
|
||||
recomputeMassInformation();
|
||||
}
|
||||
|
||||
// Recompute the center of mass, total mass and inertia tensor of the body using all
|
||||
// the collision shapes attached to the body.
|
||||
void RigidBody::recomputeMassInformation() {
|
||||
|
||||
mInitMass = decimal(0.0);
|
||||
mMassInverse = decimal(0.0);
|
||||
mInertiaTensorLocal.setToZero();
|
||||
mInertiaTensorLocalInverse.setToZero();
|
||||
mCenterOfMassLocal.setToZero();
|
||||
|
||||
// If it is STATIC or KINEMATIC body
|
||||
if (mType == STATIC || mType == KINEMATIC) {
|
||||
mCenterOfMassWorld = mTransform.getPosition();
|
||||
return;
|
||||
}
|
||||
|
||||
assert(mType == DYNAMIC);
|
||||
|
||||
// Compute the total mass of the body
|
||||
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
|
||||
mInitMass += shape->getMass();
|
||||
mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass();
|
||||
}
|
||||
|
||||
if (mInitMass > decimal(0.0)) {
|
||||
mMassInverse = decimal(1.0) / mInitMass;
|
||||
}
|
||||
else {
|
||||
mInitMass = decimal(1.0);
|
||||
mMassInverse = decimal(1.0);
|
||||
}
|
||||
|
||||
// Compute the center of mass
|
||||
const Vector3 oldCenterOfMass = mCenterOfMassWorld;
|
||||
mCenterOfMassLocal *= mMassInverse;
|
||||
mCenterOfMassWorld = mTransform * mCenterOfMassLocal;
|
||||
|
||||
// Compute the total mass and inertia tensor using all the collision shapes
|
||||
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
|
||||
|
||||
// Get the inertia tensor of the collision shape in its local-space
|
||||
Matrix3x3 inertiaTensor;
|
||||
shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass());
|
||||
|
||||
// Convert the collision shape inertia tensor into the local-space of the body
|
||||
const Transform& shapeTransform = shape->getLocalToBodyTransform();
|
||||
Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix();
|
||||
inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose();
|
||||
|
||||
// Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape
|
||||
// center into a inertia tensor w.r.t to the body origin.
|
||||
Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal;
|
||||
decimal offsetSquare = offset.lengthSquare();
|
||||
Matrix3x3 offsetMatrix;
|
||||
offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0));
|
||||
offsetMatrix[1].setAllValues(decimal(0.0), offsetSquare, decimal(0.0));
|
||||
offsetMatrix[2].setAllValues(decimal(0.0), decimal(0.0), offsetSquare);
|
||||
offsetMatrix[0] += offset * (-offset.x);
|
||||
offsetMatrix[1] += offset * (-offset.y);
|
||||
offsetMatrix[2] += offset * (-offset.z);
|
||||
offsetMatrix *= shape->getMass();
|
||||
|
||||
mInertiaTensorLocal += inertiaTensor + offsetMatrix;
|
||||
}
|
||||
|
||||
// Compute the local inverse inertia tensor
|
||||
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
|
||||
|
||||
// Update the linear velocity of the center of mass
|
||||
mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -57,6 +57,13 @@ class RigidBody : public CollisionBody {
|
|||
/// Intial mass of the body
|
||||
decimal mInitMass;
|
||||
|
||||
/// Center of mass of the body in local-space coordinates.
|
||||
/// The center of mass can therefore be different from the body origin
|
||||
Vector3 mCenterOfMassLocal;
|
||||
|
||||
/// Center of mass of the body in world-space coordinates
|
||||
Vector3 mCenterOfMassWorld;
|
||||
|
||||
/// Linear velocity of the body
|
||||
Vector3 mLinearVelocity;
|
||||
|
||||
|
|
@ -69,7 +76,8 @@ class RigidBody : public CollisionBody {
|
|||
/// Current external torque on the body
|
||||
Vector3 mExternalTorque;
|
||||
|
||||
/// Local inertia tensor of the body (in local-space)
|
||||
/// Local inertia tensor of the body (in local-space) with respect to the
|
||||
/// center of mass of the body
|
||||
Matrix3x3 mInertiaTensorLocal;
|
||||
|
||||
/// Inverse of the inertia tensor of the body
|
||||
|
|
@ -104,13 +112,15 @@ class RigidBody : public CollisionBody {
|
|||
/// Remove a joint from the joints list
|
||||
void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint);
|
||||
|
||||
/// Update the transform of the body after a change of the center of mass
|
||||
void updateTransformWithCenterOfMass();
|
||||
|
||||
public :
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
RigidBody(const Transform& transform, decimal mass, CollisionShape* collisionShape,
|
||||
bodyindex id);
|
||||
RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id);
|
||||
|
||||
/// Destructor
|
||||
virtual ~RigidBody();
|
||||
|
|
@ -121,9 +131,6 @@ class RigidBody : public CollisionBody {
|
|||
/// Return the mass of the body
|
||||
decimal getMass() const;
|
||||
|
||||
/// Set the mass of the body
|
||||
void setMass(decimal mass);
|
||||
|
||||
/// Return the linear velocity
|
||||
Vector3 getLinearVelocity() const;
|
||||
|
||||
|
|
@ -178,8 +185,8 @@ class RigidBody : public CollisionBody {
|
|||
/// Set the variable to know whether or not the body is sleeping
|
||||
virtual void setIsSleeping(bool isSleeping);
|
||||
|
||||
/// Apply an external force to the body at its gravity center.
|
||||
void applyForceToCenter(const Vector3& force);
|
||||
/// Apply an external force to the body at its center of mass.
|
||||
void applyForceToCenterOfMass(const Vector3& force);
|
||||
|
||||
/// Apply an external force to the body at a given point (in world-space coordinates).
|
||||
void applyForce(const Vector3& force, const Vector3& point);
|
||||
|
|
@ -187,6 +194,18 @@ class RigidBody : public CollisionBody {
|
|||
/// Apply an external torque to the body.
|
||||
void applyTorque(const Vector3& torque);
|
||||
|
||||
/// Add a collision shape to the body.
|
||||
const CollisionShape* addCollisionShape(const CollisionShape& collisionShape,
|
||||
decimal mass,
|
||||
const Transform& transform = Transform::identity());
|
||||
|
||||
/// Remove a collision shape from the body
|
||||
virtual void removeCollisionShape(const CollisionShape* collisionShape);
|
||||
|
||||
/// Recompute the center of mass, total mass and inertia tensor of the body using all
|
||||
/// the collision shapes attached to the body.
|
||||
void recomputeMassInformation();
|
||||
|
||||
// -------------------- Friendship -------------------- //
|
||||
|
||||
friend class DynamicsWorld;
|
||||
|
|
@ -329,12 +348,12 @@ inline void RigidBody::setIsSleeping(bool isSleeping) {
|
|||
Body::setIsSleeping(isSleeping);
|
||||
}
|
||||
|
||||
// Apply an external force to the body at its gravity center.
|
||||
// Apply an external force to the body at its center of mass.
|
||||
/// If the body is sleeping, calling this method will wake it up. Note that the
|
||||
/// force will we added to the sum of the applied forces and that this sum will be
|
||||
/// reset to zero at the end of each call of the DynamicsWorld::update() method.
|
||||
/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
|
||||
inline void RigidBody::applyForceToCenter(const Vector3& force) {
|
||||
inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) {
|
||||
|
||||
// If it is not a dynamic body, we do nothing
|
||||
if (mType != DYNAMIC) return;
|
||||
|
|
@ -349,7 +368,7 @@ inline void RigidBody::applyForceToCenter(const Vector3& force) {
|
|||
}
|
||||
|
||||
// Apply an external force to the body at a given point (in world-space coordinates).
|
||||
/// If the point is not at the center of gravity of the body, it will also
|
||||
/// If the point is not at the center of mass of the body, it will also
|
||||
/// generate some torque and therefore, change the angular velocity of the body.
|
||||
/// If the body is sleeping, calling this method will wake it up. Note that the
|
||||
/// force will we added to the sum of the applied forces and that this sum will be
|
||||
|
|
@ -389,6 +408,13 @@ inline void RigidBody::applyTorque(const Vector3& torque) {
|
|||
mExternalTorque += torque;
|
||||
}
|
||||
|
||||
/// Update the transform of the body after a change of the center of mass
|
||||
inline void RigidBody::updateTransformWithCenterOfMass() {
|
||||
|
||||
// Translate the body according to the translation of the center of mass position
|
||||
mTransform.setPosition(mCenterOfMassWorld - mTransform.getOrientation() * mCenterOfMassLocal);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -32,6 +32,7 @@
|
|||
/// ReactPhysics3D namespace
|
||||
namespace reactphysics3d {
|
||||
|
||||
// TODO : DELETE THIS CLASS
|
||||
// Structure BroadPhasePair
|
||||
/**
|
||||
* This structure represents a pair of bodies
|
||||
|
|
@ -79,16 +80,7 @@ struct BroadPhasePair {
|
|||
bool operator!=(const BroadPhasePair& broadPhasePair2) const;
|
||||
};
|
||||
|
||||
// Return the pair of bodies index
|
||||
inline bodyindexpair BroadPhasePair::computeBodiesIndexPair(CollisionBody* body1,
|
||||
CollisionBody* body2) {
|
||||
// Construct the pair of body index
|
||||
bodyindexpair indexPair = body1->getID() < body2->getID() ?
|
||||
std::make_pair(body1->getID(), body2->getID()) :
|
||||
std::make_pair(body2->getID(), body1->getID());
|
||||
assert(indexPair.first != indexPair.second);
|
||||
return indexPair;
|
||||
}
|
||||
|
||||
|
||||
// Return the pair of bodies index
|
||||
inline bodyindexpair BroadPhasePair::getBodiesIndexPair() const {
|
||||
|
|
|
|||
|
|
@ -44,20 +44,16 @@ using namespace std;
|
|||
|
||||
// Constructor
|
||||
CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator)
|
||||
: mWorld(world), mMemoryAllocator(memoryAllocator),
|
||||
: mWorld(world), mBroadPhaseAlgorithm(*this),
|
||||
mNarrowPhaseGJKAlgorithm(memoryAllocator),
|
||||
mNarrowPhaseSphereVsSphereAlgorithm(memoryAllocator) {
|
||||
mNarrowPhaseSphereVsSphereAlgorithm(memoryAllocator),
|
||||
mIsCollisionShapesAdded(false) {
|
||||
|
||||
// Create the broad-phase algorithm that will be used (Sweep and Prune with AABB)
|
||||
mBroadPhaseAlgorithm = new SweepAndPruneAlgorithm(*this);
|
||||
assert(mBroadPhaseAlgorithm != NULL);
|
||||
}
|
||||
|
||||
// Destructor
|
||||
CollisionDetection::~CollisionDetection() {
|
||||
|
||||
// Delete the broad-phase algorithm
|
||||
delete mBroadPhaseAlgorithm;
|
||||
}
|
||||
|
||||
// Compute the collision detection
|
||||
|
|
@ -77,58 +73,58 @@ void CollisionDetection::computeBroadPhase() {
|
|||
|
||||
PROFILE("CollisionDetection::computeBroadPhase()");
|
||||
|
||||
// Notify the broad-phase algorithm about the bodies that have moved since last frame
|
||||
for (set<CollisionBody*>::iterator it = mWorld->getBodiesBeginIterator();
|
||||
it != mWorld->getBodiesEndIterator(); it++) {
|
||||
// If new collision shapes have been added to bodies
|
||||
if (mIsCollisionShapesAdded) {
|
||||
|
||||
// If the body has moved
|
||||
if ((*it)->mHasMoved) {
|
||||
|
||||
// Notify the broad-phase that the body has moved
|
||||
mBroadPhaseAlgorithm->updateObject(*it, (*it)->getAABB());
|
||||
}
|
||||
}
|
||||
// Ask the broad-phase to recompute the overlapping pairs of collision
|
||||
// shapes. This call can only add new overlapping pairs in the collision
|
||||
// detection.
|
||||
mBroadPhaseAlgorithm.computeOverlappingPairs();
|
||||
}
|
||||
}
|
||||
|
||||
// Compute the narrow-phase collision detection
|
||||
void CollisionDetection::computeNarrowPhase() {
|
||||
|
||||
PROFILE("CollisionDetection::computeNarrowPhase()");
|
||||
|
||||
map<bodyindexpair, BroadPhasePair*>::iterator it;
|
||||
|
||||
// For each possible collision pair of bodies
|
||||
map<overlappingpairid, OverlappingPair*>::iterator it;
|
||||
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); it++) {
|
||||
ContactPointInfo* contactInfo = NULL;
|
||||
|
||||
BroadPhasePair* pair = (*it).second;
|
||||
assert(pair != NULL);
|
||||
OverlappingPair* pair = it->second;
|
||||
|
||||
CollisionBody* const body1 = pair->body1;
|
||||
CollisionBody* const body2 = pair->body2;
|
||||
ProxyShape* shape1 = pair->getShape1();
|
||||
ProxyShape* shape2 = pair->getShape2();
|
||||
CollisionBody* const body1 = shape1->getBody();
|
||||
CollisionBody* const body2 = shape2->getBody();
|
||||
|
||||
// Update the contact cache of the overlapping pair
|
||||
mWorld->updateOverlappingPair(pair);
|
||||
|
||||
// Check if the two bodies are allowed to collide, otherwise, we do not test for collision
|
||||
if (pair->body1->getType() != DYNAMIC && pair->body2->getType() != DYNAMIC) continue;
|
||||
if (mNoCollisionPairs.count(pair->getBodiesIndexPair()) > 0) continue;
|
||||
if (body1->getType() != DYNAMIC && body2->getType() != DYNAMIC) continue;
|
||||
bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
|
||||
if (mNoCollisionPairs.count(bodiesIndex) > 0) continue;
|
||||
|
||||
// Check if the two bodies are sleeping, if so, we do no test collision between them
|
||||
if (body1->isSleeping() && body2->isSleeping()) continue;
|
||||
|
||||
// Select the narrow phase algorithm to use according to the two collision shapes
|
||||
NarrowPhaseAlgorithm& narrowPhaseAlgorithm = SelectNarrowPhaseAlgorithm(
|
||||
body1->getCollisionShape(),
|
||||
body2->getCollisionShape());
|
||||
shape1->getCollisionShape(),
|
||||
shape2->getCollisionShape());
|
||||
|
||||
// Notify the narrow-phase algorithm about the overlapping pair we are going to test
|
||||
narrowPhaseAlgorithm.setCurrentOverlappingPair(pair);
|
||||
|
||||
// Use the narrow-phase collision detection algorithm to check
|
||||
// if there really is a collision
|
||||
if (narrowPhaseAlgorithm.testCollision(body1->getCollisionShape(), body1->getTransform(),
|
||||
body2->getCollisionShape(), body2->getTransform(),
|
||||
const Transform transform1 = body1->getTransform() * shape1->getLocalToBodyTransform();
|
||||
const Transform transform2 = body2->getTransform() * shape2->getLocalToBodyTransform();
|
||||
if (narrowPhaseAlgorithm.testCollision(shape1->getCollisionShape(), transform1,
|
||||
shape2->getCollisionShape(), transform2,
|
||||
contactInfo)) {
|
||||
assert(contactInfo != NULL);
|
||||
|
||||
|
|
@ -143,20 +139,38 @@ void CollisionDetection::computeNarrowPhase() {
|
|||
|
||||
// Delete and remove the contact info from the memory allocator
|
||||
contactInfo->ContactPointInfo::~ContactPointInfo();
|
||||
mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
|
||||
mWorld->mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Allow the broadphase to notify the collision detection about an overlapping pair.
|
||||
/// This method is called by a broad-phase collision detection algorithm
|
||||
void CollisionDetection::broadPhaseNotifyAddedOverlappingPair(BodyPair* addedPair) {
|
||||
void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) {
|
||||
|
||||
// Compute the overlapping pair ID
|
||||
overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2);
|
||||
|
||||
// Check if the overlapping pair already exists
|
||||
if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return;
|
||||
|
||||
// Create the overlapping pair and add it into the set of overlapping pairs
|
||||
OverlappingPair* newPair = new (mWorld->mMemoryAllocator.allocate(sizeof(OverlappingPair)))
|
||||
OverlappingPair(shape1, shape2, mWorld->mMemoryAllocator);
|
||||
assert(newPair != NULL);
|
||||
std::pair<map<overlappingpairid, OverlappingPair*>::iterator, bool> check =
|
||||
mOverlappingPairs.insert(make_pair(pairID, newPair));
|
||||
assert(check.second);
|
||||
|
||||
/* TODO : DELETE THIS
|
||||
// Get the pair of body index
|
||||
bodyindexpair indexPair = addedPair->getBodiesIndexPair();
|
||||
|
||||
// If the overlapping pair already exists, we don't do anything
|
||||
if (mOverlappingPairs.count(indexPair) > 0) return;
|
||||
|
||||
// Create the corresponding broad-phase pair object
|
||||
BroadPhasePair* broadPhasePair = new (mMemoryAllocator.allocate(sizeof(BroadPhasePair)))
|
||||
BroadPhasePair* broadPhasePair = new (mWorld->mMemoryAllocator.allocate(sizeof(BroadPhasePair)))
|
||||
BroadPhasePair(addedPair->body1, addedPair->body2);
|
||||
assert(broadPhasePair != NULL);
|
||||
|
||||
|
|
@ -168,23 +182,47 @@ void CollisionDetection::broadPhaseNotifyAddedOverlappingPair(BodyPair* addedPai
|
|||
|
||||
// Notify the world about the new broad-phase overlapping pair
|
||||
mWorld->notifyAddedOverlappingPair(broadPhasePair);
|
||||
*/
|
||||
}
|
||||
|
||||
// Allow the broadphase to notify the collision detection about a removed overlapping pair
|
||||
void CollisionDetection::broadPhaseNotifyRemovedOverlappingPair(BodyPair* removedPair) {
|
||||
void CollisionDetection::removeOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) {
|
||||
|
||||
// Get the pair of body index
|
||||
bodyindexpair indexPair = removedPair->getBodiesIndexPair();
|
||||
// Compute the overlapping pair ID
|
||||
overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2);
|
||||
|
||||
// Get the broad-phase pair
|
||||
BroadPhasePair* broadPhasePair = mOverlappingPairs.find(indexPair)->second;
|
||||
assert(broadPhasePair != NULL);
|
||||
// If the overlapping
|
||||
std::map<overlappingpairid, OverlappingPair*>::iterator it;
|
||||
it = mOverlappingPairs.find(indexPair);
|
||||
if ()
|
||||
|
||||
// Notify the world about the removed broad-phase pair
|
||||
mWorld->notifyRemovedOverlappingPair(broadPhasePair);
|
||||
// TODO : DELETE THIS
|
||||
//mWorld->notifyRemovedOverlappingPair(broadPhasePair);
|
||||
|
||||
// Remove the overlapping pair from the memory allocator
|
||||
broadPhasePair->BroadPhasePair::~BroadPhasePair();
|
||||
mMemoryAllocator.release(broadPhasePair, sizeof(BroadPhasePair));
|
||||
mOverlappingPairs.erase(indexPair);
|
||||
mOverlappingPairs.find(pairID)->second->OverlappingPair::~OverlappingPair();
|
||||
mWorld->mMemoryAllocator.release(mOverlappingPairs[indexPair], sizeof(OverlappingPair));
|
||||
mOverlappingPairs.erase(pairID);
|
||||
}
|
||||
|
||||
// Remove a body from the collision detection
|
||||
void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
|
||||
|
||||
// Remove all the overlapping pairs involving this proxy shape
|
||||
std::map<overlappingpairid, OverlappingPair*>::iterator it;
|
||||
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
|
||||
if (it->second->getShape1()->getBroadPhaseID() == proxyShape->getBroadPhaseID() ||
|
||||
it->second->getShape2()->getBroadPhaseID() == proxyShape->getBroadPhaseID()) {
|
||||
std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
|
||||
++it;
|
||||
mOverlappingPairs.erase(itToRemove);
|
||||
}
|
||||
else {
|
||||
++it;
|
||||
}
|
||||
}
|
||||
|
||||
// Remove the body from the broad-phase
|
||||
mBroadPhaseAlgorithm.removeProxyCollisionShape(proxyShape);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@
|
|||
// Libraries
|
||||
#include "../body/CollisionBody.h"
|
||||
#include "broadphase/BroadPhaseAlgorithm.h"
|
||||
#include "BroadPhasePair.h"
|
||||
#include "../engine/OverlappingPair.h"
|
||||
#include "narrowphase/GJK/GJKAlgorithm.h"
|
||||
#include "narrowphase/SphereVsSphereAlgorithm.h"
|
||||
#include "../memory/MemoryAllocator.h"
|
||||
|
|
@ -62,14 +62,11 @@ class CollisionDetection {
|
|||
/// Pointer to the physics world
|
||||
CollisionWorld* mWorld;
|
||||
|
||||
/// Memory allocator
|
||||
MemoryAllocator& mMemoryAllocator;
|
||||
|
||||
/// Broad-phase overlapping pairs
|
||||
std::map<bodyindexpair, BroadPhasePair*> mOverlappingPairs;
|
||||
std::map<overlappingpairid, OverlappingPair*> mOverlappingPairs;
|
||||
|
||||
/// Broad-phase algorithm
|
||||
BroadPhaseAlgorithm* mBroadPhaseAlgorithm;
|
||||
BroadPhaseAlgorithm mBroadPhaseAlgorithm;
|
||||
|
||||
/// Narrow-phase GJK algorithm
|
||||
GJKAlgorithm mNarrowPhaseGJKAlgorithm;
|
||||
|
|
@ -80,6 +77,9 @@ class CollisionDetection {
|
|||
/// Set of pair of bodies that cannot collide between each other
|
||||
std::set<bodyindexpair> mNoCollisionPairs;
|
||||
|
||||
/// True if some collision shapes have been added previously
|
||||
bool mIsCollisionShapesAdded;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Private copy-constructor
|
||||
|
|
@ -95,8 +95,11 @@ class CollisionDetection {
|
|||
void computeNarrowPhase();
|
||||
|
||||
/// Select the narrow phase algorithm to use given two collision shapes
|
||||
NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(CollisionShape* collisionShape1,
|
||||
CollisionShape* collisionShape2);
|
||||
NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(const CollisionShape* collisionShape1,
|
||||
const CollisionShape* collisionShape2);
|
||||
|
||||
/// Remove an overlapping pair if it is not overlapping anymore
|
||||
void removeOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
|
||||
|
||||
public :
|
||||
|
||||
|
|
@ -108,31 +111,32 @@ class CollisionDetection {
|
|||
/// Destructor
|
||||
~CollisionDetection();
|
||||
|
||||
/// Add a body to the collision detection
|
||||
void addBody(CollisionBody* body);
|
||||
/// Add a proxy collision shape to the collision detection
|
||||
void addProxyCollisionShape(ProxyShape* proxyShape);
|
||||
|
||||
/// Remove a body from the collision detection
|
||||
void removeBody(CollisionBody* body);
|
||||
/// Remove a proxy collision shape from the collision detection
|
||||
void removeProxyCollisionShape(ProxyShape* proxyShape);
|
||||
|
||||
/// Update a proxy collision shape (that has moved for instance)
|
||||
void updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb);
|
||||
|
||||
/// Add a pair of bodies that cannot collide with each other
|
||||
void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2);
|
||||
|
||||
/// Remove a pair of bodies that cannot collide with each other
|
||||
void removeNoCollisionPair(CollisionBody *body1, CollisionBody *body2);
|
||||
void removeNoCollisionPair(CollisionBody* body1, CollisionBody* body2);
|
||||
|
||||
/// Compute the collision detection
|
||||
void computeCollisionDetection();
|
||||
|
||||
/// Allow the broadphase to notify the collision detection about a new overlapping pair.
|
||||
void broadPhaseNotifyAddedOverlappingPair(BodyPair* pair);
|
||||
|
||||
/// Allow the broadphase to notify the collision detection about a removed overlapping pair
|
||||
void broadPhaseNotifyRemovedOverlappingPair(BodyPair* pair);
|
||||
/// Allow the broadphase to notify the collision detection about an overlapping pair.
|
||||
void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
|
||||
};
|
||||
|
||||
// Select the narrow-phase collision algorithm to use given two collision shapes
|
||||
inline NarrowPhaseAlgorithm& CollisionDetection::SelectNarrowPhaseAlgorithm(
|
||||
CollisionShape* collisionShape1, CollisionShape* collisionShape2) {
|
||||
const CollisionShape* collisionShape1,
|
||||
const CollisionShape* collisionShape2) {
|
||||
|
||||
// Sphere vs Sphere algorithm
|
||||
if (collisionShape1->getType() == SPHERE && collisionShape2->getType() == SPHERE) {
|
||||
|
|
@ -144,29 +148,29 @@ inline NarrowPhaseAlgorithm& CollisionDetection::SelectNarrowPhaseAlgorithm(
|
|||
}
|
||||
|
||||
// Add a body to the collision detection
|
||||
inline void CollisionDetection::addBody(CollisionBody* body) {
|
||||
inline void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape) {
|
||||
|
||||
// Add the body to the broad-phase
|
||||
mBroadPhaseAlgorithm->addObject(body, body->getAABB());
|
||||
}
|
||||
mBroadPhaseAlgorithm.addProxyCollisionShape(proxyShape);
|
||||
|
||||
// Remove a body from the collision detection
|
||||
inline void CollisionDetection::removeBody(CollisionBody* body) {
|
||||
|
||||
// Remove the body from the broad-phase
|
||||
mBroadPhaseAlgorithm->removeObject(body);
|
||||
}
|
||||
mIsCollisionShapesAdded = true;
|
||||
}
|
||||
|
||||
// Add a pair of bodies that cannot collide with each other
|
||||
inline void CollisionDetection::addNoCollisionPair(CollisionBody* body1,
|
||||
CollisionBody* body2) {
|
||||
mNoCollisionPairs.insert(BroadPhasePair::computeBodiesIndexPair(body1, body2));
|
||||
mNoCollisionPairs.insert(OverlappingPair::computeBodiesIndexPair(body1, body2));
|
||||
}
|
||||
|
||||
// Remove a pair of bodies that cannot collide with each other
|
||||
inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1,
|
||||
CollisionBody* body2) {
|
||||
mNoCollisionPairs.erase(BroadPhasePair::computeBodiesIndexPair(body1, body2));
|
||||
mNoCollisionPairs.erase(OverlappingPair::computeBodiesIndexPair(body1, body2));
|
||||
}
|
||||
|
||||
// Update a proxy collision shape (that has moved for instance)
|
||||
inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb) {
|
||||
mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -31,11 +31,223 @@ using namespace reactphysics3d;
|
|||
|
||||
// Constructor
|
||||
BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
|
||||
:mPairManager(collisionDetection), mCollisionDetection(collisionDetection) {
|
||||
:mDynamicAABBTree(*this), mNbMovedShapes(0), mNbAllocatedMovedShapes(8),
|
||||
mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8),
|
||||
mPairManager(collisionDetection), mCollisionDetection(collisionDetection) {
|
||||
|
||||
// Allocate memory for the array of non-static bodies IDs
|
||||
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
|
||||
assert(mMovedShapes);
|
||||
|
||||
// Allocate memory for the array of potential overlapping pairs
|
||||
mPotentialPairs = (BroadPair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPair));
|
||||
assert(mPotentialPairs);
|
||||
}
|
||||
|
||||
// Destructor
|
||||
BroadPhaseAlgorithm::~BroadPhaseAlgorithm() {
|
||||
|
||||
// Release the memory for the array of non-static bodies IDs
|
||||
free(mMovedShapes);
|
||||
|
||||
// Release the memory for the array of potential overlapping pairs
|
||||
free(mPotentialPairs);
|
||||
}
|
||||
|
||||
// Add a collision shape in the array of shapes that have moved in the last simulation step
|
||||
// and that need to be tested again for broad-phase overlapping.
|
||||
void BroadPhaseAlgorithm::addMovedCollisionShape(int broadPhaseID) {
|
||||
|
||||
// Allocate more elements in the array of bodies that have moved if necessary
|
||||
if (mNbAllocatedMovedShapes == mNbMovedShapes) {
|
||||
mNbAllocatedMovedShapes *= 2;
|
||||
int* oldArray = mMovedShapes;
|
||||
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
|
||||
assert(mMovedShapes);
|
||||
memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int));
|
||||
free(oldArray);
|
||||
}
|
||||
|
||||
// Store the broad-phase ID into the array of bodies that have moved
|
||||
mMovedShapes[mNbMovedShapes] = broadPhaseID;
|
||||
mNbMovedShapes++;
|
||||
}
|
||||
|
||||
// Remove a collision shape from the array of shapes that have moved in the last simulation step
|
||||
// and that need to be tested again for broad-phase overlapping.
|
||||
void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
|
||||
|
||||
assert(mNbNonUsedMovedShapes <= mNbMovedShapes);
|
||||
|
||||
// If less than the quarter of allocated elements of the non-static bodies IDs array
|
||||
// are used, we release some allocated memory
|
||||
if ((mNbMovedShapes - mNbNonUsedMovedShapes) < mNbAllocatedMovedShapes / 4 &&
|
||||
mNbAllocatedMovedShapes > 8) {
|
||||
|
||||
mNbAllocatedMovedShapes /= 2;
|
||||
int* oldArray = mMovedShapes;
|
||||
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
|
||||
assert(mMovedShapes);
|
||||
uint nbElements = 0;
|
||||
for (uint i=0; i<mNbMovedShapes; i++) {
|
||||
if (oldArray[i] != -1) {
|
||||
mMovedShapes[nbElements] = oldArray[i];
|
||||
nbElements++;
|
||||
}
|
||||
}
|
||||
mNbMovedShapes = nbElements;
|
||||
mNbNonUsedMovedShapes = 0;
|
||||
free(oldArray);
|
||||
}
|
||||
|
||||
// Remove the broad-phase ID from the array
|
||||
for (uint i=0; i<mNbMovedShapes; i++) {
|
||||
if (mMovedShapes[i] == broadPhaseID) {
|
||||
mMovedShapes[i] = -1;
|
||||
mNbNonUsedMovedShapes++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Add a proxy collision shape into the broad-phase collision detection
|
||||
void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape) {
|
||||
|
||||
// Add the collision shape into the dynamic AABB tree and get its broad-phase ID
|
||||
mDynamicAABBTree.addObject(proxyShape);
|
||||
|
||||
// Add the collision shape into the array of bodies that have moved (or have been created)
|
||||
// during the last simulation step
|
||||
addMovedCollisionShape(proxyShape->mBroadPhaseID);
|
||||
}
|
||||
|
||||
// Remove a proxy collision shape from the broad-phase collision detection
|
||||
void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
|
||||
|
||||
int broadPhaseID = proxyShape->mBroadPhaseID;
|
||||
|
||||
// Remove the collision shape from the dynamic AABB tree
|
||||
mDynamicAABBTree.removeObject(broadPhaseID);
|
||||
|
||||
// Remove the collision shape into the array of bodies that have moved (or have been created)
|
||||
// during the last simulation step
|
||||
removeMovedCollisionShape(broadPhaseID);
|
||||
}
|
||||
|
||||
// Notify the broad-phase that a collision shape has moved and need to be updated
|
||||
void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) {
|
||||
|
||||
int broadPhaseID = proxyShape->mBroadPhaseID;
|
||||
|
||||
assert(broadPhaseID >= 0);
|
||||
|
||||
// Update the dynamic AABB tree according to the movement of the collision shape
|
||||
bool hasBeenReInserted = mDynamicAABBTree.updateObject(broadPhaseID, aabb);
|
||||
|
||||
// If the collision shape has moved out of its fat AABB (and therefore has been reinserted
|
||||
// into the tree).
|
||||
if (hasBeenReInserted) {
|
||||
|
||||
// Add the collision shape into the array of bodies that have moved (or have been created)
|
||||
// during the last simulation step
|
||||
addMovedCollisionShape(broadPhaseID);
|
||||
}
|
||||
}
|
||||
|
||||
// Compute all the overlapping pairs of collision shapes
|
||||
void BroadPhaseAlgorithm::computeOverlappingPairs() {
|
||||
|
||||
// Reset the potential overlapping pairs
|
||||
mNbPotentialPairs = 0;
|
||||
|
||||
// For all collision shapes that have moved (or have been created) during the
|
||||
// last simulation step
|
||||
for (uint i=0; i<mNbMovedShapes; i++) {
|
||||
uint shapeID = mMovedShapes[i];
|
||||
|
||||
if (shapeID == -1) continue;
|
||||
|
||||
// Get the fat AABB of the collision shape from the dynamic AABB tree
|
||||
const AABB& shapeAABB = mDynamicAABBTree.getFatAABB(shapeID);
|
||||
|
||||
// Ask the dynamic AABB tree to report all collision shapes that overlap with
|
||||
// this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
|
||||
// by the dynamic AABB tree for each potential overlapping pair.
|
||||
mDynamicAABBTree.reportAllShapesOverlappingWith(shapeID, shapeAABB);
|
||||
}
|
||||
|
||||
// Reset the array of collision shapes that have move (or have been created) during the
|
||||
// last simulation step
|
||||
mMovedShapes = 0;
|
||||
|
||||
// Sort the array of potential overlapping pairs in order to remove duplicate pairs
|
||||
std::sort(mPotentialPairs, mPotentialPairs + mNbPotentialPairs, BroadPair::smallerThan);
|
||||
|
||||
// Check all the potential overlapping pairs avoiding duplicates to report unique
|
||||
// overlapping pairs
|
||||
uint i=0;
|
||||
while (i < mNbPotentialPairs) {
|
||||
|
||||
// Get a potential overlapping pair
|
||||
BroadPair* pair = mPotentialPairs + i;
|
||||
i++;
|
||||
|
||||
// Get the two collision shapes of the pair
|
||||
ProxyShape* shape1 = mDynamicAABBTree.getCollisionShape(pair->collisionShape1ID);
|
||||
ProxyShape* shape2 = mDynamicAABBTree.getCollisionShape(pair->collisionShape2ID);
|
||||
|
||||
// Notify the collision detection about the overlapping pair
|
||||
mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
|
||||
|
||||
// Skip the duplicate overlapping pairs
|
||||
while (i < mNbPotentialPairs) {
|
||||
|
||||
// Get the next pair
|
||||
BroadPair* nextPair = mPotentialPairs + i;
|
||||
|
||||
// If the next pair is different from the previous one, we stop skipping pairs
|
||||
if (nextPair->collisionShape1ID != pair->collisionShape1ID ||
|
||||
nextPair->collisionShape2ID != pair->collisionShape2ID) {
|
||||
break;
|
||||
}
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
// If the number of potential overlapping pairs is less than the quarter of allocated
|
||||
// number of overlapping pairs
|
||||
if (mNbPotentialPairs < mNbAllocatedPotentialPairs / 4 && mNbPotentialPairs > 8) {
|
||||
|
||||
// Reduce the number of allocated potential overlapping pairs
|
||||
BroadPair* oldPairs = mPotentialPairs;
|
||||
mNbAllocatedPotentialPairs /= 2;
|
||||
mPotentialPairs = (BroadPair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPair));
|
||||
assert(mPotentialPairs);
|
||||
memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPair));
|
||||
free(oldPairs);
|
||||
}
|
||||
}
|
||||
|
||||
// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
|
||||
void BroadPhaseAlgorithm::notifyOverlappingPair(int node1ID, int node2ID) {
|
||||
|
||||
// If both the nodes are the same, we do not create store the overlapping pair
|
||||
if (node1ID == node2ID) return;
|
||||
|
||||
// If we need to allocate more memory for the array of potential overlapping pairs
|
||||
if (mNbPotentialPairs == mNbAllocatedPotentialPairs) {
|
||||
|
||||
// Allocate more memory for the array of potential pairs
|
||||
BroadPair* oldPairs = mPotentialPairs;
|
||||
mNbAllocatedPotentialPairs *= 2;
|
||||
mPotentialPairs = (BroadPair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPair));
|
||||
assert(mPotentialPairs);
|
||||
memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPair));
|
||||
free(oldPairs);
|
||||
}
|
||||
|
||||
// Add the new potential pair into the array of potential overlapping pairs
|
||||
mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID);
|
||||
mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID);
|
||||
mNbPotentialPairs++;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -38,16 +38,42 @@ namespace reactphysics3d {
|
|||
// Declarations
|
||||
class CollisionDetection;
|
||||
|
||||
// TODO : Check that when a kinematic or static body is manually moved, the dynamic aabb tree
|
||||
// is correctly updated
|
||||
|
||||
// TODO : Replace the names "body, bodies" by "collision shapes"
|
||||
|
||||
// TODO : Remove the pair manager
|
||||
|
||||
// TODO : RENAME THIS
|
||||
// Structure BroadPair
|
||||
/**
|
||||
* This structure represent a potential overlapping pair during the broad-phase collision
|
||||
* detection.
|
||||
*/
|
||||
struct BroadPair {
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Broad-phase ID of the first collision shape
|
||||
int collisionShape1ID;
|
||||
|
||||
/// Broad-phase ID of the second collision shape
|
||||
int collisionShape2ID;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Method used to compare two pairs for sorting algorithm
|
||||
static bool smallerThan(const BroadPair& pair1, const BroadPair& pair2);
|
||||
};
|
||||
|
||||
// Class BroadPhaseAlgorithm
|
||||
/**
|
||||
* This class represents an algorithm the broad-phase collision detection. The
|
||||
* goal of the broad-phase collision detection is to compute the pair of bodies
|
||||
* that can collide. But it's important to understand that the
|
||||
* broad-phase doesn't compute only body pairs that can collide but
|
||||
* could also pairs of body that doesn't collide but are very close.
|
||||
* The goal of the broad-phase is to remove pairs of body that cannot
|
||||
* collide in order to avoid to much bodies to be tested in the
|
||||
* narrow-phase.
|
||||
* This class represents the broad-phase collision detection. The
|
||||
* goal of the broad-phase collision detection is to compute the pairs of bodies
|
||||
* that have their AABBs overlapping. Only those pairs of bodies will be tested
|
||||
* later for collision during the narrow-phase collision detection. A dynamic AABB
|
||||
* tree data structure is used for fast broad-phase collision detection.
|
||||
*/
|
||||
class BroadPhaseAlgorithm {
|
||||
|
||||
|
|
@ -58,6 +84,32 @@ class BroadPhaseAlgorithm {
|
|||
/// Dynamic AABB tree
|
||||
DynamicAABBTree mDynamicAABBTree;
|
||||
|
||||
/// 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
|
||||
/// for overlapping in the next simulation step.
|
||||
int* mMovedShapes;
|
||||
|
||||
/// Number of collision shapes in the array of shapes that have moved during the last
|
||||
/// simulation step.
|
||||
uint mNbMovedShapes;
|
||||
|
||||
/// Number of allocated elements for the array of shapes that have moved during the last
|
||||
/// simulation step.
|
||||
uint mNbAllocatedMovedShapes;
|
||||
|
||||
/// Number of non-used elements in the array of shapes that have moved during the last
|
||||
/// simulation step.
|
||||
uint mNbNonUsedMovedShapes;
|
||||
|
||||
/// Temporary array of potential overlapping pairs (with potential duplicates)
|
||||
BroadPair* mPotentialPairs;
|
||||
|
||||
/// Number of potential overlapping pairs
|
||||
uint mNbPotentialPairs;
|
||||
|
||||
/// Number of allocated elements for the array of potential overlapping pairs
|
||||
uint mNbAllocatedPotentialPairs;
|
||||
|
||||
/// Pair manager containing the overlapping pairs
|
||||
PairManager mPairManager;
|
||||
|
||||
|
|
@ -80,24 +132,50 @@ class BroadPhaseAlgorithm {
|
|||
BroadPhaseAlgorithm(CollisionDetection& collisionDetection);
|
||||
|
||||
/// Destructor
|
||||
virtual ~BroadPhaseAlgorithm();
|
||||
~BroadPhaseAlgorithm();
|
||||
|
||||
/// Notify the broad-phase about a new object in the world
|
||||
virtual void addObject(CollisionBody* body, const AABB& aabb)=0;
|
||||
/// Add a proxy collision shape into the broad-phase collision detection
|
||||
void addProxyCollisionShape(ProxyShape* proxyShape);
|
||||
|
||||
/// Notify the broad-phase about an object that has been removed from the world
|
||||
virtual void removeObject(CollisionBody* body)=0;
|
||||
/// Remove a proxy collision shape from the broad-phase collision detection
|
||||
void removeProxyCollisionShape(ProxyShape* proxyShape);
|
||||
|
||||
/// Notify the broad-phase that the AABB of an object has changed
|
||||
virtual void updateObject(CollisionBody* body, const AABB& aabb)=0;
|
||||
/// Notify the broad-phase that a collision shape has moved and need to be updated
|
||||
void updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb);
|
||||
|
||||
/// Add a collision shape in the array of shapes that have moved in the last simulation step
|
||||
/// and that need to be tested again for broad-phase overlapping.
|
||||
void addMovedCollisionShape(int broadPhaseID);
|
||||
|
||||
/// Remove a collision shape from the array of shapes that have moved in the last simulation
|
||||
/// step and that need to be tested again for broad-phase overlapping.
|
||||
void removeMovedCollisionShape(int broadPhaseID);
|
||||
|
||||
/// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
|
||||
void notifyOverlappingPair(int node1ID, int node2ID);
|
||||
|
||||
/// Compute all the overlapping pairs of collision shapes
|
||||
void computeOverlappingPairs();
|
||||
|
||||
/// Return a pointer to the first active pair (used to iterate over the active pairs)
|
||||
// TODO : DELETE THIS
|
||||
BodyPair* beginOverlappingPairsPointer() const;
|
||||
|
||||
/// Return a pointer to the last active pair (used to iterate over the active pairs)
|
||||
// TODO : DELETE THIS
|
||||
BodyPair* endOverlappingPairsPointer() const;
|
||||
};
|
||||
|
||||
// Method used to compare two pairs for sorting algorithm
|
||||
inline bool BroadPair::smallerThan(const BroadPair& pair1, const BroadPair& pair2) {
|
||||
|
||||
if (pair1.collisionShape1ID < pair2.collisionShape1ID) return true;
|
||||
if (pair1.collisionShape1ID == pair2.collisionShape1ID) {
|
||||
return pair1.collisionShape2ID < pair2.collisionShape2ID;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// Return a pointer to the first active pair (used to iterate over the overlapping pairs)
|
||||
inline BodyPair* BroadPhaseAlgorithm::beginOverlappingPairsPointer() const {
|
||||
return mPairManager.beginOverlappingPairsPointer();
|
||||
|
|
|
|||
|
|
@ -25,6 +25,8 @@
|
|||
|
||||
// Libraries
|
||||
#include "DynamicAABBTree.h"
|
||||
#include "BroadPhaseAlgorithm.h"
|
||||
#include "../../memory/Stack.h"
|
||||
|
||||
using namespace reactphysics3d;
|
||||
|
||||
|
|
@ -32,7 +34,7 @@ using namespace reactphysics3d;
|
|||
const int TreeNode::NULL_TREE_NODE = -1;
|
||||
|
||||
// Constructor
|
||||
DynamicAABBTree::DynamicAABBTree() {
|
||||
DynamicAABBTree::DynamicAABBTree(BroadPhaseAlgorithm& broadPhase) : mBroadPhase(broadPhase){
|
||||
|
||||
mRootNodeID = TreeNode::NULL_TREE_NODE;
|
||||
mNbNodes = 0;
|
||||
|
|
@ -92,7 +94,7 @@ int DynamicAABBTree::allocateNode() {
|
|||
mNodes[freeNodeID].parentID = TreeNode::NULL_TREE_NODE;
|
||||
mNodes[freeNodeID].leftChildID = TreeNode::NULL_TREE_NODE;
|
||||
mNodes[freeNodeID].rightChildID = TreeNode::NULL_TREE_NODE;
|
||||
mNodes[freeNodeID].collisionShape = NULL;
|
||||
mNodes[freeNodeID].proxyShape = NULL;
|
||||
mNodes[freeNodeID].height = 0;
|
||||
mNbNodes++;
|
||||
|
||||
|
|
@ -134,7 +136,7 @@ void DynamicAABBTree::releaseNode(int nodeID) {
|
|||
|
||||
// Add an object into the tree. This method creates a new leaf node in the tree and
|
||||
// returns the ID of the corresponding node.
|
||||
int DynamicAABBTree::addObject(CollisionShape* collisionShape, const AABB& aabb) {
|
||||
void DynamicAABBTree::addObject(ProxyShape* proxyShape) {
|
||||
|
||||
// Get the next available node (or allocate new ones if necessary)
|
||||
int nodeID = allocateNode();
|
||||
|
|
@ -145,7 +147,7 @@ int DynamicAABBTree::addObject(CollisionShape* collisionShape, const AABB& aabb)
|
|||
mNodes[nodeID].aabb.setMax(mNodes[nodeID].aabb.getMax() + gap);
|
||||
|
||||
// Set the collision shape
|
||||
mNodes[nodeID].collisionShape = collisionShape;
|
||||
mNodes[nodeID].proxyShape = proxyShape;
|
||||
|
||||
// Set the height of the node in the tree
|
||||
mNodes[nodeID].height = 0;
|
||||
|
|
@ -153,8 +155,9 @@ int DynamicAABBTree::addObject(CollisionShape* collisionShape, const AABB& aabb)
|
|||
// Insert the new leaf node in the tree
|
||||
insertLeafNode(nodeID);
|
||||
|
||||
// Return the node ID
|
||||
return nodeID;
|
||||
// Set the broad-phase ID of the proxy shape
|
||||
proxyShape->mBroadPhaseID = nodeID;
|
||||
assert(nodeID >= 0);
|
||||
}
|
||||
|
||||
// Remove an object from the tree
|
||||
|
|
@ -172,7 +175,7 @@ void DynamicAABBTree::removeObject(int nodeID) {
|
|||
/// If the new AABB of the object that has moved is still inside its fat AABB, then
|
||||
/// nothing is done. Otherwise, the corresponding node is removed and reinserted into the tree.
|
||||
/// The method returns true if the object has been reinserted into the tree.
|
||||
bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement) {
|
||||
bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB) {
|
||||
|
||||
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
|
||||
assert(mNodes[nodeID].isLeaf());
|
||||
|
|
@ -185,31 +188,11 @@ bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector
|
|||
// If the new AABB is outside the fat AABB, we remove the corresponding node
|
||||
removeLeafNode(nodeID);
|
||||
|
||||
// Compute a new fat AABB for the new AABB by taking the object displacement into account
|
||||
AABB fatAABB = newAABB;
|
||||
// Compute a new fat AABB for the new AABB
|
||||
mNodes[nodeID].aabb = newAABB;
|
||||
const Vector3 gap(DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP);
|
||||
fatAABB.mMinCoordinates -= gap;
|
||||
fatAABB.mMaxCoordinates += gap;
|
||||
const Vector3 displacementGap = AABB_DISPLACEMENT_MULTIPLIER * displacement;
|
||||
if (displacementGap.x < decimal(0.0)) {
|
||||
fatAABB.mMinCoordinates.x += displacementGap.x;
|
||||
}
|
||||
else {
|
||||
fatAABB.mMaxCoordinates.x += displacementGap.x;
|
||||
}
|
||||
if (displacementGap.y < decimal(0.0)) {
|
||||
fatAABB.mMinCoordinates.y += displacementGap.y;
|
||||
}
|
||||
else {
|
||||
fatAABB.mMaxCoordinates.y += displacementGap.y;
|
||||
}
|
||||
if (displacementGap.z < decimal(0.0)) {
|
||||
fatAABB.mMinCoordinates.z += displacementGap.z;
|
||||
}
|
||||
else {
|
||||
fatAABB.mMaxCoordinates.z += displacementGap.z;
|
||||
}
|
||||
mNodes[nodeID].aabb = fatAABB;
|
||||
mNodes[nodeID].aabb.mMinCoordinates -= gap;
|
||||
mNodes[nodeID].aabb.mMaxCoordinates += gap;
|
||||
|
||||
// Reinsert the node into the tree
|
||||
insertLeafNode(nodeID);
|
||||
|
|
@ -292,7 +275,7 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
|
|||
int oldParentNode = mNodes[siblingNode].parentID;
|
||||
int newParentNode = allocateNode();
|
||||
mNodes[newParentNode].parentID = oldParentNode;
|
||||
mNodes[newParentNode].collisionShape = NULL;
|
||||
mNodes[newParentNode].proxyShape = NULL;
|
||||
mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB);
|
||||
mNodes[newParentNode].height = mNodes[siblingNode].height + 1;
|
||||
|
||||
|
|
@ -552,3 +535,44 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
|
|||
// If the sub-tree is balanced, return the current root node
|
||||
return nodeID;
|
||||
}
|
||||
|
||||
// Report all shapes overlapping with the AABB given in parameter.
|
||||
/// For each overlapping shape with the AABB given in parameter, the
|
||||
/// BroadPhase::notifyOverlappingPair() method is called to store a
|
||||
/// potential overlapping pair.
|
||||
void DynamicAABBTree::reportAllShapesOverlappingWith(int nodeID, const AABB& aabb) {
|
||||
|
||||
// Create a stack with the nodes to visit
|
||||
Stack<int, 64> stack;
|
||||
stack.push(mRootNodeID);
|
||||
|
||||
// While there are still nodes to visit
|
||||
while(stack.getNbElements() > 0) {
|
||||
|
||||
// Get the next node ID to visit
|
||||
int nodeIDToVisit = stack.pop();
|
||||
|
||||
// Skip it if it is a null node
|
||||
if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) continue;
|
||||
|
||||
// Get the corresponding node
|
||||
const TreeNode* nodeToVisit = mNodes + nodeIDToVisit;
|
||||
|
||||
// If the AABB in parameter overlaps with the AABB of the node to visit
|
||||
if (aabb.testCollision(nodeToVisit->aabb)) {
|
||||
|
||||
// If the node is a leaf
|
||||
if (nodeToVisit->isLeaf()) {
|
||||
|
||||
// Notify the broad-phase about a new potential overlapping pair
|
||||
mBroadPhase.notifyOverlappingPair(nodeID, nodeIDToVisit);
|
||||
}
|
||||
else { // If the node is not a leaf
|
||||
|
||||
// We need to visit its children
|
||||
stack.push(nodeToVisit->leftChildID);
|
||||
stack.push(nodeToVisit->rightChildID);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -29,11 +29,14 @@
|
|||
// Libraries
|
||||
#include "../../configuration.h"
|
||||
#include "../shapes/AABB.h"
|
||||
#include "../shapes/CollisionShape.h"
|
||||
#include "../../body/CollisionBody.h"
|
||||
|
||||
/// Namespace ReactPhysics3D
|
||||
namespace reactphysics3d {
|
||||
|
||||
// Declarations
|
||||
class BroadPhaseAlgorithm;
|
||||
|
||||
// Structure TreeNode
|
||||
/**
|
||||
* This structure represents a node of the dynamic AABB tree.
|
||||
|
|
@ -63,7 +66,7 @@ struct TreeNode {
|
|||
AABB aabb;
|
||||
|
||||
/// Pointer to the corresponding collision shape (in case this node is a leaf)
|
||||
CollisionShape* collisionShape;
|
||||
ProxyShape* proxyShape;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
|
|
@ -85,6 +88,9 @@ class DynamicAABBTree {
|
|||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Reference to the broad-phase
|
||||
BroadPhaseAlgorithm& mBroadPhase;
|
||||
|
||||
/// Pointer to the memory location of the nodes of the tree
|
||||
TreeNode* mNodes;
|
||||
|
||||
|
|
@ -122,19 +128,28 @@ class DynamicAABBTree {
|
|||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
DynamicAABBTree();
|
||||
DynamicAABBTree(BroadPhaseAlgorithm& broadPhase);
|
||||
|
||||
/// Destructor
|
||||
~DynamicAABBTree();
|
||||
|
||||
/// Add an object into the tree
|
||||
int addObject(CollisionShape* collisionShape, const AABB& aabb);
|
||||
void addObject(ProxyShape* proxyShape);
|
||||
|
||||
/// Remove an object from the tree
|
||||
void removeObject(int nodeID);
|
||||
|
||||
/// Update the dynamic tree after an object has moved.
|
||||
bool updateObject(int nodeID, const AABB &newAABB, const Vector3 &displacement);
|
||||
bool updateObject(int nodeID, const AABB& newAABB);
|
||||
|
||||
/// Return the fat AABB corresponding to a given node ID
|
||||
const AABB& getFatAABB(int nodeID) const;
|
||||
|
||||
/// Return the collision shape of a given leaf node of the tree
|
||||
ProxyShape* getCollisionShape(int nodeID) const;
|
||||
|
||||
/// Report all shapes overlapping with the AABB given in parameter.
|
||||
void reportAllShapesOverlappingWith(int nodeID, const AABB& aabb);
|
||||
};
|
||||
|
||||
// Return true if the node is a leaf of the tree
|
||||
|
|
@ -142,6 +157,19 @@ inline bool TreeNode::isLeaf() const {
|
|||
return leftChildID == NULL_TREE_NODE;
|
||||
}
|
||||
|
||||
// Return the fat AABB corresponding to a given node ID
|
||||
inline const AABB& DynamicAABBTree::getFatAABB(int nodeID) const {
|
||||
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
|
||||
return mNodes[nodeID].aabb;
|
||||
}
|
||||
|
||||
// Return the collision shape of a given leaf node of the tree
|
||||
inline ProxyShape* DynamicAABBTree::getCollisionShape(int nodeID) const {
|
||||
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
|
||||
assert(mNodes[nodeID].isLeaf());
|
||||
return mNodes[nodeID].proxyShape;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -69,18 +69,18 @@ class NoBroadPhaseAlgorithm : public BroadPhaseAlgorithm {
|
|||
virtual ~NoBroadPhaseAlgorithm();
|
||||
|
||||
/// Notify the broad-phase about a new object in the world
|
||||
virtual void addObject(CollisionBody* body, const AABB& aabb);
|
||||
virtual void addProxyCollisionShape(CollisionBody* body, const AABB& aabb);
|
||||
|
||||
/// Notify the broad-phase about an object that has been removed from the world
|
||||
virtual void removeObject(CollisionBody* body);
|
||||
virtual void removeProxyCollisionShape(CollisionBody* body);
|
||||
|
||||
/// Notify the broad-phase that the AABB of an object has changed
|
||||
virtual void updateObject(CollisionBody* body, const AABB& aabb);
|
||||
virtual void updateProxyCollisionShape(CollisionBody* body, const AABB& aabb);
|
||||
};
|
||||
|
||||
|
||||
// Notify the broad-phase about a new object in the world
|
||||
inline void NoBroadPhaseAlgorithm::addObject(CollisionBody* body, const AABB& aabb) {
|
||||
inline void NoBroadPhaseAlgorithm::addProxyCollisionShape(CollisionBody* body, const AABB& aabb) {
|
||||
|
||||
// For each body that is already in the world
|
||||
for (std::set<CollisionBody*>::iterator it = mBodies.begin(); it != mBodies.end(); ++it) {
|
||||
|
|
@ -94,7 +94,7 @@ inline void NoBroadPhaseAlgorithm::addObject(CollisionBody* body, const AABB& aa
|
|||
}
|
||||
|
||||
// Notify the broad-phase about an object that has been removed from the world
|
||||
inline void NoBroadPhaseAlgorithm::removeObject(CollisionBody* body) {
|
||||
inline void NoBroadPhaseAlgorithm::removeProxyCollisionShape(CollisionBody* body) {
|
||||
|
||||
// For each body that is in the world
|
||||
for (std::set<CollisionBody*>::iterator it = mBodies.begin(); it != mBodies.end(); ++it) {
|
||||
|
|
@ -111,7 +111,7 @@ inline void NoBroadPhaseAlgorithm::removeObject(CollisionBody* body) {
|
|||
}
|
||||
|
||||
// Notify the broad-phase that the AABB of an object has changed
|
||||
inline void NoBroadPhaseAlgorithm::updateObject(CollisionBody* body, const AABB& aabb) {
|
||||
inline void NoBroadPhaseAlgorithm::updateProxyCollisionShape(CollisionBody* body, const AABB& aabb) {
|
||||
// Do nothing
|
||||
return;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -104,7 +104,7 @@ BodyPair* PairManager::addPair(CollisionBody* body1, CollisionBody* body2) {
|
|||
mHashTable[hashValue] = mNbOverlappingPairs++;
|
||||
|
||||
// Notify the collision detection about this new overlapping pair
|
||||
mCollisionDetection.broadPhaseNotifyAddedOverlappingPair(newPair);
|
||||
mCollisionDetection.broadPhaseNotifyOverlappingPair(newPair);
|
||||
|
||||
// Return a pointer to the new created pair
|
||||
return newPair;
|
||||
|
|
|
|||
|
|
@ -26,6 +26,8 @@
|
|||
#ifndef REACTPHYSICS3D_PAIR_MANAGER_H
|
||||
#define REACTPHYSICS3D_PAIR_MANAGER_H
|
||||
|
||||
// TODO : REMOVE THE PAIR MANAGER CLASS
|
||||
|
||||
// Libraries
|
||||
#include "../../body/CollisionBody.h"
|
||||
#include <utility>
|
||||
|
|
|
|||
|
|
@ -81,7 +81,7 @@ SweepAndPruneAlgorithm::~SweepAndPruneAlgorithm() {
|
|||
|
||||
// Notify the broad-phase about a new object in the world
|
||||
/// This method adds the AABB of the object ion to broad-phase
|
||||
void SweepAndPruneAlgorithm::addObject(CollisionBody* body, const AABB& aabb) {
|
||||
void SweepAndPruneAlgorithm::addProxyCollisionShape(CollisionBody* body, const AABB& aabb) {
|
||||
|
||||
bodyindex boxIndex;
|
||||
|
||||
|
|
@ -144,11 +144,11 @@ void SweepAndPruneAlgorithm::addObject(CollisionBody* body, const AABB& aabb) {
|
|||
// correct position in the array. This will also create the overlapping
|
||||
// pairs in the pair manager if the new AABB is overlapping with others
|
||||
// AABBs
|
||||
updateObject(body, aabb);
|
||||
updateProxyCollisionShape(body, aabb);
|
||||
}
|
||||
|
||||
// Notify the broad-phase about an object that has been removed from the world
|
||||
void SweepAndPruneAlgorithm::removeObject(CollisionBody* body) {
|
||||
void SweepAndPruneAlgorithm::removeProxyCollisionShape(CollisionBody* body) {
|
||||
|
||||
assert(mNbBoxes > 0);
|
||||
|
||||
|
|
|
|||
|
|
@ -180,13 +180,13 @@ class SweepAndPruneAlgorithm : public BroadPhaseAlgorithm {
|
|||
virtual ~SweepAndPruneAlgorithm();
|
||||
|
||||
/// Notify the broad-phase about a new object in the world.
|
||||
virtual void addObject(CollisionBody* body, const AABB& aabb);
|
||||
virtual void addProxyCollisionShape(CollisionBody* body, const AABB& aabb);
|
||||
|
||||
/// Notify the broad-phase about a object that has been removed from the world
|
||||
virtual void removeObject(CollisionBody* body);
|
||||
virtual void removeProxyCollisionShape(CollisionBody* body);
|
||||
|
||||
/// Notify the broad-phase that the AABB of an object has changed
|
||||
virtual void updateObject(CollisionBody* body, const AABB& aabb);
|
||||
virtual void updateProxyCollisionShape(CollisionBody* body, const AABB& aabb);
|
||||
};
|
||||
|
||||
/// Encode a floating value into a integer value in order to
|
||||
|
|
@ -229,7 +229,7 @@ inline bool SweepAndPruneAlgorithm::testIntersect2D(const BoxAABB& box1, const B
|
|||
}
|
||||
|
||||
// Notify the broad-phase that the AABB of an object has changed
|
||||
inline void SweepAndPruneAlgorithm::updateObject(CollisionBody* body, const AABB& aabb) {
|
||||
inline void SweepAndPruneAlgorithm::updateProxyCollisionShape(CollisionBody* body, const AABB& aabb) {
|
||||
|
||||
// Compute the corresponding AABB with integer coordinates
|
||||
AABBInt aabbInt(aabb);
|
||||
|
|
|
|||
|
|
@ -83,9 +83,9 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
|
|||
/// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
|
||||
/// the correct penetration depth
|
||||
bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex,
|
||||
CollisionShape* collisionShape1,
|
||||
ProxyShape* collisionShape1,
|
||||
const Transform& transform1,
|
||||
CollisionShape* collisionShape2,
|
||||
ProxyShape* collisionShape2,
|
||||
const Transform& transform2,
|
||||
Vector3& v, ContactPointInfo*& contactInfo) {
|
||||
|
||||
|
|
|
|||
|
|
@ -119,9 +119,9 @@ class EPAAlgorithm {
|
|||
|
||||
/// Compute the penetration depth with EPA algorithm.
|
||||
bool computePenetrationDepthAndContactPoints(const Simplex& simplex,
|
||||
CollisionShape* collisionShape1,
|
||||
ProxyShape* collisionShape1,
|
||||
const Transform& transform1,
|
||||
CollisionShape* collisionShape2,
|
||||
ProxyShape* collisionShape2,
|
||||
const Transform& transform2,
|
||||
Vector3& v, ContactPointInfo*& contactInfo);
|
||||
};
|
||||
|
|
|
|||
|
|
@ -57,11 +57,8 @@ GJKAlgorithm::~GJKAlgorithm() {
|
|||
/// algorithm on the enlarged object to obtain a simplex polytope that contains the
|
||||
/// origin, they we give that simplex polytope to the EPA algorithm which will compute
|
||||
/// the correct penetration depth and contact points between the enlarged objects.
|
||||
bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1,
|
||||
const Transform& transform1,
|
||||
CollisionShape* collisionShape2,
|
||||
const Transform& transform2,
|
||||
ContactPointInfo*& contactInfo) {
|
||||
bool GJKAlgorithm::testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2,
|
||||
ContactPointInfo*& contactInfo) {
|
||||
|
||||
Vector3 suppA; // Support point of object A
|
||||
Vector3 suppB; // Support point of object B
|
||||
|
|
@ -71,6 +68,12 @@ bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1,
|
|||
decimal vDotw;
|
||||
decimal prevDistSquare;
|
||||
|
||||
// Get the local-space to world-space transforms
|
||||
const Transform transform1 = collisionShape1->getBody()->getTransform() *
|
||||
collisionShape1->getLocalToBodyTransform();
|
||||
const Transform transform2 = collisionShape2->getBody()->getTransform() *
|
||||
collisionShape2->getLocalToBodyTransform();
|
||||
|
||||
// Transform a point from local space of body 2 to local
|
||||
// space of body 1 (the GJK algorithm is done in local space of body 1)
|
||||
Transform body2Tobody1 = transform1.getInverse() * transform2;
|
||||
|
|
@ -89,7 +92,7 @@ bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1,
|
|||
Simplex simplex;
|
||||
|
||||
// Get the previous point V (last cached separating axis)
|
||||
Vector3 v = mCurrentOverlappingPair->previousSeparatingAxis;
|
||||
Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis();
|
||||
|
||||
// Initialize the upper bound for the square distance
|
||||
decimal distSquare = DECIMAL_LARGEST;
|
||||
|
|
@ -110,7 +113,7 @@ bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1,
|
|||
if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
|
||||
|
||||
// Cache the current separating axis for frame coherence
|
||||
mCurrentOverlappingPair->previousSeparatingAxis = v;
|
||||
mCurrentOverlappingPair->setCachedSeparatingAxis(v);
|
||||
|
||||
// No intersection, we return false
|
||||
return false;
|
||||
|
|
@ -259,9 +262,9 @@ bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1,
|
|||
/// assumed to intersect in the original objects (without margin). Therefore such
|
||||
/// a polytope must exist. Then, we give that polytope to the EPA algorithm to
|
||||
/// compute the correct penetration depth and contact points of the enlarged objects.
|
||||
bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(CollisionShape* collisionShape1,
|
||||
bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(ProxyShape* collisionShape1,
|
||||
const Transform& transform1,
|
||||
CollisionShape* collisionShape2,
|
||||
ProxyShape* collisionShape2,
|
||||
const Transform& transform2,
|
||||
ContactPointInfo*& contactInfo,
|
||||
Vector3& v) {
|
||||
|
|
|
|||
|
|
@ -74,9 +74,9 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
|
|||
GJKAlgorithm& operator=(const GJKAlgorithm& algorithm);
|
||||
|
||||
/// Compute the penetration depth for enlarged objects.
|
||||
bool computePenetrationDepthForEnlargedObjects(CollisionShape* collisionShape1,
|
||||
bool computePenetrationDepthForEnlargedObjects(ProxyShape* collisionShape1,
|
||||
const Transform& transform1,
|
||||
CollisionShape* collisionShape2,
|
||||
ProxyShape* collisionShape2,
|
||||
const Transform& transform2,
|
||||
ContactPointInfo*& contactInfo, Vector3& v);
|
||||
|
||||
|
|
@ -91,10 +91,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
|
|||
~GJKAlgorithm();
|
||||
|
||||
/// Return true and compute a contact info if the two bounding volumes collide.
|
||||
virtual bool testCollision(CollisionShape *collisionShape1,
|
||||
const Transform& transform1,
|
||||
CollisionShape *collisionShape2,
|
||||
const Transform& transform2,
|
||||
virtual bool testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2,
|
||||
ContactPointInfo*& contactInfo);
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -31,7 +31,7 @@
|
|||
#include "../../constraint/ContactPoint.h"
|
||||
#include "../broadphase/PairManager.h"
|
||||
#include "../../memory/MemoryAllocator.h"
|
||||
#include "../BroadPhasePair.h"
|
||||
#include "../../engine/OverlappingPair.h"
|
||||
|
||||
|
||||
/// Namespace ReactPhysics3D
|
||||
|
|
@ -54,7 +54,7 @@ class NarrowPhaseAlgorithm {
|
|||
MemoryAllocator& mMemoryAllocator;
|
||||
|
||||
/// Overlapping pair of the bodies currently tested for collision
|
||||
BroadPhasePair* mCurrentOverlappingPair;
|
||||
OverlappingPair* mCurrentOverlappingPair;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
|
|
@ -75,18 +75,15 @@ class NarrowPhaseAlgorithm {
|
|||
virtual ~NarrowPhaseAlgorithm();
|
||||
|
||||
/// Set the current overlapping pair of bodies
|
||||
void setCurrentOverlappingPair(BroadPhasePair* overlappingPair);
|
||||
void setCurrentOverlappingPair(OverlappingPair* overlappingPair);
|
||||
|
||||
/// Return true and compute a contact info if the two bounding volume collide
|
||||
virtual bool testCollision(CollisionShape* collisionShape1,
|
||||
const Transform& transform1,
|
||||
CollisionShape* collisionShape2,
|
||||
const Transform& transform2,
|
||||
virtual bool testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2,
|
||||
ContactPointInfo*& contactInfo)=0;
|
||||
};
|
||||
|
||||
// Set the current overlapping pair of bodies
|
||||
inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(BroadPhasePair *overlappingPair) {
|
||||
inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* overlappingPair) {
|
||||
mCurrentOverlappingPair = overlappingPair;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -41,16 +41,22 @@ SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() {
|
|||
|
||||
}
|
||||
|
||||
bool SphereVsSphereAlgorithm::testCollision(CollisionShape* collisionShape1,
|
||||
const Transform& transform1,
|
||||
CollisionShape* collisionShape2,
|
||||
const Transform& transform2,
|
||||
bool SphereVsSphereAlgorithm::testCollision(ProxyShape* collisionShape1,
|
||||
ProxyShape* collisionShape2,
|
||||
ContactPointInfo*& contactInfo) {
|
||||
|
||||
// Get the sphere collision shapes
|
||||
const SphereShape* sphereShape1 = dynamic_cast<const SphereShape*>(collisionShape1);
|
||||
const SphereShape* sphereShape2 = dynamic_cast<const SphereShape*>(collisionShape2);
|
||||
|
||||
const CollisionShape* shape1 = collisionShape1->getCollisionShape();
|
||||
const CollisionShape* shape2 = collisionShape2->getCollisionShape();
|
||||
const SphereShape* sphereShape1 = dynamic_cast<const SphereShape*>(shape1);
|
||||
const SphereShape* sphereShape2 = dynamic_cast<const SphereShape*>(shape2);
|
||||
|
||||
// Get the local-space to world-space transforms
|
||||
const Transform transform1 = collisionShape1->getBody()->getTransform() *
|
||||
collisionShape1->getLocalToBodyTransform();
|
||||
const Transform transform2 = collisionShape2->getBody()->getTransform() *
|
||||
collisionShape2->getLocalToBodyTransform();
|
||||
|
||||
// Compute the distance between the centers
|
||||
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
|
||||
decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
|
||||
|
|
|
|||
|
|
@ -63,10 +63,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
|
|||
virtual ~SphereVsSphereAlgorithm();
|
||||
|
||||
/// Return true and compute a contact info if the two bounding volume collide
|
||||
virtual bool testCollision(CollisionShape* collisionShape1,
|
||||
const Transform& transform1,
|
||||
CollisionShape* collisionShape2,
|
||||
const Transform& transform2,
|
||||
virtual bool testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2,
|
||||
ContactPointInfo*& contactInfo);
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -61,3 +61,15 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const
|
|||
0.0, factor * (xSquare + zSquare), 0.0,
|
||||
0.0, 0.0, factor * (xSquare + ySquare));
|
||||
}
|
||||
|
||||
// Constructor
|
||||
ProxyBoxShape::ProxyBoxShape(const BoxShape* shape, CollisionBody* body,
|
||||
const Transform& transform, decimal mass)
|
||||
:ProxyShape(body, transform, mass), mCollisionShape(shape){
|
||||
|
||||
}
|
||||
|
||||
// Destructor
|
||||
ProxyBoxShape::~ProxyBoxShape() {
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -89,16 +89,69 @@ class BoxShape : public CollisionShape {
|
|||
virtual size_t getSizeInBytes() const;
|
||||
|
||||
/// Return a local support point in a given direction with the object margin
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const;
|
||||
|
||||
/// 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) const;
|
||||
|
||||
/// Return the local inertia tensor of the collision shape
|
||||
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
|
||||
|
||||
/// Test equality between two box shapes
|
||||
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
|
||||
|
||||
/// Create a proxy collision shape for the collision shape
|
||||
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
||||
const Transform& transform, decimal mass) const;
|
||||
};
|
||||
|
||||
// Class ProxyBoxShape
|
||||
/**
|
||||
* The proxy collision shape for a box shape.
|
||||
*/
|
||||
class ProxyBoxShape : public ProxyShape {
|
||||
|
||||
private:
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Pointer to the actual collision shape
|
||||
const BoxShape* mCollisionShape;
|
||||
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Private copy-constructor
|
||||
ProxyBoxShape(const ProxyBoxShape& proxyShape);
|
||||
|
||||
/// Private assignment operator
|
||||
ProxyBoxShape& operator=(const ProxyBoxShape& proxyShape);
|
||||
|
||||
public:
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
ProxyBoxShape(const BoxShape* shape, CollisionBody* body,
|
||||
const Transform& transform, decimal mass);
|
||||
|
||||
/// Destructor
|
||||
~ProxyBoxShape();
|
||||
|
||||
/// Return the collision shape
|
||||
virtual const CollisionShape* getCollisionShape() const;
|
||||
|
||||
/// Return the number of bytes used by the proxy collision shape
|
||||
virtual size_t getSizeInBytes() const;
|
||||
|
||||
/// Return a local support point in a given direction with the object margin
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
|
||||
|
||||
/// Return a local support point in a given direction without the object margin
|
||||
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction);
|
||||
|
||||
/// Return the current collision shape margin
|
||||
virtual decimal getMargin() const;
|
||||
};
|
||||
|
||||
// Allocate and return a copy of the object
|
||||
|
|
@ -128,7 +181,7 @@ inline size_t BoxShape::getSizeInBytes() const {
|
|||
}
|
||||
|
||||
// Return a local support point in a given direction with the object margin
|
||||
inline Vector3 BoxShape::getLocalSupportPointWithMargin(const Vector3& direction) {
|
||||
inline Vector3 BoxShape::getLocalSupportPointWithMargin(const Vector3& direction) const {
|
||||
|
||||
assert(mMargin > 0.0);
|
||||
|
||||
|
|
@ -138,7 +191,7 @@ inline Vector3 BoxShape::getLocalSupportPointWithMargin(const Vector3& direction
|
|||
}
|
||||
|
||||
// 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) const {
|
||||
|
||||
return Vector3(direction.x < 0.0 ? -mExtent.x : mExtent.x,
|
||||
direction.y < 0.0 ? -mExtent.y : mExtent.y,
|
||||
|
|
@ -151,6 +204,38 @@ inline bool BoxShape::isEqualTo(const CollisionShape& otherCollisionShape) const
|
|||
return (mExtent == otherShape.mExtent);
|
||||
}
|
||||
|
||||
// Create a proxy collision shape for the collision shape
|
||||
inline ProxyShape* BoxShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
||||
const Transform& transform, decimal mass) const {
|
||||
return new (allocator.allocate(sizeof(ProxyBoxShape))) ProxyBoxShape(this, body,
|
||||
transform, mass);
|
||||
}
|
||||
|
||||
// Return the collision shape
|
||||
inline const CollisionShape* ProxyBoxShape::getCollisionShape() const {
|
||||
return mCollisionShape;
|
||||
}
|
||||
|
||||
// Return the number of bytes used by the proxy collision shape
|
||||
inline size_t ProxyBoxShape::getSizeInBytes() const {
|
||||
return sizeof(ProxyBoxShape);
|
||||
}
|
||||
|
||||
// Return a local support point in a given direction with the object margin
|
||||
inline Vector3 ProxyBoxShape::getLocalSupportPointWithMargin(const Vector3& direction) {
|
||||
return mCollisionShape->getLocalSupportPointWithMargin(direction);
|
||||
}
|
||||
|
||||
// Return a local support point in a given direction without the object margin
|
||||
inline Vector3 ProxyBoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) {
|
||||
return mCollisionShape->getLocalSupportPointWithoutMargin(direction);
|
||||
}
|
||||
|
||||
// Return the current object margin
|
||||
inline decimal ProxyBoxShape::getMargin() const {
|
||||
return mCollisionShape->getMargin();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -55,7 +55,7 @@ CapsuleShape::~CapsuleShape() {
|
|||
/// Therefore, in this method, we compute the support points of both top and bottom spheres of
|
||||
/// the capsule and return the point with the maximum dot product with the direction vector. Note
|
||||
/// that the object margin is implicitly the radius and height of the capsule.
|
||||
Vector3 CapsuleShape::getLocalSupportPointWithMargin(const Vector3& direction) {
|
||||
Vector3 CapsuleShape::getLocalSupportPointWithMargin(const Vector3& direction) const {
|
||||
|
||||
// If the direction vector is not the zero vector
|
||||
if (direction.lengthSquare() >= MACHINE_EPSILON * MACHINE_EPSILON) {
|
||||
|
|
@ -87,7 +87,7 @@ Vector3 CapsuleShape::getLocalSupportPointWithMargin(const Vector3& direction) {
|
|||
}
|
||||
|
||||
// Return a local support point in a given direction without the object margin.
|
||||
Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) {
|
||||
Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
|
||||
|
||||
// If the dot product of the direction and the local Y axis (dotProduct = direction.y)
|
||||
// is positive
|
||||
|
|
@ -123,3 +123,15 @@ void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) co
|
|||
0.0, Iyy, 0.0,
|
||||
0.0, 0.0, IxxAndzz);
|
||||
}
|
||||
|
||||
// Constructor
|
||||
ProxyCapsuleShape::ProxyCapsuleShape(const CapsuleShape* shape, CollisionBody* body,
|
||||
const Transform& transform, decimal mass)
|
||||
:ProxyShape(body, transform, mass), mCollisionShape(shape){
|
||||
|
||||
}
|
||||
|
||||
// Destructor
|
||||
ProxyCapsuleShape::~ProxyCapsuleShape() {
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -86,10 +86,10 @@ class CapsuleShape : public CollisionShape {
|
|||
virtual size_t getSizeInBytes() const;
|
||||
|
||||
/// Return a local support point in a given direction with the object margin.
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const;
|
||||
|
||||
/// 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) const;
|
||||
|
||||
/// Return the local bounds of the shape in x, y and z directions
|
||||
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
|
||||
|
|
@ -99,6 +99,59 @@ class CapsuleShape : public CollisionShape {
|
|||
|
||||
/// Test equality between two capsule shapes
|
||||
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
|
||||
|
||||
/// Create a proxy collision shape for the collision shape
|
||||
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
||||
const Transform& transform, decimal mass) const;
|
||||
};
|
||||
|
||||
// Class ProxyCapsuleShape
|
||||
/**
|
||||
* The proxy collision shape for a capsule shape.
|
||||
*/
|
||||
class ProxyCapsuleShape : public ProxyShape {
|
||||
|
||||
private:
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Pointer to the actual collision shape
|
||||
const CapsuleShape* mCollisionShape;
|
||||
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Private copy-constructor
|
||||
ProxyCapsuleShape(const ProxyCapsuleShape& proxyShape);
|
||||
|
||||
/// Private assignment operator
|
||||
ProxyCapsuleShape& operator=(const ProxyCapsuleShape& proxyShape);
|
||||
|
||||
public:
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
ProxyCapsuleShape(const CapsuleShape* shape, CollisionBody* body,
|
||||
const Transform& transform, decimal mass);
|
||||
|
||||
/// Destructor
|
||||
~ProxyCapsuleShape();
|
||||
|
||||
/// Return the collision shape
|
||||
virtual const CollisionShape* getCollisionShape() const;
|
||||
|
||||
/// Return the number of bytes used by the proxy collision shape
|
||||
virtual size_t getSizeInBytes() const;
|
||||
|
||||
/// Return a local support point in a given direction with the object margin
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
|
||||
|
||||
/// Return a local support point in a given direction without the object margin
|
||||
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction);
|
||||
|
||||
/// Return the current collision shape margin
|
||||
virtual decimal getMargin() const;
|
||||
};
|
||||
|
||||
/// Allocate and return a copy of the object
|
||||
|
|
@ -142,6 +195,38 @@ inline bool CapsuleShape::isEqualTo(const CollisionShape& otherCollisionShape) c
|
|||
return (mRadius == otherShape.mRadius && mHalfHeight == otherShape.mHalfHeight);
|
||||
}
|
||||
|
||||
// Create a proxy collision shape for the collision shape
|
||||
inline ProxyShape* CapsuleShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
||||
const Transform& transform, decimal mass) const {
|
||||
return new (allocator.allocate(sizeof(ProxyCapsuleShape))) ProxyCapsuleShape(this, body,
|
||||
transform, mass);
|
||||
}
|
||||
|
||||
// Return the collision shape
|
||||
inline const CollisionShape* ProxyCapsuleShape::getCollisionShape() const {
|
||||
return mCollisionShape;
|
||||
}
|
||||
|
||||
// Return the number of bytes used by the proxy collision shape
|
||||
inline size_t ProxyCapsuleShape::getSizeInBytes() const {
|
||||
return sizeof(ProxyCapsuleShape);
|
||||
}
|
||||
|
||||
// Return a local support point in a given direction with the object margin
|
||||
inline Vector3 ProxyCapsuleShape::getLocalSupportPointWithMargin(const Vector3& direction) {
|
||||
return mCollisionShape->getLocalSupportPointWithMargin(direction);
|
||||
}
|
||||
|
||||
// Return a local support point in a given direction without the object margin
|
||||
inline Vector3 ProxyCapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) {
|
||||
return mCollisionShape->getLocalSupportPointWithoutMargin(direction);
|
||||
}
|
||||
|
||||
// Return the current object margin
|
||||
inline decimal ProxyCapsuleShape::getMargin() const {
|
||||
return mCollisionShape->getMargin();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -47,8 +47,8 @@ CollisionShape::~CollisionShape() {
|
|||
assert(mNbSimilarCreatedShapes == 0);
|
||||
}
|
||||
|
||||
// Update the AABB of a body using its collision shape
|
||||
void CollisionShape::updateAABB(AABB& aabb, const Transform& transform) {
|
||||
// Compute the world-space AABB of the collision shape given a transform
|
||||
void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const {
|
||||
|
||||
// Get the local bounds in x,y and z direction
|
||||
Vector3 minBounds;
|
||||
|
|
@ -72,3 +72,15 @@ void CollisionShape::updateAABB(AABB& aabb, const Transform& transform) {
|
|||
aabb.setMin(minCoordinates);
|
||||
aabb.setMax(maxCoordinates);
|
||||
}
|
||||
|
||||
// Constructor
|
||||
ProxyShape::ProxyShape(CollisionBody* body, const Transform& transform, decimal mass)
|
||||
:mBody(body), mLocalToBodyTransform(transform), mMass(mass), mNext(NULL),
|
||||
mBroadPhaseID(-1) {
|
||||
|
||||
}
|
||||
|
||||
// Destructor
|
||||
ProxyShape::~ProxyShape() {
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -32,6 +32,7 @@
|
|||
#include "../../mathematics/Vector3.h"
|
||||
#include "../../mathematics/Matrix3x3.h"
|
||||
#include "AABB.h"
|
||||
#include "../../memory/MemoryAllocator.h"
|
||||
|
||||
/// ReactPhysics3D namespace
|
||||
namespace reactphysics3d {
|
||||
|
|
@ -40,7 +41,8 @@ namespace reactphysics3d {
|
|||
enum CollisionShapeType {BOX, SPHERE, CONE, CYLINDER, CAPSULE, CONVEX_MESH};
|
||||
|
||||
// Declarations
|
||||
class Body;
|
||||
class CollisionBody;
|
||||
class ProxyShape;
|
||||
|
||||
// Class CollisionShape
|
||||
/**
|
||||
|
|
@ -95,20 +97,14 @@ class CollisionShape {
|
|||
/// Return the number of bytes used by the collision shape
|
||||
virtual size_t getSizeInBytes() const = 0;
|
||||
|
||||
/// Return a local support point in a given direction with the object margin
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction)=0;
|
||||
|
||||
/// Return a local support point in a given direction without the object margin
|
||||
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction)=0;
|
||||
|
||||
/// Return the local bounds of the shape in x, y and z directions
|
||||
virtual void getLocalBounds(Vector3& min, Vector3& max) const=0;
|
||||
|
||||
/// Return the local inertia tensor of the collision shapes
|
||||
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0;
|
||||
|
||||
/// Update the AABB of a body using its collision shape
|
||||
virtual void updateAABB(AABB& aabb, const Transform& transform);
|
||||
/// Compute the world-space AABB of the collision shape given a transform
|
||||
virtual void computeAABB(AABB& aabb, const Transform& transform) const;
|
||||
|
||||
/// Increment the number of similar allocated collision shapes
|
||||
void incrementNbSimilarCreatedShapes();
|
||||
|
|
@ -121,6 +117,93 @@ class CollisionShape {
|
|||
|
||||
/// Test equality between two collision shapes of the same type (same derived classes).
|
||||
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const=0;
|
||||
|
||||
/// Create a proxy collision shape for the collision shape
|
||||
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
||||
const Transform& transform, decimal mass) const=0;
|
||||
};
|
||||
|
||||
|
||||
// Class ProxyShape
|
||||
/**
|
||||
* The CollisionShape instances are supposed to be unique for memory optimization. For instance,
|
||||
* consider two rigid bodies with the same sphere collision shape. In this situation, we will have
|
||||
* a unique instance of SphereShape but we need to differentiate between the two instances during
|
||||
* the collision detection. They do not have the same position in the world and they do not
|
||||
* belong to the same rigid body. The ProxyShape class is used for that purpose by attaching a
|
||||
* rigid body with one of its collision shape. A body can have multiple proxy shapes (one for
|
||||
* each collision shape attached to the body).
|
||||
*/
|
||||
class ProxyShape {
|
||||
|
||||
private:
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Pointer to the parent body
|
||||
CollisionBody* mBody;
|
||||
|
||||
/// Local-space to parent body-space transform (does not change over time)
|
||||
const Transform mLocalToBodyTransform;
|
||||
|
||||
/// Mass (in kilogramms) of the corresponding collision shape
|
||||
decimal mMass;
|
||||
|
||||
/// Pointer to the next proxy shape of the body (linked list)
|
||||
ProxyShape* mNext;
|
||||
|
||||
/// Broad-phase ID (node ID in the dynamic AABB tree)
|
||||
int mBroadPhaseID;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Private copy-constructor
|
||||
ProxyShape(const ProxyShape& proxyShape);
|
||||
|
||||
/// Private assignment operator
|
||||
ProxyShape& operator=(const ProxyShape& proxyShape);
|
||||
|
||||
public:
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
ProxyShape(CollisionBody* body, const Transform& transform, decimal mass);
|
||||
|
||||
/// Destructor
|
||||
~ProxyShape();
|
||||
|
||||
/// Return the collision shape
|
||||
virtual const CollisionShape* getCollisionShape() const=0;
|
||||
|
||||
/// Return the number of bytes used by the proxy collision shape
|
||||
virtual size_t getSizeInBytes() const=0;
|
||||
|
||||
/// Return the parent body
|
||||
CollisionBody* getBody() const;
|
||||
|
||||
/// Return the mass of the collision shape
|
||||
decimal getMass() const;
|
||||
|
||||
/// Return the local to parent body transform
|
||||
const Transform& getLocalToBodyTransform() const;
|
||||
|
||||
/// Return a local support point in a given direction with the object margin
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction)=0;
|
||||
|
||||
/// Return a local support point in a given direction without the object margin
|
||||
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction)=0;
|
||||
|
||||
/// Return the current object margin
|
||||
virtual decimal getMargin() const=0;
|
||||
|
||||
// -------------------- Friendship -------------------- //
|
||||
|
||||
friend class OverlappingPair;
|
||||
friend class CollisionBody;
|
||||
friend class RigidBody;
|
||||
friend class BroadPhaseAlgorithm;
|
||||
friend class DynamicAABBTree;
|
||||
};
|
||||
|
||||
// Return the type of the collision shape
|
||||
|
|
@ -165,6 +248,21 @@ inline bool CollisionShape::operator==(const CollisionShape& otherCollisionShape
|
|||
return otherCollisionShape.isEqualTo(*this);
|
||||
}
|
||||
|
||||
// Return the parent body
|
||||
inline CollisionBody* ProxyShape::getBody() const {
|
||||
return mBody;
|
||||
}
|
||||
|
||||
// Return the mass of the collision shape
|
||||
inline decimal ProxyShape::getMass() const {
|
||||
return mMass;
|
||||
}
|
||||
|
||||
// Return the local to parent body transform
|
||||
inline const Transform& ProxyShape::getLocalToBodyTransform() const {
|
||||
return mLocalToBodyTransform;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -54,7 +54,7 @@ ConeShape::~ConeShape() {
|
|||
}
|
||||
|
||||
// Return a local support point in a given direction with the object margin
|
||||
Vector3 ConeShape::getLocalSupportPointWithMargin(const Vector3& direction) {
|
||||
Vector3 ConeShape::getLocalSupportPointWithMargin(const Vector3& direction) const {
|
||||
|
||||
// Compute the support point without the margin
|
||||
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction);
|
||||
|
|
@ -70,7 +70,7 @@ Vector3 ConeShape::getLocalSupportPointWithMargin(const Vector3& direction) {
|
|||
}
|
||||
|
||||
// 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) const {
|
||||
|
||||
const Vector3& v = direction;
|
||||
decimal sinThetaTimesLengthV = mSinTheta * v.length();
|
||||
|
|
@ -92,3 +92,15 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction) {
|
|||
|
||||
return supportPoint;
|
||||
}
|
||||
|
||||
// Constructor
|
||||
ProxyConeShape::ProxyConeShape(const ConeShape* shape, CollisionBody* body,
|
||||
const Transform& transform, decimal mass)
|
||||
:ProxyShape(body, transform, mass), mCollisionShape(shape){
|
||||
|
||||
}
|
||||
|
||||
// Destructor
|
||||
ProxyConeShape::~ProxyConeShape() {
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -94,10 +94,10 @@ class ConeShape : public CollisionShape {
|
|||
virtual size_t getSizeInBytes() const;
|
||||
|
||||
/// Return a local support point in a given direction with the object margin
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const;
|
||||
|
||||
/// 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) const;
|
||||
|
||||
/// Return the local bounds of the shape in x, y and z directions
|
||||
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
|
||||
|
|
@ -107,6 +107,59 @@ class ConeShape : public CollisionShape {
|
|||
|
||||
/// Test equality between two cone shapes
|
||||
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
|
||||
|
||||
/// Create a proxy collision shape for the collision shape
|
||||
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
||||
const Transform& transform, decimal mass) const;
|
||||
};
|
||||
|
||||
// Class ProxyConeShape
|
||||
/**
|
||||
* The proxy collision shape for a cone shape.
|
||||
*/
|
||||
class ProxyConeShape : public ProxyShape {
|
||||
|
||||
private:
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Pointer to the actual collision shape
|
||||
const ConeShape* mCollisionShape;
|
||||
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Private copy-constructor
|
||||
ProxyConeShape(const ProxyConeShape& proxyShape);
|
||||
|
||||
/// Private assignment operator
|
||||
ProxyConeShape& operator=(const ProxyConeShape& proxyShape);
|
||||
|
||||
public:
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
ProxyConeShape(const ConeShape* shape, CollisionBody* body,
|
||||
const Transform& transform, decimal mass);
|
||||
|
||||
/// Destructor
|
||||
~ProxyConeShape();
|
||||
|
||||
/// Return the collision shape
|
||||
virtual const CollisionShape* getCollisionShape() const;
|
||||
|
||||
/// Return the number of bytes used by the proxy collision shape
|
||||
virtual size_t getSizeInBytes() const;
|
||||
|
||||
/// Return a local support point in a given direction with the object margin
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
|
||||
|
||||
/// Return a local support point in a given direction without the object margin
|
||||
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction);
|
||||
|
||||
/// Return the current collision shape margin
|
||||
virtual decimal getMargin() const;
|
||||
};
|
||||
|
||||
// Allocate and return a copy of the object
|
||||
|
|
@ -158,6 +211,38 @@ inline bool ConeShape::isEqualTo(const CollisionShape& otherCollisionShape) cons
|
|||
return (mRadius == otherShape.mRadius && mHalfHeight == otherShape.mHalfHeight);
|
||||
}
|
||||
|
||||
// Create a proxy collision shape for the collision shape
|
||||
inline ProxyShape* ConeShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
||||
const Transform& transform, decimal mass) const {
|
||||
return new (allocator.allocate(sizeof(ProxyConeShape))) ProxyConeShape(this, body,
|
||||
transform, mass);
|
||||
}
|
||||
|
||||
// Return the collision shape
|
||||
inline const CollisionShape* ProxyConeShape::getCollisionShape() const {
|
||||
return mCollisionShape;
|
||||
}
|
||||
|
||||
// Return the number of bytes used by the proxy collision shape
|
||||
inline size_t ProxyConeShape::getSizeInBytes() const {
|
||||
return sizeof(ProxyConeShape);
|
||||
}
|
||||
|
||||
// Return a local support point in a given direction with the object margin
|
||||
inline Vector3 ProxyConeShape::getLocalSupportPointWithMargin(const Vector3& direction) {
|
||||
return mCollisionShape->getLocalSupportPointWithMargin(direction);
|
||||
}
|
||||
|
||||
// Return a local support point in a given direction without the object margin
|
||||
inline Vector3 ProxyConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction) {
|
||||
return mCollisionShape->getLocalSupportPointWithoutMargin(direction);
|
||||
}
|
||||
|
||||
// Return the current object margin
|
||||
inline decimal ProxyConeShape::getMargin() const {
|
||||
return mCollisionShape->getMargin();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -35,7 +35,7 @@ using namespace reactphysics3d;
|
|||
ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride,
|
||||
decimal margin)
|
||||
: CollisionShape(CONVEX_MESH, margin), mNbVertices(nbVertices), mMinBounds(0, 0, 0),
|
||||
mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false), mCachedSupportVertex(0) {
|
||||
mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) {
|
||||
assert(nbVertices > 0);
|
||||
assert(stride > 0);
|
||||
assert(margin > decimal(0.0));
|
||||
|
|
@ -58,7 +58,7 @@ ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices,
|
|||
/// the addVertex() method.
|
||||
ConvexMeshShape::ConvexMeshShape(decimal margin)
|
||||
: CollisionShape(CONVEX_MESH, margin), mNbVertices(0), mMinBounds(0, 0, 0),
|
||||
mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false), mCachedSupportVertex(0) {
|
||||
mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) {
|
||||
assert(margin > decimal(0.0));
|
||||
}
|
||||
|
||||
|
|
@ -67,8 +67,7 @@ ConvexMeshShape::ConvexMeshShape(const ConvexMeshShape& shape)
|
|||
: CollisionShape(shape), mVertices(shape.mVertices), mNbVertices(shape.mNbVertices),
|
||||
mMinBounds(shape.mMinBounds), mMaxBounds(shape.mMaxBounds),
|
||||
mIsEdgesInformationUsed(shape.mIsEdgesInformationUsed),
|
||||
mEdgesAdjacencyList(shape.mEdgesAdjacencyList),
|
||||
mCachedSupportVertex(shape.mCachedSupportVertex) {
|
||||
mEdgesAdjacencyList(shape.mEdgesAdjacencyList) {
|
||||
|
||||
assert(mNbVertices == mVertices.size());
|
||||
}
|
||||
|
|
@ -79,10 +78,11 @@ ConvexMeshShape::~ConvexMeshShape() {
|
|||
}
|
||||
|
||||
// Return a local support point in a given direction with the object margin
|
||||
Vector3 ConvexMeshShape::getLocalSupportPointWithMargin(const Vector3& direction) {
|
||||
Vector3 ConvexMeshShape::getLocalSupportPointWithMargin(const Vector3& direction,
|
||||
uint& cachedSupportVertex) const {
|
||||
|
||||
// Get the support point without the margin
|
||||
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction);
|
||||
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedSupportVertex);
|
||||
|
||||
// Get the unit direction vector
|
||||
Vector3 unitDirection = direction;
|
||||
|
|
@ -103,7 +103,8 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithMargin(const Vector3& direction
|
|||
/// it as a start in a hill-climbing (local search) process to find the new support vertex which
|
||||
/// will be in most of the cases very close to the previous one. Using hill-climbing, this method
|
||||
/// runs in almost constant time.
|
||||
Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction) {
|
||||
Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
|
||||
uint& cachedSupportVertex) const {
|
||||
|
||||
assert(mNbVertices == mVertices.size());
|
||||
|
||||
|
|
@ -112,7 +113,7 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
|
|||
|
||||
assert(mEdgesAdjacencyList.size() == mNbVertices);
|
||||
|
||||
uint maxVertex = mCachedSupportVertex;
|
||||
uint maxVertex = cachedSupportVertex;
|
||||
decimal maxDotProduct = direction.dot(mVertices[maxVertex]);
|
||||
bool isOptimal;
|
||||
|
||||
|
|
@ -142,7 +143,7 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
|
|||
} while(!isOptimal);
|
||||
|
||||
// Cache the support vertex
|
||||
mCachedSupportVertex = maxVertex;
|
||||
cachedSupportVertex = maxVertex;
|
||||
|
||||
// Return the support vertex
|
||||
return mVertices[maxVertex];
|
||||
|
|
@ -204,11 +205,7 @@ bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const
|
|||
|
||||
if (mNbVertices != otherShape.mNbVertices) return false;
|
||||
|
||||
// If edges information is used, it means that a collison shape object will store
|
||||
// cached data (previous support vertex) and therefore, we should not reuse the shape
|
||||
// for another body. Therefore, we consider that all convex mesh shape using edges
|
||||
// information are different.
|
||||
if (mIsEdgesInformationUsed) return false;
|
||||
if (mIsEdgesInformationUsed != otherShape.mIsEdgesInformationUsed) return false;
|
||||
|
||||
if (mEdgesAdjacencyList.size() != otherShape.mEdgesAdjacencyList.size()) return false;
|
||||
|
||||
|
|
@ -226,3 +223,16 @@ bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const
|
|||
|
||||
return true;
|
||||
}
|
||||
|
||||
// Constructor
|
||||
ProxyConvexMeshShape::ProxyConvexMeshShape(const ConvexMeshShape* shape, CollisionBody* body,
|
||||
const Transform& transform, decimal mass)
|
||||
:ProxyShape(body, transform, mass), mCollisionShape(shape),
|
||||
mCachedSupportVertex(0) {
|
||||
|
||||
}
|
||||
|
||||
// Destructor
|
||||
ProxyConvexMeshShape::~ProxyConvexMeshShape() {
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -77,9 +77,6 @@ class ConvexMeshShape : public CollisionShape {
|
|||
/// Adjacency list representing the edges of the mesh
|
||||
std::map<uint, std::set<uint> > mEdgesAdjacencyList;
|
||||
|
||||
/// Cached support vertex index (previous support vertex)
|
||||
uint mCachedSupportVertex;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Private copy-constructor
|
||||
|
|
@ -112,10 +109,12 @@ class ConvexMeshShape : public CollisionShape {
|
|||
virtual size_t getSizeInBytes() const;
|
||||
|
||||
/// Return a local support point in a given direction with the object margin
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
|
||||
uint& cachedSupportVertex) const;
|
||||
|
||||
/// 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,
|
||||
uint& cachedSupportVertex) const;
|
||||
|
||||
/// Return the local bounds of the shape in x, y and z directions
|
||||
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
|
||||
|
|
@ -123,7 +122,7 @@ class ConvexMeshShape : public CollisionShape {
|
|||
/// Return the local inertia tensor of the collision shape.
|
||||
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
|
||||
|
||||
/// Test equality between two cone shapes
|
||||
/// Test equality between two collision shapes
|
||||
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
|
||||
|
||||
/// Add a vertex into the convex mesh
|
||||
|
|
@ -138,6 +137,62 @@ class ConvexMeshShape : public CollisionShape {
|
|||
/// Set the variable to know if the edges information is used to speed up the
|
||||
/// collision detection
|
||||
void setIsEdgesInformationUsed(bool isEdgesUsed);
|
||||
|
||||
/// Create a proxy collision shape for the collision shape
|
||||
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
||||
const Transform& transform, decimal mass) const;
|
||||
};
|
||||
|
||||
|
||||
// Class ProxyConvexMeshSphape
|
||||
/**
|
||||
* The proxy collision shape for a convex mesh shape.
|
||||
*/
|
||||
class ProxyConvexMeshShape : public ProxyShape {
|
||||
|
||||
private:
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Pointer to the actual collision shape
|
||||
const ConvexMeshShape* mCollisionShape;
|
||||
|
||||
/// Cached support vertex index (previous support vertex for hill-climbing)
|
||||
uint mCachedSupportVertex;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Private copy-constructor
|
||||
ProxyConvexMeshShape(const ProxyConvexMeshShape& proxyShape);
|
||||
|
||||
/// Private assignment operator
|
||||
ProxyConvexMeshShape& operator=(const ProxyConvexMeshShape& proxyShape);
|
||||
|
||||
public:
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
ProxyConvexMeshShape(const ConvexMeshShape* shape, CollisionBody* body,
|
||||
const Transform& transform, decimal mass);
|
||||
|
||||
/// Destructor
|
||||
~ProxyConvexMeshShape();
|
||||
|
||||
/// Return the collision shape
|
||||
virtual const CollisionShape* getCollisionShape() const;
|
||||
|
||||
/// Return the number of bytes used by the proxy collision shape
|
||||
virtual size_t getSizeInBytes() const;
|
||||
|
||||
/// Return a local support point in a given direction with the object margin
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
|
||||
|
||||
/// Return a local support point in a given direction without the object margin
|
||||
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction);
|
||||
|
||||
/// Return the current collision shape margin
|
||||
virtual decimal getMargin() const;
|
||||
};
|
||||
|
||||
// Allocate and return a copy of the object
|
||||
|
|
@ -222,6 +277,40 @@ inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) {
|
|||
mIsEdgesInformationUsed = isEdgesUsed;
|
||||
}
|
||||
|
||||
// Create a proxy collision shape for the collision shape
|
||||
inline ProxyShape* ConvexMeshShape::createProxyShape(MemoryAllocator& allocator,
|
||||
CollisionBody* body,
|
||||
const Transform& transform,
|
||||
decimal mass) const {
|
||||
return new (allocator.allocate(sizeof(ProxyConvexMeshShape))) ProxyConvexMeshShape(this, body,
|
||||
transform, mass);
|
||||
}
|
||||
|
||||
// Return the collision shape
|
||||
inline const CollisionShape* ProxyConvexMeshShape::getCollisionShape() const {
|
||||
return mCollisionShape;
|
||||
}
|
||||
|
||||
// Return the number of bytes used by the proxy collision shape
|
||||
inline size_t ProxyConvexMeshShape::getSizeInBytes() const {
|
||||
return sizeof(ProxyConvexMeshShape);
|
||||
}
|
||||
|
||||
// Return a local support point in a given direction with the object margin
|
||||
inline Vector3 ProxyConvexMeshShape::getLocalSupportPointWithMargin(const Vector3& direction) {
|
||||
return mCollisionShape->getLocalSupportPointWithMargin(direction, mCachedSupportVertex);
|
||||
}
|
||||
|
||||
// Return a local support point in a given direction without the object margin
|
||||
inline Vector3 ProxyConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction) {
|
||||
return mCollisionShape->getLocalSupportPointWithoutMargin(direction, mCachedSupportVertex);
|
||||
}
|
||||
|
||||
// Return the current object margin
|
||||
inline decimal ProxyConvexMeshShape::getMargin() const {
|
||||
return mCollisionShape->getMargin();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -50,7 +50,7 @@ CylinderShape::~CylinderShape() {
|
|||
}
|
||||
|
||||
// Return a local support point in a given direction with the object margin
|
||||
Vector3 CylinderShape::getLocalSupportPointWithMargin(const Vector3& direction) {
|
||||
Vector3 CylinderShape::getLocalSupportPointWithMargin(const Vector3& direction) const {
|
||||
|
||||
// Compute the support point without the margin
|
||||
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction);
|
||||
|
|
@ -66,7 +66,7 @@ Vector3 CylinderShape::getLocalSupportPointWithMargin(const Vector3& direction)
|
|||
}
|
||||
|
||||
// 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) const {
|
||||
|
||||
Vector3 supportPoint(0.0, 0.0, 0.0);
|
||||
decimal uDotv = direction.y;
|
||||
|
|
@ -85,3 +85,15 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio
|
|||
|
||||
return supportPoint;
|
||||
}
|
||||
|
||||
// Constructor
|
||||
ProxyCylinderShape::ProxyCylinderShape(const CylinderShape* cylinderShape, CollisionBody* body,
|
||||
const Transform& transform, decimal mass)
|
||||
:ProxyShape(body, transform, mass), mCollisionShape(cylinderShape){
|
||||
|
||||
}
|
||||
|
||||
// Destructor
|
||||
ProxyCylinderShape::~ProxyCylinderShape() {
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -91,10 +91,10 @@ class CylinderShape : public CollisionShape {
|
|||
virtual size_t getSizeInBytes() const;
|
||||
|
||||
/// Return a local support point in a given direction with the object margin
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const;
|
||||
|
||||
/// 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) const;
|
||||
|
||||
/// Return the local bounds of the shape in x, y and z directions
|
||||
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
|
||||
|
|
@ -104,6 +104,60 @@ class CylinderShape : public CollisionShape {
|
|||
|
||||
/// Test equality between two cylinder shapes
|
||||
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
|
||||
|
||||
/// Create a proxy collision shape for the collision shape
|
||||
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
||||
const Transform& transform, decimal mass) const;
|
||||
|
||||
};
|
||||
|
||||
// Class ProxyCylinderShape
|
||||
/**
|
||||
* The proxy collision shape for a cylinder shape.
|
||||
*/
|
||||
class ProxyCylinderShape : public ProxyShape {
|
||||
|
||||
private:
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Pointer to the actual collision shape
|
||||
const CylinderShape* mCollisionShape;
|
||||
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Private copy-constructor
|
||||
ProxyCylinderShape(const ProxyCylinderShape& proxyShape);
|
||||
|
||||
/// Private assignment operator
|
||||
ProxyCylinderShape& operator=(const ProxyCylinderShape& proxyShape);
|
||||
|
||||
public:
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
ProxyCylinderShape(const CylinderShape* cylinderShape, CollisionBody* body,
|
||||
const Transform& transform, decimal mass);
|
||||
|
||||
/// Destructor
|
||||
~ProxyCylinderShape();
|
||||
|
||||
/// Return the collision shape
|
||||
virtual const CollisionShape* getCollisionShape() const;
|
||||
|
||||
/// Return the number of bytes used by the proxy collision shape
|
||||
virtual size_t getSizeInBytes() const;
|
||||
|
||||
/// Return a local support point in a given direction with the object margin
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
|
||||
|
||||
/// Return a local support point in a given direction without the object margin
|
||||
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction);
|
||||
|
||||
/// Return the current collision shape margin
|
||||
virtual decimal getMargin() const;
|
||||
};
|
||||
|
||||
/// Allocate and return a copy of the object
|
||||
|
|
@ -155,6 +209,38 @@ inline bool CylinderShape::isEqualTo(const CollisionShape& otherCollisionShape)
|
|||
return (mRadius == otherShape.mRadius && mHalfHeight == otherShape.mHalfHeight);
|
||||
}
|
||||
|
||||
// Create a proxy collision shape for the collision shape
|
||||
inline ProxyShape* CylinderShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
||||
const Transform& transform, decimal mass) const {
|
||||
return new (allocator.allocate(sizeof(ProxyCylinderShape))) ProxyCylinderShape(this, body,
|
||||
transform, mass);
|
||||
}
|
||||
|
||||
// Return the collision shape
|
||||
inline const CollisionShape* ProxyCylinderShape::getCollisionShape() const {
|
||||
return mCollisionShape;
|
||||
}
|
||||
|
||||
// Return the number of bytes used by the proxy collision shape
|
||||
inline size_t ProxyCylinderShape::getSizeInBytes() const {
|
||||
return sizeof(ProxyCylinderShape);
|
||||
}
|
||||
|
||||
// Return a local support point in a given direction with the object margin
|
||||
inline Vector3 ProxyCylinderShape::getLocalSupportPointWithMargin(const Vector3& direction) {
|
||||
return mCollisionShape->getLocalSupportPointWithMargin(direction);
|
||||
}
|
||||
|
||||
// Return a local support point in a given direction without the object margin
|
||||
inline Vector3 ProxyCylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction) {
|
||||
return mCollisionShape->getLocalSupportPointWithoutMargin(direction);
|
||||
}
|
||||
|
||||
// Return the current object margin
|
||||
inline decimal ProxyCylinderShape::getMargin() const {
|
||||
return mCollisionShape->getMargin();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -45,3 +45,15 @@ SphereShape::SphereShape(const SphereShape& shape)
|
|||
SphereShape::~SphereShape() {
|
||||
|
||||
}
|
||||
|
||||
// Constructor
|
||||
ProxySphereShape::ProxySphereShape(const SphereShape* shape, CollisionBody* body,
|
||||
const Transform& transform, decimal mass)
|
||||
:ProxyShape(body, transform, mass), mCollisionShape(shape){
|
||||
|
||||
}
|
||||
|
||||
// Destructor
|
||||
ProxySphereShape::~ProxySphereShape() {
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -78,10 +78,10 @@ class SphereShape : public CollisionShape {
|
|||
virtual size_t getSizeInBytes() const;
|
||||
|
||||
/// Return a local support point in a given direction with the object margin
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const;
|
||||
|
||||
/// 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) const;
|
||||
|
||||
/// Return the local bounds of the shape in x, y and z directions.
|
||||
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
|
||||
|
|
@ -90,10 +90,64 @@ class SphereShape : public CollisionShape {
|
|||
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
|
||||
|
||||
/// Update the AABB of a body using its collision shape
|
||||
virtual void updateAABB(AABB& aabb, const Transform& transform);
|
||||
virtual void computeAABB(AABB& aabb, const Transform& transform);
|
||||
|
||||
/// Test equality between two sphere shapes
|
||||
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
|
||||
|
||||
/// Create a proxy collision shape for the collision shape
|
||||
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
||||
const Transform& transform, decimal mass) const;
|
||||
};
|
||||
|
||||
|
||||
// Class ProxySphereShape
|
||||
/**
|
||||
* The proxy collision shape for a sphere shape.
|
||||
*/
|
||||
class ProxySphereShape : public ProxyShape {
|
||||
|
||||
private:
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Pointer to the actual collision shape
|
||||
const SphereShape* mCollisionShape;
|
||||
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Private copy-constructor
|
||||
ProxySphereShape(const ProxySphereShape& proxyShape);
|
||||
|
||||
/// Private assignment operator
|
||||
ProxySphereShape& operator=(const ProxySphereShape& proxyShape);
|
||||
|
||||
public:
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
ProxySphereShape(const SphereShape* shape, CollisionBody* body,
|
||||
const Transform& transform, decimal mass);
|
||||
|
||||
/// Destructor
|
||||
~ProxySphereShape();
|
||||
|
||||
/// Return the collision shape
|
||||
virtual const CollisionShape* getCollisionShape() const;
|
||||
|
||||
/// Return the number of bytes used by the proxy collision shape
|
||||
virtual size_t getSizeInBytes() const;
|
||||
|
||||
/// Return a local support point in a given direction with the object margin
|
||||
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
|
||||
|
||||
/// Return a local support point in a given direction without the object margin
|
||||
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction);
|
||||
|
||||
/// Return the current collision shape margin
|
||||
virtual decimal getMargin() const;
|
||||
};
|
||||
|
||||
/// Allocate and return a copy of the object
|
||||
|
|
@ -112,7 +166,7 @@ inline size_t SphereShape::getSizeInBytes() const {
|
|||
}
|
||||
|
||||
// Return a local support point in a given direction with the object margin
|
||||
inline Vector3 SphereShape::getLocalSupportPointWithMargin(const Vector3& direction) {
|
||||
inline Vector3 SphereShape::getLocalSupportPointWithMargin(const Vector3& direction) const {
|
||||
|
||||
// If the direction vector is not the zero vector
|
||||
if (direction.lengthSquare() >= MACHINE_EPSILON * MACHINE_EPSILON) {
|
||||
|
|
@ -127,7 +181,7 @@ inline Vector3 SphereShape::getLocalSupportPointWithMargin(const Vector3& direct
|
|||
}
|
||||
|
||||
// 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) const {
|
||||
|
||||
// Return the center of the sphere (the radius is taken into account in the object margin)
|
||||
return Vector3(0.0, 0.0, 0.0);
|
||||
|
|
@ -157,7 +211,7 @@ inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal ma
|
|||
}
|
||||
|
||||
// Update the AABB of a body using its collision shape
|
||||
inline void SphereShape::updateAABB(AABB& aabb, const Transform& transform) {
|
||||
inline void SphereShape::computeAABB(AABB& aabb, const Transform& transform) {
|
||||
|
||||
// Get the local extents in x,y and z direction
|
||||
Vector3 extents(mRadius, mRadius, mRadius);
|
||||
|
|
@ -173,6 +227,38 @@ inline bool SphereShape::isEqualTo(const CollisionShape& otherCollisionShape) co
|
|||
return (mRadius == otherShape.mRadius);
|
||||
}
|
||||
|
||||
// Create a proxy collision shape for the collision shape
|
||||
inline ProxyShape* SphereShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
||||
const Transform& transform, decimal mass) const {
|
||||
return new (allocator.allocate(sizeof(ProxySphereShape))) ProxySphereShape(this, body,
|
||||
transform, mass);
|
||||
}
|
||||
|
||||
// Return the collision shape
|
||||
inline const CollisionShape* ProxySphereShape::getCollisionShape() const {
|
||||
return mCollisionShape;
|
||||
}
|
||||
|
||||
// Return the number of bytes used by the proxy collision shape
|
||||
inline size_t ProxySphereShape::getSizeInBytes() const {
|
||||
return sizeof(ProxySphereShape);
|
||||
}
|
||||
|
||||
// Return a local support point in a given direction with the object margin
|
||||
inline Vector3 ProxySphereShape::getLocalSupportPointWithMargin(const Vector3& direction) {
|
||||
return mCollisionShape->getLocalSupportPointWithMargin(direction);
|
||||
}
|
||||
|
||||
// Return a local support point in a given direction without the object margin
|
||||
inline Vector3 ProxySphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction) {
|
||||
return mCollisionShape->getLocalSupportPointWithoutMargin(direction);
|
||||
}
|
||||
|
||||
// Return the current object margin
|
||||
inline decimal ProxySphereShape::getMargin() const {
|
||||
return mCollisionShape->getMargin();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -125,11 +125,6 @@ const decimal DEFAULT_SLEEP_ANGULAR_VELOCITY = decimal(3.0 * (PI / 180.0));
|
|||
/// fatten to allow the collision shape to move a little bit without triggering
|
||||
/// a large modification of the tree which can be costly
|
||||
const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1);
|
||||
|
||||
/// In the dynamic AABB tree, we multiply this factor by the displacement of
|
||||
/// an object that has moved to recompute a new fat AABB
|
||||
const decimal AABB_DISPLACEMENT_MULTIPLIER = decimal(2.0);
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -53,9 +53,9 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
|
|||
mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
|
||||
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
|
||||
|
||||
// Get the bodies positions and orientations
|
||||
const Vector3& x1 = mBody1->getTransform().getPosition();
|
||||
const Vector3& x2 = mBody2->getTransform().getPosition();
|
||||
// Get the bodies center of mass and orientations
|
||||
const Vector3& x1 = mBody1->mCenterOfMassWorld;
|
||||
const Vector3& x2 = mBody2->mCenterOfMassWorld;
|
||||
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
|
||||
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
|
||||
|
||||
|
|
@ -164,7 +164,7 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
|
|||
// do not execute this method
|
||||
if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
|
||||
|
||||
// Get the bodies positions and orientations
|
||||
// Get the bodies center of mass and orientations
|
||||
Vector3& x1 = constraintSolverData.positions[mIndexBody1];
|
||||
Vector3& x2 = constraintSolverData.positions[mIndexBody2];
|
||||
Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
|
||||
|
|
@ -214,7 +214,7 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
|
|||
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
|
||||
const Vector3 w1 = mI1 * angularImpulseBody1;
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
// Update the body center of mass and orientation of body 1
|
||||
x1 += v1;
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
|
|
|||
|
|
@ -62,8 +62,8 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
|
|||
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
|
||||
|
||||
// Get the bodies positions and orientations
|
||||
const Vector3& x1 = mBody1->getTransform().getPosition();
|
||||
const Vector3& x2 = mBody2->getTransform().getPosition();
|
||||
const Vector3& x1 = mBody1->mCenterOfMassWorld;
|
||||
const Vector3& x2 = mBody2->mCenterOfMassWorld;
|
||||
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
|
||||
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
|
||||
|
||||
|
|
|
|||
|
|
@ -77,8 +77,8 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
|
|||
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
|
||||
|
||||
// Get the bodies positions and orientations
|
||||
const Vector3& x1 = mBody1->getTransform().getPosition();
|
||||
const Vector3& x2 = mBody2->getTransform().getPosition();
|
||||
const Vector3& x1 = mBody1->mCenterOfMassWorld;
|
||||
const Vector3& x2 = mBody2->mCenterOfMassWorld;
|
||||
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
|
||||
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
|
||||
|
||||
|
|
|
|||
|
|
@ -76,8 +76,8 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
|
|||
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
|
||||
|
||||
// Get the bodies positions and orientations
|
||||
const Vector3& x1 = mBody1->getTransform().getPosition();
|
||||
const Vector3& x2 = mBody2->getTransform().getPosition();
|
||||
const Vector3& x1 = mBody1-mCenterOfMassWorld;
|
||||
const Vector3& x2 = mBody2->mCenterOfMassWorld;
|
||||
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
|
||||
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
|
||||
|
||||
|
|
@ -679,6 +679,7 @@ void SliderJoint::enableMotor(bool isMotorEnabled) {
|
|||
}
|
||||
|
||||
// Return the current translation value of the joint
|
||||
// TODO : Check if we need to compare rigid body position or center of mass here
|
||||
decimal SliderJoint::getTranslation() const {
|
||||
|
||||
// Get the bodies positions and orientations
|
||||
|
|
|
|||
|
|
@ -43,27 +43,15 @@ CollisionWorld::~CollisionWorld() {
|
|||
assert(mBodies.empty());
|
||||
}
|
||||
|
||||
// Notify the world about a new broad-phase overlapping pair
|
||||
void CollisionWorld::notifyAddedOverlappingPair(const BroadPhasePair* addedPair) {
|
||||
|
||||
// TODO : Implement this method
|
||||
}
|
||||
|
||||
// Notify the world about a removed broad-phase overlapping pair
|
||||
void CollisionWorld::notifyRemovedOverlappingPair(const BroadPhasePair* removedPair) {
|
||||
|
||||
// TODO : Implement this method
|
||||
}
|
||||
|
||||
// Notify the world about a new narrow-phase contact
|
||||
void CollisionWorld::notifyNewContact(const BroadPhasePair* broadPhasePair,
|
||||
void CollisionWorld::notifyNewContact(const OverlappingPair *broadPhasePair,
|
||||
const ContactPointInfo* contactInfo) {
|
||||
|
||||
// TODO : Implement this method
|
||||
}
|
||||
|
||||
// Update the overlapping pair
|
||||
inline void CollisionWorld::updateOverlappingPair(const BroadPhasePair* pair) {
|
||||
inline void CollisionWorld::updateOverlappingPair(const OverlappingPair *pair) {
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -87,7 +75,7 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform,
|
|||
mBodies.insert(collisionBody);
|
||||
|
||||
// Add the collision body to the collision detection
|
||||
mCollisionDetection.addBody(collisionBody);
|
||||
mCollisionDetection.addProxyCollisionShape(collisionBody);
|
||||
|
||||
// Return the pointer to the rigid body
|
||||
return collisionBody;
|
||||
|
|
@ -97,7 +85,7 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform,
|
|||
void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
|
||||
|
||||
// Remove the body from the collision detection
|
||||
mCollisionDetection.removeBody(collisionBody);
|
||||
mCollisionDetection.removeProxyCollisionShape(collisionBody);
|
||||
|
||||
// Add the body ID to the list of free IDs
|
||||
mFreeBodiesIDs.push_back(collisionBody->getID());
|
||||
|
|
@ -129,7 +117,7 @@ bodyindex CollisionWorld::computeNextAvailableBodyID() {
|
|||
return bodyID;
|
||||
}
|
||||
|
||||
// Create a new collision shape.
|
||||
// Create a new collision shape in the world.
|
||||
/// First, this methods checks that the new collision shape does not exist yet in the
|
||||
/// world. If it already exists, we do not allocate memory for a new one but instead
|
||||
/// we reuse the existing one. The goal is to only allocate memory for a single
|
||||
|
|
|
|||
|
|
@ -85,28 +85,22 @@ class CollisionWorld {
|
|||
/// Private assignment operator
|
||||
CollisionWorld& operator=(const CollisionWorld& world);
|
||||
|
||||
/// Notify the world about a new broad-phase overlapping pair
|
||||
virtual void notifyAddedOverlappingPair(const BroadPhasePair* addedPair);
|
||||
|
||||
/// Notify the world about a removed broad-phase overlapping pair
|
||||
virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair);
|
||||
|
||||
/// Notify the world about a new narrow-phase contact
|
||||
virtual void notifyNewContact(const BroadPhasePair* pair,
|
||||
virtual void notifyNewContact(const OverlappingPair* pair,
|
||||
const ContactPointInfo* contactInfo);
|
||||
|
||||
/// Update the overlapping pair
|
||||
virtual void updateOverlappingPair(const BroadPhasePair* pair);
|
||||
virtual void updateOverlappingPair(const OverlappingPair* pair);
|
||||
|
||||
/// Return the next available body ID
|
||||
bodyindex computeNextAvailableBodyID();
|
||||
|
||||
/// Create a new collision shape.
|
||||
CollisionShape* createCollisionShape(const CollisionShape& collisionShape);
|
||||
|
||||
/// Remove a collision shape.
|
||||
void removeCollisionShape(CollisionShape* collisionShape);
|
||||
|
||||
/// Create a new collision shape in the world.
|
||||
CollisionShape* createCollisionShape(const CollisionShape& collisionShape);
|
||||
|
||||
public :
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
|
@ -133,6 +127,8 @@ class CollisionWorld {
|
|||
// -------------------- Friends -------------------- //
|
||||
|
||||
friend class CollisionDetection;
|
||||
friend class CollisionBody;
|
||||
friend class RigidBody;
|
||||
};
|
||||
|
||||
// Return an iterator to the beginning of the bodies of the physics world
|
||||
|
|
|
|||
|
|
@ -87,8 +87,8 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
|
|||
RigidBody* body2 = externalManifold->getContactPoint(0)->getBody2();
|
||||
|
||||
// Get the position of the two bodies
|
||||
Vector3 x1 = body1->getTransform().getPosition();
|
||||
Vector3 x2 = body2->getTransform().getPosition();
|
||||
const Vector3& x1 = body1->mCenterOfMassWorld;
|
||||
const Vector3& x2 = body2->mCenterOfMassWorld;
|
||||
|
||||
// Initialize the internal contact manifold structure using the external
|
||||
// contact manifold
|
||||
|
|
|
|||
|
|
@ -34,6 +34,14 @@
|
|||
using namespace reactphysics3d;
|
||||
using namespace std;
|
||||
|
||||
// TODO : Check if we really need to store the contact manifolds also in mContactManifolds.
|
||||
|
||||
// TODO : Check how to compute the initial inertia tensor now (especially when a body has multiple
|
||||
// collision shapes.
|
||||
|
||||
// TODO : Check the Body::setType() function in Box2D to be sure we are not missing any update
|
||||
// of the collision shapes and broad-phase modification.
|
||||
|
||||
// Constructor
|
||||
DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
|
||||
: CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityEnabled(true),
|
||||
|
|
@ -128,9 +136,6 @@ void DynamicsWorld::update() {
|
|||
// Integrate the velocities
|
||||
integrateRigidBodiesVelocities();
|
||||
|
||||
// Reset the movement boolean variable of each body to false
|
||||
resetBodiesMovementVariable();
|
||||
|
||||
// Update the timer
|
||||
mTimer.nextStep();
|
||||
|
||||
|
|
@ -199,6 +204,9 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
|
|||
Quaternion newOrientation = currentOrientation + Quaternion(0, newAngVelocity) *
|
||||
currentOrientation * decimal(0.5) * dt;
|
||||
|
||||
// Update the world-space center of mass
|
||||
// TODO : IMPLEMENT THIS
|
||||
|
||||
// Update the Transform of the body
|
||||
Transform newTransform(newPosition, newOrientation.getUnit());
|
||||
bodies[b]->setTransform(newTransform);
|
||||
|
|
@ -218,6 +226,9 @@ void DynamicsWorld::updateRigidBodiesAABB() {
|
|||
// If the body has moved
|
||||
if ((*it)->mHasMoved) {
|
||||
|
||||
// Update the transform of the body due to the change of its center of mass
|
||||
(*it)->updateTransformWithCenterOfMass();
|
||||
|
||||
// Update the AABB of the rigid body
|
||||
(*it)->updateAABB();
|
||||
}
|
||||
|
|
@ -444,7 +455,7 @@ void DynamicsWorld::solvePositionCorrection() {
|
|||
|
||||
// Get the position/orientation of the rigid body
|
||||
const Transform& transform = bodies[b]->getTransform();
|
||||
mConstrainedPositions[index] = transform.getPosition();
|
||||
mConstrainedPositions[index] = bodies[b]->mCenterOfMassWorld;
|
||||
mConstrainedOrientations[index]= transform.getOrientation();
|
||||
}
|
||||
|
||||
|
|
@ -463,20 +474,20 @@ void DynamicsWorld::solvePositionCorrection() {
|
|||
|
||||
uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
|
||||
|
||||
// Get the new position/orientation of the body
|
||||
const Vector3& newPosition = mConstrainedPositions[index];
|
||||
const Quaternion& newOrientation = mConstrainedOrientations[index];
|
||||
// Update the position of the center of mass of the body
|
||||
bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index];
|
||||
|
||||
// Update the Transform of the body
|
||||
Transform newTransform(newPosition, newOrientation.getUnit());
|
||||
bodies[b]->setTransform(newTransform);
|
||||
// Update the orientation of the body
|
||||
bodies[b]->mTransform.setOrientation(mConstrainedOrientations[index].getUnit());
|
||||
|
||||
// Update the Transform of the body (using the new center of mass and new orientation)
|
||||
bodies[b]->updateTransformWithCenterOfMass();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Create a rigid body into the physics world
|
||||
RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal mass,
|
||||
const CollisionShape& collisionShape) {
|
||||
RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal mass) {
|
||||
|
||||
// Compute the body ID
|
||||
bodyindex bodyID = computeNextAvailableBodyID();
|
||||
|
|
@ -484,22 +495,18 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal ma
|
|||
// Largest index cannot be used (it is used for invalid index)
|
||||
assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::max());
|
||||
|
||||
// Create a collision shape for the rigid body into the world
|
||||
CollisionShape* newCollisionShape = createCollisionShape(collisionShape);
|
||||
|
||||
// Create the rigid body
|
||||
RigidBody* rigidBody = new (mMemoryAllocator.allocate(sizeof(RigidBody))) RigidBody(transform,
|
||||
mass,
|
||||
newCollisionShape,
|
||||
bodyID);
|
||||
mass, bodyID);
|
||||
assert(rigidBody != NULL);
|
||||
|
||||
// Add the rigid body to the physics world
|
||||
mBodies.insert(rigidBody);
|
||||
mRigidBodies.insert(rigidBody);
|
||||
|
||||
// TODO : DELETE THIS
|
||||
// Add the rigid body to the collision detection
|
||||
mCollisionDetection.addBody(rigidBody);
|
||||
//mCollisionDetection.addProxyCollisionShape(rigidBody);
|
||||
|
||||
// Return the pointer to the rigid body
|
||||
return rigidBody;
|
||||
|
|
@ -508,14 +515,16 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal ma
|
|||
// Destroy a rigid body and all the joints which it belongs
|
||||
void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
|
||||
|
||||
// TODO : DELETE THIS
|
||||
// Remove the body from the collision detection
|
||||
mCollisionDetection.removeBody(rigidBody);
|
||||
//mCollisionDetection.removeProxyCollisionShape(rigidBody);
|
||||
|
||||
// Add the body ID to the list of free IDs
|
||||
mFreeBodiesIDs.push_back(rigidBody->getID());
|
||||
|
||||
// TODO : DELETE THIS
|
||||
// Remove the collision shape from the world
|
||||
removeCollisionShape(rigidBody->getCollisionShape());
|
||||
//removeCollisionShape(rigidBody->getCollisionShape());
|
||||
|
||||
// Destroy all the joints in which the rigid body to be destroyed is involved
|
||||
JointListElement* element;
|
||||
|
|
@ -910,33 +919,6 @@ void DynamicsWorld::updateSleepingBodies() {
|
|||
}
|
||||
}
|
||||
|
||||
// Notify the world about a new broad-phase overlapping pair
|
||||
void DynamicsWorld::notifyAddedOverlappingPair(const BroadPhasePair* addedPair) {
|
||||
|
||||
// Get the pair of body index
|
||||
bodyindexpair indexPair = addedPair->getBodiesIndexPair();
|
||||
|
||||
// Add the pair into the set of overlapping pairs (if not there yet)
|
||||
OverlappingPair* newPair = new (mMemoryAllocator.allocate(sizeof(OverlappingPair)))
|
||||
OverlappingPair(addedPair->body1, addedPair->body2, mMemoryAllocator);
|
||||
assert(newPair != NULL);
|
||||
std::pair<map<bodyindexpair, OverlappingPair*>::iterator, bool> check =
|
||||
mOverlappingPairs.insert(make_pair(indexPair, newPair));
|
||||
assert(check.second);
|
||||
}
|
||||
|
||||
// Notify the world about a removed broad-phase overlapping pair
|
||||
void DynamicsWorld::notifyRemovedOverlappingPair(const BroadPhasePair* removedPair) {
|
||||
|
||||
// Get the pair of body index
|
||||
std::pair<bodyindex, bodyindex> indexPair = removedPair->getBodiesIndexPair();
|
||||
|
||||
// Remove the overlapping pair from the memory allocator
|
||||
mOverlappingPairs.find(indexPair)->second->OverlappingPair::~OverlappingPair();
|
||||
mMemoryAllocator.release(mOverlappingPairs[indexPair], sizeof(OverlappingPair));
|
||||
mOverlappingPairs.erase(indexPair);
|
||||
}
|
||||
|
||||
// Notify the world about a new narrow-phase contact
|
||||
void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair,
|
||||
const ContactPointInfo* contactInfo) {
|
||||
|
|
|
|||
|
|
@ -154,6 +154,11 @@ class DynamicsWorld : public CollisionWorld {
|
|||
void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
|
||||
Vector3 newAngVelocity);
|
||||
|
||||
/// Add a contact manifold to the linked list of contact manifolds of the two bodies
|
||||
/// involed in the corresponding contact.
|
||||
void addContactManifoldToBody(ContactManifold* contactManifold,
|
||||
CollisionBody *body1, CollisionBody *body2);
|
||||
|
||||
/// Compute and set the interpolation factor to all bodies
|
||||
void setInterpolationFactorToAllBodies();
|
||||
|
||||
|
|
@ -172,9 +177,6 @@ class DynamicsWorld : public CollisionWorld {
|
|||
/// Cleanup the constrained velocities array at each step
|
||||
void cleanupConstrainedVelocitiesArray();
|
||||
|
||||
/// Reset the boolean movement variable of each body
|
||||
void resetBodiesMovementVariable();
|
||||
|
||||
/// Compute the islands of awake bodies.
|
||||
void computeIslands();
|
||||
|
||||
|
|
@ -184,12 +186,6 @@ class DynamicsWorld : public CollisionWorld {
|
|||
/// Update the overlapping pair
|
||||
virtual void updateOverlappingPair(const BroadPhasePair* pair);
|
||||
|
||||
/// Notify the world about a new broad-phase overlapping pair
|
||||
virtual void notifyAddedOverlappingPair(const BroadPhasePair* addedPair);
|
||||
|
||||
/// Notify the world about a removed broad-phase overlapping pair
|
||||
virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair);
|
||||
|
||||
/// Notify the world about a new narrow-phase contact
|
||||
virtual void notifyNewContact(const BroadPhasePair* pair,
|
||||
const ContactPointInfo* contactInfo);
|
||||
|
|
@ -230,8 +226,7 @@ class DynamicsWorld : public CollisionWorld {
|
|||
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
|
||||
|
||||
/// Create a rigid body into the physics world.
|
||||
RigidBody* createRigidBody(const Transform& transform, decimal mass,
|
||||
const CollisionShape& collisionShape);
|
||||
RigidBody* createRigidBody(const Transform& transform, decimal mass);
|
||||
|
||||
/// Destroy a rigid body and all the joints which it belongs
|
||||
void destroyRigidBody(RigidBody* rigidBody);
|
||||
|
|
@ -245,11 +240,6 @@ class DynamicsWorld : public CollisionWorld {
|
|||
/// Add the joint to the list of joints of the two bodies involved in the joint
|
||||
void addJointToBody(Joint* joint);
|
||||
|
||||
/// Add a contact manifold to the linked list of contact manifolds of the two bodies
|
||||
/// involed in the corresponding contact.
|
||||
void addContactManifoldToBody(ContactManifold* contactManifold,
|
||||
CollisionBody *body1, CollisionBody *body2);
|
||||
|
||||
/// Reset all the contact manifolds linked list of each body
|
||||
void resetContactManifoldListsOfBodies();
|
||||
|
||||
|
|
@ -369,21 +359,11 @@ inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool
|
|||
mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive);
|
||||
}
|
||||
|
||||
// Reset the boolean movement variable of each body
|
||||
inline void DynamicsWorld::resetBodiesMovementVariable() {
|
||||
|
||||
// For each rigid body
|
||||
for (std::set<RigidBody*>::iterator it = getRigidBodiesBeginIterator();
|
||||
it != getRigidBodiesEndIterator(); it++) {
|
||||
|
||||
// Set the hasMoved variable to false
|
||||
(*it)->mHasMoved = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Update the overlapping pair
|
||||
inline void DynamicsWorld::updateOverlappingPair(const BroadPhasePair* pair) {
|
||||
|
||||
// TODO : CHECK WHERE TO CALL THIS METHOD
|
||||
|
||||
// Get the pair of body index
|
||||
std::pair<bodyindex, bodyindex> indexPair = pair->getBodiesIndexPair();
|
||||
|
||||
|
|
|
|||
|
|
@ -28,15 +28,19 @@
|
|||
|
||||
// Libraries
|
||||
#include "ContactManifold.h"
|
||||
#include "../collision/shapes/CollisionShape.h"
|
||||
|
||||
/// ReactPhysics3D namespace
|
||||
namespace reactphysics3d {
|
||||
|
||||
// Type for the overlapping pair ID
|
||||
typedef std::pair<uint, uint> overlappingpairid;
|
||||
|
||||
// Class OverlappingPair
|
||||
/**
|
||||
* This class represents a pair of two bodies that are overlapping
|
||||
* This class represents a pair of two proxy collision shapes that are overlapping
|
||||
* during the broad-phase collision detection. It is created when
|
||||
* the two bodies start to overlap and is destroyed when they do not
|
||||
* the two proxy collision shapes start to overlap and is destroyed when they do not
|
||||
* overlap anymore. This class contains a contact manifold that
|
||||
* store all the contact points between the two bodies.
|
||||
*/
|
||||
|
|
@ -46,11 +50,11 @@ class OverlappingPair {
|
|||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Pointer to the first body of the contact
|
||||
CollisionBody* mBody1;
|
||||
/// Pointer to the first proxy collision shape
|
||||
ProxyShape* mShape1;
|
||||
|
||||
/// Pointer to the second body of the contact
|
||||
CollisionBody* mBody2;
|
||||
/// Pointer to the second proxy collision shape
|
||||
ProxyShape* mShape2;
|
||||
|
||||
/// Persistent contact manifold
|
||||
ContactManifold mContactManifold;
|
||||
|
|
@ -71,17 +75,16 @@ class OverlappingPair {
|
|||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
OverlappingPair(CollisionBody* body1, CollisionBody* body2,
|
||||
MemoryAllocator& memoryAllocator);
|
||||
OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& memoryAllocator);
|
||||
|
||||
/// Destructor
|
||||
~OverlappingPair();
|
||||
|
||||
/// Return the pointer to first body
|
||||
CollisionBody* const getBody1() const;
|
||||
/// Return the pointer to first proxy collision shape
|
||||
ProxyShape* const getShape1() const;
|
||||
|
||||
/// Return the pointer to second body
|
||||
CollisionBody* const getBody2() const;
|
||||
ProxyShape* const getShape2() const;
|
||||
|
||||
/// Add a contact to the contact cache
|
||||
void addContact(ContactPoint* contact);
|
||||
|
|
@ -93,6 +96,7 @@ class OverlappingPair {
|
|||
Vector3 getCachedSeparatingAxis() const;
|
||||
|
||||
/// Set the cached separating axis
|
||||
// TODO : Check that this variable is correctly used allong the collision detection process
|
||||
void setCachedSeparatingAxis(const Vector3& axis);
|
||||
|
||||
/// Return the number of contacts in the cache
|
||||
|
|
@ -101,19 +105,25 @@ class OverlappingPair {
|
|||
/// Return the contact manifold
|
||||
ContactManifold* getContactManifold();
|
||||
|
||||
/// Return the pair of bodies index
|
||||
static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2);
|
||||
|
||||
/// Return the pair of bodies index of the pair
|
||||
static bodyindexpair computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2);
|
||||
|
||||
// -------------------- Friendship -------------------- //
|
||||
|
||||
friend class DynamicsWorld;
|
||||
};
|
||||
|
||||
// Return the pointer to first body
|
||||
inline CollisionBody* const OverlappingPair::getBody1() const {
|
||||
return mBody1;
|
||||
inline ProxyShape* const OverlappingPair::getShape1() const {
|
||||
return mShape1;
|
||||
}
|
||||
|
||||
// Return the pointer to second body
|
||||
inline CollisionBody* const OverlappingPair::getBody2() const {
|
||||
return mBody2;
|
||||
inline ProxyShape* const OverlappingPair::getShape2() const {
|
||||
return mShape2;
|
||||
}
|
||||
|
||||
// Add a contact to the contact manifold
|
||||
|
|
@ -123,7 +133,7 @@ inline void OverlappingPair::addContact(ContactPoint* contact) {
|
|||
|
||||
// Update the contact manifold
|
||||
inline void OverlappingPair::update() {
|
||||
mContactManifold.update(mBody1->getTransform(), mBody2->getTransform());
|
||||
mContactManifold.update(mShape1->getBody()->getTransform(), mShape2->getBody()->getTransform());
|
||||
}
|
||||
|
||||
// Return the cached separating axis
|
||||
|
|
@ -147,6 +157,30 @@ inline ContactManifold* OverlappingPair::getContactManifold() {
|
|||
return &mContactManifold;
|
||||
}
|
||||
|
||||
// Return the pair of bodies index
|
||||
inline overlappingpairid OverlappingPair::computeID(ProxyShape* shape1, ProxyShape* shape2) {
|
||||
assert(shape1->mBroadPhaseID >= 0 && shape2->mBroadPhaseID >= 0);
|
||||
|
||||
// Construct the pair of body index
|
||||
overlappingpairid pairID = shape1->mBroadPhaseID < shape2->mBroadPhaseID ?
|
||||
std::make_pair(shape1->mBroadPhaseID, shape2->mBroadPhaseID) :
|
||||
std::make_pair(shape2->mBroadPhaseID, shape1->mBroadPhaseID);
|
||||
assert(pairID.first != pairID.second);
|
||||
return pairID;
|
||||
}
|
||||
|
||||
// Return the pair of bodies index
|
||||
inline bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body1,
|
||||
CollisionBody* body2) {
|
||||
|
||||
// Construct the pair of body index
|
||||
bodyindexpair indexPair = body1->getID() < body2->getID() ?
|
||||
std::make_pair(body1->getID(), body2->getID()) :
|
||||
std::make_pair(body2->getID(), body1->getID());
|
||||
assert(indexPair.first != indexPair.second);
|
||||
return indexPair;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
125
src/memory/Stack.h
Normal file
125
src/memory/Stack.h
Normal file
|
|
@ -0,0 +1,125 @@
|
|||
/********************************************************************************
|
||||
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
|
||||
* Copyright (c) 2010-2014 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_STACK_H
|
||||
#define REACTPHYSICS3D_STACK_H
|
||||
|
||||
// Libraries
|
||||
#include "../configuration.h"
|
||||
|
||||
namespace reactphysics3d {
|
||||
|
||||
// Class Stack
|
||||
/**
|
||||
* This class represents a simple generic stack with an initial capacity. If the number
|
||||
* of elements exceeds the capacity, the heap will be used to allocated more memory.
|
||||
*/
|
||||
template<typename T, uint capacity>
|
||||
class Stack {
|
||||
|
||||
private:
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Initial array that contains the elements of the stack
|
||||
T mInitArray[capacity];
|
||||
|
||||
/// Pointer to the first element of the stack
|
||||
T* mElements;
|
||||
|
||||
/// Number of elements in the stack
|
||||
uint mNbElements;
|
||||
|
||||
/// Number of allocated elements in the stack
|
||||
uint mNbAllocatedElements;
|
||||
|
||||
public:
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
Stack() : mElements(mInitArray), mNbElements(0), mNbAllocatedElements(capacity) {
|
||||
|
||||
}
|
||||
|
||||
/// Destructor
|
||||
~Stack() {
|
||||
|
||||
// If elements have been allocated on the heap
|
||||
if (mInitArray != mElements) {
|
||||
|
||||
// Release the memory allocated on the heap
|
||||
free(mElements);
|
||||
}
|
||||
}
|
||||
|
||||
/// Push an element into the stack
|
||||
void push(const T& element);
|
||||
|
||||
/// Pop an element from the stack (remove it from the stack and return it)
|
||||
T pop();
|
||||
|
||||
/// Return the number of elments in the stack
|
||||
uint getNbElements() const;
|
||||
|
||||
};
|
||||
|
||||
// Push an element into the stack
|
||||
template<typename T, uint capacity>
|
||||
inline void Stack<T, capacity>::push(const T& element) {
|
||||
|
||||
// If we need to allocate more elements
|
||||
if (mNbElements == mNbAllocatedElements) {
|
||||
T* oldElements = mElements;
|
||||
mNbAllocatedElements *= 2;
|
||||
mElements = (T*) malloc(mNbAllocatedElements * sizeof(T));
|
||||
assert(mElements);
|
||||
memcpy(mElements, oldElements, mNbElements * sizeof(T));
|
||||
if (oldElements != mInitArray) {
|
||||
free(oldElements);
|
||||
}
|
||||
}
|
||||
|
||||
mElements[mNbElements] = element;
|
||||
mNbElements++;
|
||||
}
|
||||
|
||||
// Pop an element from the stack (remove it from the stack and return it)
|
||||
template<typename T, uint capacity>
|
||||
inline T Stack<T, capacity>::pop() {
|
||||
assert(mNbElements > 0);
|
||||
mNbElements--;
|
||||
return mElements[mNbElements];
|
||||
}
|
||||
|
||||
// Return the number of elments in the stack
|
||||
template<typename T, uint capacity>
|
||||
inline uint Stack<T, capacity>::getNbElements() const {
|
||||
return mNbElements;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue
Block a user