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

Continue the implementation of the constraint solver

Browse Source
This commit is contained in:
Daniel Chappuis 2013-04-24 19:24:28 +02:00
parent 471e4f7afc
commit 2b2143d41e
26 changed files with 373 additions and 258 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/collision/CollisionDetection.cpp
View File

@ -99,7 +99,7 @@ void CollisionDetection::computeNarrowPhase() {
// For each possible collision pair of bodies
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); it++) {
ContactInfo* contactInfo = NULL;
ContactPointInfo* contactInfo = NULL;
BroadPhasePair* pair = (*it).second;
assert(pair != NULL);
@ -125,12 +125,18 @@ void CollisionDetection::computeNarrowPhase() {
contactInfo)) {
assert(contactInfo != NULL);
// Set the bodies of the contact
contactInfo->body1 = dynamic_cast<RigidBody*>(body1);
contactInfo->body2 = dynamic_cast<RigidBody*>(body2);
assert(contactInfo->body1 != NULL);
assert(contactInfo->body2 != NULL);
// Notify the world about the new narrow-phase contact
mWorld->notifyNewContact(pair, contactInfo);
// Delete and remove the contact info from the memory allocator
contactInfo->ContactInfo::~ContactInfo();
mMemoryAllocator.release(contactInfo, sizeof(ContactInfo));
contactInfo->ContactPointInfo::~ContactPointInfo();
mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
}
}
}

2
src/collision/CollisionDetection.h
View File

@ -33,7 +33,7 @@
#include "narrowphase/GJK/GJKAlgorithm.h"
#include "narrowphase/SphereVsSphereAlgorithm.h"
#include "../memory/MemoryAllocator.h"
#include "ContactInfo.h"
#include "../constraint/ContactPoint.h"
#include <vector>
#include <map>
#include <set>

38
src/collision/ContactInfo.cpp
View File

@ -1,38 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "ContactInfo.h"
using namespace reactphysics3d;
// Constructor
ContactInfo::ContactInfo(const Vector3& normal, decimal penetrationDepth,
const Vector3& localPoint1, const Vector3& localPoint2)
: normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1),
localPoint2(localPoint2) {
}

81
src/collision/ContactInfo.h
View File

@ -1,81 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_CONTACT_INFO_H
#define REACTPHYSICS3D_CONTACT_INFO_H
// Libraries
#include "../collision/shapes/BoxShape.h"
#include "../mathematics/mathematics.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
// Structure ContactInfo
/**
* This structure contains informations about a collision contact
* computed during the narrow-phase collision detection. Those
* informations are used to compute the contact set for a contact
* between two bodies.
*/
struct ContactInfo {
private:
// -------------------- Methods -------------------- //
/// Private copy-constructor
ContactInfo(const ContactInfo& contactInfo);
/// Private assignment operator
ContactInfo& operator=(const ContactInfo& contactInfo);
public:
// -------------------- Attributes -------------------- //
/// Normal vector the the collision contact in world space
const Vector3 normal;
/// Penetration depth of the contact
const decimal penetrationDepth;
/// Contact point of body 1 in local space of body 1
const Vector3 localPoint1;
/// Contact point of body 2 in local space of body 2
const Vector3 localPoint2;
// -------------------- Methods -------------------- //
/// Constructor
ContactInfo(const Vector3& normal, decimal penetrationDepth,
const Vector3& localPoint1, const Vector3& localPoint2);
};
}
#endif

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

@ -87,7 +87,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
Vector3& v, ContactInfo*& contactInfo) {
Vector3& v, ContactPointInfo*& contactInfo) {
Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
Vector3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
@ -400,7 +400,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
assert(penetrationDepth > 0.0);
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal,
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
penetrationDepth,
pALocal, pBLocal);

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

@ -29,7 +29,7 @@
// Libraries
#include "../GJK/Simplex.h"
#include "../../shapes/CollisionShape.h"
#include "../../ContactInfo.h"
#include "../../../constraint/ContactPoint.h"
#include "../../../mathematics/mathematics.h"
#include "TriangleEPA.h"
#include "../../../memory/MemoryAllocator.h"
@ -123,7 +123,7 @@ class EPAAlgorithm {
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
Vector3& v, ContactInfo*& contactInfo);
Vector3& v, ContactPointInfo*& contactInfo);
};
// Add a triangle face in the candidate triangle heap in the EPA algorithm

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

@ -61,7 +61,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
ContactInfo*& contactInfo) {
ContactPointInfo*& contactInfo) {
Vector3 suppA; // Support point of object A
Vector3 suppB; // Support point of object B
@ -137,7 +137,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
if (penetrationDepth <= 0.0) return false;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal,
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
penetrationDepth,
pA, pB);
@ -169,7 +169,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
if (penetrationDepth <= 0.0) return false;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal,
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
penetrationDepth,
pA, pB);
@ -199,7 +199,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
if (penetrationDepth <= 0.0) return false;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal,
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
penetrationDepth,
pA, pB);
@ -236,7 +236,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
if (penetrationDepth <= 0.0) return false;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal,
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
penetrationDepth,
pA, pB);
@ -263,7 +263,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
ContactInfo*& contactInfo,
ContactPointInfo*& contactInfo,
Vector3& v) {
Simplex simplex;
Vector3 suppA;

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

@ -28,7 +28,7 @@
// Libraries
#include "../NarrowPhaseAlgorithm.h"
#include "../../ContactInfo.h"
#include "../../../constraint/ContactPoint.h"
#include "../../../collision/shapes/CollisionShape.h"
#include "../EPA/EPAAlgorithm.h"
@ -78,7 +78,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
ContactInfo*& contactInfo, Vector3& v);
ContactPointInfo*& contactInfo, Vector3& v);
public :
@ -95,7 +95,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
ContactInfo*& contactInfo);
ContactPointInfo*& contactInfo);
};
}

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

@ -28,7 +28,7 @@
// Libraries
#include "../../body/Body.h"
#include "../ContactInfo.h"
#include "../../constraint/ContactPoint.h"
#include "../broadphase/PairManager.h"
#include "../../memory/MemoryAllocator.h"
#include "../BroadPhasePair.h"
@ -36,7 +36,7 @@
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Class NarrowPhaseAlgorithm
/**
* This class is an abstract class that represents an algorithm
@ -82,7 +82,7 @@ class NarrowPhaseAlgorithm {
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
ContactInfo*& contactInfo)=0;
ContactPointInfo*& contactInfo)=0;
};
// Set the current overlapping pair of bodies

4
src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
View File

@ -45,7 +45,7 @@ bool SphereVsSphereAlgorithm::testCollision(const CollisionShape* collisionShape
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
ContactInfo*& contactInfo) {
ContactPointInfo*& contactInfo) {
// Get the sphere collision shapes
const SphereShape* sphereShape1 = dynamic_cast<const SphereShape*>(collisionShape1);
@ -69,7 +69,7 @@ bool SphereVsSphereAlgorithm::testCollision(const CollisionShape* collisionShape
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(
vectorBetweenCenters.getUnit(), penetrationDepth,
intersectionOnBody1, intersectionOnBody2);

81
src/collision/narrowphase/SphereVsSphereAlgorithm.cpp.autosave Normal file
View File

@ -0,0 +1,81 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "SphereVsSphereAlgorithm.h"
#include "../../collision/shapes/SphereShape.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
SphereVsSphereAlgorithm::SphereVsSphereAlgorithm(MemoryAllocator& memoryAllocator)
:NarrowPhaseAlgorithm(memoryAllocator) {
}
// Destructor
SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() {
}
bool SphereVsSphereAlgorithm::testCollision(const CollisionShape* collisionShape1,
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
ContactPointInfo*& contactInfo) {
// Get the sphere collision shapes
const SphereShape* sphereShape1 = dynamic_cast<const SphereShape*>(collisionShape1);
const SphereShape* sphereShape2 = dynamic_cast<const SphereShape*>(collisionShape2);
// Compute the distance between the centers
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
// Compute the sum of the radius
decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
// If the sphere collision shapes intersect
if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
Vector3 centerSphere2InBody1LocalSpace = transform1.inverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.inverse() * transform1.getPosition();
Vector3 intersectionOnBody1 = sphereShape1->getRadius() *
centerSphere2InBody1LocalSpace.getUnit();
Vector3 intersectionOnBody2 = sphereShape2->getRadius() *
centerSphere1InBody2LocalSpace.getUnit();
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(
vectorBetweenCenters.getUnit(), penetrationDepth,
intersectionOnBody1, intersectionOnBody2);
return true;
}
return false;
}

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

@ -28,7 +28,7 @@
// Libraries
#include "../../body/Body.h"
#include "../ContactInfo.h"
#include "../../constraint/ContactPoint.h"
#include "NarrowPhaseAlgorithm.h"
@ -67,7 +67,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
ContactInfo*& contactInfo);
ContactPointInfo*& contactInfo);
};
}

5
src/constraint/BallAndSocketJoint.cpp
View File

@ -29,9 +29,8 @@
using namespace reactphysics3d;
// Constructor
BallAndSocketJoint::BallAndSocketJoint(RigidBody* const body1, RigidBody* const body2,
bool active, ConstraintType type)
: Constraint(body1, body2, active, type){
BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo &jointInfo)
: Constraint(jointInfo){
}

36
src/constraint/BallAndSocketJoint.h
View File

@ -28,9 +28,28 @@
// Libraries
#include "Constraint.h"
#include "../mathematics/mathematics.h"
namespace reactphysics3d {
// Structure BallAndSocketJointInfo
/**
* This structure is used to gather the information needed to create a ball-and-socket
* joint. This structure will be used to create the actual ball-and-socket joint.
*/
struct BallAndSocketJointInfo : public ConstraintInfo {
public :
// -------------------- Attributes -------------------- //
/// Anchor point (in world space coordinates)
Vector3 anchorPoint;
/// Constructor
BallAndSocketJointInfo() : ConstraintInfo(BALLSOCKETJOINT) {}
};
// Class BallAndSocketJoint
/**
* This class represents a ball-and-socket joint that allows arbitrary rotation
@ -42,18 +61,31 @@ class BallAndSocketJoint : public Constraint {
// -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local space coordinates)
Vector3 mLocalAnchorPoint1;
/// Anchor point of body 2 (in local space coordinates)
Vector3 mLocalAnchorPoint2;
public :
// -------------------- Methods -------------------- //
/// Constructor
BallAndSocketJoint(RigidBody* const body1, RigidBody* const body2,
bool active, ConstraintType type);
BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo);
/// Destructor
virtual ~BallAndSocketJoint();
/// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const;
};
// Return the number of bytes used by the joint
inline size_t BallAndSocketJoint::getSizeInBytes() const {
return sizeof(BallAndSocketJoint);
}
}
#endif

8
src/constraint/Constraint.cpp
View File

@ -29,10 +29,12 @@
using namespace reactphysics3d;
// Constructor
Constraint::Constraint(RigidBody* const body1, RigidBody* const body2,
bool active, ConstraintType type)
:mBody1(body1), mBody2(body2), mActive(active), mType(type) {
Constraint::Constraint(const ConstraintInfo& constraintInfo)
:mBody1(constraintInfo.body1), mBody2(constraintInfo.body2), mActive(true),
mType(constraintInfo.type) {
assert(mBody1 != NULL);
assert(mBody2 != NULL);
}
// Destructor

39
src/constraint/Constraint.h
View File

@ -36,6 +36,39 @@ namespace reactphysics3d {
// Enumeration for the type of a constraint
enum ConstraintType {CONTACT, BALLSOCKETJOINT};
// Structure ConstraintInfo
/**
* This structure is used to gather the information needed to create a constraint.
*/
struct ConstraintInfo {
public :
// -------------------- Attributes -------------------- //
/// First rigid body of the constraint
RigidBody* body1;
/// Second rigid body of the constraint
RigidBody* body2;
/// Type of the constraint
ConstraintType type;
/// Constructor
ConstraintInfo(ConstraintType constraintType)
: body1(NULL), body2(NULL), type(constraintType) {}
/// Constructor
ConstraintInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, ConstraintType constraintType)
: body1(rigidBody1), body2(rigidBody2), type(constraintType) {
}
/// Destructor
virtual ~ConstraintInfo() {}
};
// Class Constraint
/**
* This abstract class represents a constraint in the physics engine.
@ -74,8 +107,7 @@ class Constraint {
// -------------------- Methods -------------------- //
/// Constructor
Constraint(RigidBody* const body1, RigidBody* const body2,
bool active, ConstraintType type);
Constraint(const ConstraintInfo& constraintInfo);
/// Destructor
virtual ~Constraint();
@ -91,6 +123,9 @@ class Constraint {
/// Return the type of the constraint
ConstraintType getType() const;
/// Return the number of bytes used by the constraint
virtual size_t getSizeInBytes() const = 0;
};
// Return the reference to the body 1

15
src/constraint/ContactPoint.cpp
View File

@ -30,14 +30,13 @@ using namespace reactphysics3d;
using namespace std;
// Constructor
ContactPoint::ContactPoint(RigidBody* const body1, RigidBody* const body2,
const ContactInfo* contactInfo)
: Constraint(body1, body2, true, CONTACT), mNormal(contactInfo->normal),
mPenetrationDepth(contactInfo->penetrationDepth),
mLocalPointOnBody1(contactInfo->localPoint1),
mLocalPointOnBody2(contactInfo->localPoint2),
mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1),
mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2),
ContactPoint::ContactPoint(const ContactPointInfo& contactInfo)
: Constraint(contactInfo), mNormal(contactInfo.normal),
mPenetrationDepth(contactInfo.penetrationDepth),
mLocalPointOnBody1(contactInfo.localPoint1),
mLocalPointOnBody2(contactInfo.localPoint2),
mWorldPointOnBody1(contactInfo.body1->getTransform() * contactInfo.localPoint1),
mWorldPointOnBody2(contactInfo.body2->getTransform() * contactInfo.localPoint2),
mIsRestingContact(false) {
mFrictionVectors[0] = Vector3(0, 0, 0);

56
src/constraint/ContactPoint.h
View File

@ -28,7 +28,6 @@
// Libraries
#include "Constraint.h"
#include "../collision/ContactInfo.h"
#include "../body/RigidBody.h"
#include "../configuration.h"
#include "../mathematics/mathematics.h"
@ -50,6 +49,52 @@
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Structure ContactPointInfo
/**
* This structure contains informations about a collision contact
* computed during the narrow-phase collision detection. Those
* informations are used to compute the contact set for a contact
* between two bodies.
*/
struct ContactPointInfo : public ConstraintInfo {
private:
// -------------------- Methods -------------------- //
/// Private copy-constructor
ContactPointInfo(const ContactPointInfo& contactInfo);
/// Private assignment operator
ContactPointInfo& operator=(const ContactPointInfo& contactInfo);
public:
// -------------------- Attributes -------------------- //
/// Normal vector the the collision contact in world space
const Vector3 normal;
/// Penetration depth of the contact
const decimal penetrationDepth;
/// Contact point of body 1 in local space of body 1
const Vector3 localPoint1;
/// Contact point of body 2 in local space of body 2
const Vector3 localPoint2;
// -------------------- Methods -------------------- //
/// Constructor
ContactPointInfo(const Vector3& normal, decimal penetrationDepth,
const Vector3& localPoint1, const Vector3& localPoint2)
: ConstraintInfo(CONTACT), normal(normal), penetrationDepth(penetrationDepth),
localPoint1(localPoint1), localPoint2(localPoint2) {
}
};
// Class ContactPoint
/**
* This class represents a collision contact point between two
@ -108,7 +153,7 @@ class ContactPoint : public Constraint {
// -------------------- Methods -------------------- //
/// Constructor
ContactPoint(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo);
ContactPoint(const ContactPointInfo& contactInfo);
/// Destructor
virtual ~ContactPoint();
@ -176,6 +221,9 @@ class ContactPoint : public Constraint {
/// Return the penetration depth
decimal getPenetrationDepth() const;
/// Return the number of bytes used by the contact point
virtual size_t getSizeInBytes() const;
#ifdef VISUAL_DEBUG
/// Draw the contact (for debugging)
void draw() const;
@ -287,6 +335,10 @@ inline decimal ContactPoint::getPenetrationDepth() const {
return mPenetrationDepth;
}
// Return the number of bytes used by the contact point
inline size_t ContactPoint::getSizeInBytes() const {
return sizeof(ContactPoint);
}
#ifdef VISUAL_DEBUG
inline void ContactPoint::draw() const {

7
src/engine/CollisionWorld.cpp
View File

@ -56,7 +56,7 @@ void CollisionWorld::notifyRemovedOverlappingPair(const BroadPhasePair* removedP
// Notify the world about a new narrow-phase contact
void CollisionWorld::notifyNewContact(const BroadPhasePair* broadPhasePair,
const ContactInfo* contactInfo) {
const ContactPointInfo* contactInfo) {
// TODO : Implement this method
}
@ -101,7 +101,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
// Add the body ID to the list of free IDs
mFreeBodiesIDs.push_back(collisionBody->getID());
// Call the constructor of the collision body
// Call the destructor of the collision body
collisionBody->CollisionBody::~CollisionBody();
// Remove the collision body from the list of bodies
@ -181,6 +181,9 @@ void CollisionWorld::removeCollisionShape(CollisionShape* collisionShape) {
// Remove the shape from the set of shapes in the world
mCollisionShapes.remove(collisionShape);
// Call the destructor of the collision shape
collisionShape->CollisionShape::~CollisionShape();
// Deallocate the memory used by the collision shape
mMemoryAllocator.release(collisionShape, collisionShape->getSizeInBytes());
}

2
src/engine/CollisionWorld.h
View File

@ -91,7 +91,7 @@ class CollisionWorld {
virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair);
/// Notify the world about a new narrow-phase contact
virtual void notifyNewContact(const BroadPhasePair* pair, const ContactInfo* contactInfo);
virtual void notifyNewContact(const BroadPhasePair* pair, const ContactPointInfo* contactInfo);
/// Update the overlapping pair
virtual void updateOverlappingPair(const BroadPhasePair* pair);

8
src/engine/ConstraintSolver.cpp
View File

@ -29,7 +29,13 @@
using namespace reactphysics3d;
// Constructor
ConstraintSolver::ConstraintSolver() {
ConstraintSolver::ConstraintSolver(std::set<Constraint*>& joints,
std::vector<Vector3>& constrainedLinearVelocities,
std::vector<Vector3>& constrainedAngularVelocities,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
: mJoints(joints), mConstrainedLinearVelocities(constrainedLinearVelocities),
mConstrainedAngularVelocities(constrainedAngularVelocities),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex) {
}

25
src/engine/ConstraintSolver.h
View File

@ -27,6 +27,11 @@
#define REACTPHYSICS3D_CONSTRAINT_SOLVER_H
// Libraries
#include "../configuration.h"
#include "mathematics/mathematics.h"
#include "../constraint/Constraint.h"
#include <map>
#include <set>
namespace reactphysics3d {
@ -105,6 +110,20 @@ class ConstraintSolver {
// -------------------- Attributes -------------------- //
/// Reference to all the joints of the world
std::set<Constraint*>& mJoints;
/// Reference to the array of constrained linear velocities (state of the linear velocities
/// after solving the constraints)
std::vector<Vector3>& mConstrainedLinearVelocities;
/// Reference to the array of constrained angular velocities (state of the angular velocities
/// after solving the constraints)
std::vector<Vector3>& mConstrainedAngularVelocities;
/// Reference to the map of rigid body to their index in the constrained velocities array
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
/// Number of iterations of the contact solver
uint mNbIterations;
@ -116,11 +135,13 @@ class ConstraintSolver {
// -------------------- Methods -------------------- //
/// Constructor
ConstraintSolver();
ConstraintSolver(std::set<Constraint*>& joints,
std::vector<Vector3>& constrainedLinearVelocities,
std::vector<Vector3>& constrainedAngularVelocities,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
/// Destructor
~ConstraintSolver();
};
}

15
src/engine/ContactSolver.cpp
View File

@ -39,10 +39,12 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolver::SLOP = decimal(0.01);
// Constructor
ContactSolver::ContactSolver(DynamicsWorld& world,std::vector<Vector3>& constrainedLinearVelocities,
ContactSolver::ContactSolver(std::vector<ContactManifold*>& contactManifolds,
std::vector<Vector3>& constrainedLinearVelocities,
std::vector<Vector3>& constrainedAngularVelocities,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
:mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS),
:mContactManifolds(contactManifolds),
mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS),
mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
mContactConstraints(NULL),
mConstrainedLinearVelocities(constrainedLinearVelocities),
@ -62,14 +64,13 @@ ContactSolver::~ContactSolver() {
void ContactSolver::initialize() {
// TODO : Use better memory allocation here
mContactConstraints = new ContactManifoldSolver[mWorld.getNbContactManifolds()];
mContactConstraints = new ContactManifoldSolver[mContactManifolds.size()];
mNbContactManifolds = 0;
// For each contact manifold of the world
vector<ContactManifold*>::iterator it;
for (it = mWorld.getContactManifoldsBeginIterator();
it != mWorld.getContactManifoldsEndIterator(); ++it) {
for (it = mContactManifolds.begin(); it != mContactManifolds.end(); ++it) {
ContactManifold* externalManifold = *it;
@ -174,8 +175,8 @@ void ContactSolver::initialize() {
// Allocated memory for split impulse velocities
// TODO : Use better memory allocation here
mSplitLinearVelocities = new Vector3[mWorld.getNbRigidBodies()];
mSplitAngularVelocities = new Vector3[mWorld.getNbRigidBodies()];
mSplitLinearVelocities = new Vector3[mMapBodyToConstrainedVelocityIndex.size()];
mSplitAngularVelocities = new Vector3[mMapBodyToConstrainedVelocityIndex.size()];
assert(mSplitLinearVelocities != NULL);
assert(mSplitAngularVelocities != NULL);

11
src/engine/ContactSolver.h
View File

@ -342,8 +342,8 @@ class ContactSolver {
// -------------------- Attributes -------------------- //
/// Reference to the world
DynamicsWorld& mWorld;
/// Reference to all the contact manifold of the world
std::vector<ContactManifold*>& mContactManifolds;
/// Number of iterations of the contact solver
uint mNbIterations;
@ -366,11 +366,11 @@ class ContactSolver {
/// Constrained bodies
std::set<RigidBody*> mConstraintBodies;
/// Pointer to the array of constrained linear velocities (state of the linear velocities
/// Reference to the array of constrained linear velocities (state of the linear velocities
/// after solving the constraints)
std::vector<Vector3>& mConstrainedLinearVelocities;
/// Pointer to the array of constrained angular velocities (state of the angular velocities
/// Reference to the array of constrained angular velocities (state of the angular velocities
/// after solving the constraints)
std::vector<Vector3>& mConstrainedAngularVelocities;
@ -452,7 +452,8 @@ class ContactSolver {
// -------------------- Methods -------------------- //
/// Constructor
ContactSolver(DynamicsWorld& mWorld, std::vector<Vector3>& constrainedLinearVelocities,
ContactSolver(std::vector<ContactManifold*>& contactManifolds,
std::vector<Vector3>& constrainedLinearVelocities,
std::vector<Vector3>& constrainedAngularVelocities,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);

72
src/engine/DynamicsWorld.cpp
View File

@ -25,6 +25,7 @@
// Libraries
#include "DynamicsWorld.h"
#include "constraint/BallAndSocketJoint.h"
// Namespaces
using namespace reactphysics3d;
@ -33,8 +34,10 @@ using namespace std;
// Constructor
DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
: CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true),
mContactSolver(*this, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
mContactSolver(mContactManifolds, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
mMapBodyToConstrainedVelocityIndex),
mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
mMapBodyToConstrainedVelocityIndex),
mIsDeactivationActive(DEACTIVATION_ENABLED) {
}
@ -221,6 +224,8 @@ void DynamicsWorld::initConstrainedVelocitiesArray() {
i++;
}
assert(mMapBodyToConstrainedVelocityIndex.size() == mRigidBodies.size());
}
// Cleanup the constrained velocities array at each step
@ -230,6 +235,9 @@ void DynamicsWorld::cleanupConstrainedVelocitiesArray() {
mConstrainedLinearVelocities.clear();
mConstrainedAngularVelocities.clear();
// Clear the constrained bodies
mConstrainedBodies.clear();
// Clear the rigid body to velocities array index mapping
mMapBodyToConstrainedVelocityIndex.clear();
}
@ -300,7 +308,7 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
// Remove the collision shape from the world
removeCollisionShape(rigidBody->getCollisionShape());
// Call the constructor of the rigid body
// Call the destructor of the rigid body
rigidBody->RigidBody::~RigidBody();
// Remove the rigid body from the list of rigid bodies
@ -311,9 +319,51 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
mMemoryAllocator.release(rigidBody, sizeof(RigidBody));
}
// Remove all constraints in the physics world
void DynamicsWorld::removeAllConstraints() {
mConstraints.clear();
// Create a joint between two bodies in the world and return a pointer to the new joint
Constraint* DynamicsWorld::createJoint(const ConstraintInfo& jointInfo) {
Constraint* newJoint = NULL;
// Allocate memory to create the new joint
switch(jointInfo.type) {
// Ball-and-Socket joint
case BALLSOCKETJOINT:
{
void* allocatedMemory = mMemoryAllocator.allocate(sizeof(BallAndSocketJoint));
const BallAndSocketJointInfo& info = dynamic_cast<const BallAndSocketJointInfo&>(
jointInfo);
newJoint = new (allocatedMemory) BallAndSocketJoint(info);
break;
}
default:
{
assert(false);
return NULL;
}
}
// Add the joint into the world
mJoints.insert(newJoint);
// Return the pointer to the created joint
return newJoint;
}
// Destroy a joint
void DynamicsWorld::destroyJoint(Constraint* joint) {
assert(joint != NULL);
// Remove the joint from the world
mJoints.erase(joint);
// Call the destructor of the joint
joint->Constraint::~Constraint();
// Release the allocated memory
mMemoryAllocator.release(joint, joint->getSizeInBytes());
}
// Notify the world about a new broad-phase overlapping pair
@ -345,19 +395,11 @@ void DynamicsWorld::notifyRemovedOverlappingPair(const BroadPhasePair* removedPa
// Notify the world about a new narrow-phase contact
void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair,
const ContactInfo* contactInfo) {
RigidBody* const rigidBody1 = dynamic_cast<RigidBody* const>(broadPhasePair->body1);
RigidBody* const rigidBody2 = dynamic_cast<RigidBody* const>(broadPhasePair->body2);
assert(rigidBody1 != NULL);
assert(rigidBody2 != NULL);
const ContactPointInfo* contactInfo) {
// Create a new contact
ContactPoint* contact = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(
rigidBody1,
rigidBody2,
contactInfo);
*contactInfo);
assert(contact != NULL);
// Get the corresponding overlapping pair

78
src/engine/DynamicsWorld.h
View File

@ -30,6 +30,7 @@
#include "CollisionWorld.h"
#include "../collision/CollisionDetection.h"
#include "ContactSolver.h"
#include "ConstraintSolver.h"
#include "../body/RigidBody.h"
#include "Timer.h"
#include "../configuration.h"
@ -55,6 +56,9 @@ class DynamicsWorld : public CollisionWorld {
/// Contact solver
ContactSolver mContactSolver;
/// Constraint solver
ConstraintSolver mConstraintSolver;
/// True if the deactivation (sleeping) of inactive bodies is enabled
bool mIsDeactivationActive;
@ -64,8 +68,11 @@ class DynamicsWorld : public CollisionWorld {
/// All the contact constraints
std::vector<ContactManifold*> mContactManifolds;
/// All the constraints (except contact constraints)
std::vector<Constraint*> mConstraints;
/// All the joints of the world
std::set<Constraint*> mJoints;
/// All the bodies that are part of contacts or constraints
std::set<RigidBody*> mConstrainedBodies;
/// Gravity vector of the world
Vector3 mGravity;
@ -124,7 +131,7 @@ class DynamicsWorld : public CollisionWorld {
virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair);
/// Notify the world about a new narrow-phase contact
virtual void notifyNewContact(const BroadPhasePair* pair, const ContactInfo* contactInfo);
virtual void notifyNewContact(const BroadPhasePair* pair, const ContactPointInfo* contactInfo);
public :
@ -166,6 +173,12 @@ public :
/// Destroy a rigid body
void destroyRigidBody(RigidBody* rigidBody);
/// Create a joint between two bodies in the world and return a pointer to the new joint
Constraint* createJoint(const ConstraintInfo& jointInfo);
/// Destroy a joint
void destroyJoint(Constraint* joint);
/// Return the gravity vector of the world
Vector3 getGravity() const;
@ -178,30 +191,9 @@ public :
/// Return the number of rigid bodies in the world
uint getNbRigidBodies() const;
/// Add a constraint
void addConstraint(Constraint* constraint);
/// Remove a constraint
void removeConstraint(Constraint* constraint);
/// Remove all constraints and delete them (free their memory)
void removeAllConstraints();
/// Return the number of contact constraints in the world
uint getNbContactManifolds() const;
/// Return a start iterator on the constraint list
std::vector<Constraint*>::iterator getConstraintsBeginIterator();
/// Return a end iterator on the constraint list
std::vector<Constraint*>::iterator getConstraintsEndIterator();
/// Return a start iterator on the contact manifolds list
std::vector<ContactManifold*>::iterator getContactManifoldsBeginIterator();
/// Return a end iterator on the contact manifolds list
std::vector<ContactManifold*>::iterator getContactManifoldsEndIterator();
/// Return an iterator to the beginning of the rigid bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
@ -259,24 +251,6 @@ inline void DynamicsWorld::updateOverlappingPair(const BroadPhasePair* pair) {
overlappingPair->update();
}
// Add a constraint into the physics world
inline void DynamicsWorld::addConstraint(Constraint* constraint) {
assert(constraint != 0);
mConstraints.push_back(constraint);
}
// Remove a constraint and free its memory
inline void DynamicsWorld::removeConstraint(Constraint* constraint) {
std::vector<Constraint*>::iterator it;
assert(constraint != NULL);
it = std::find(mConstraints.begin(), mConstraints.end(), constraint);
assert(*it == constraint);
delete *it;
mConstraints.erase(it);
}
// Return the gravity vector of the world
inline Vector3 DynamicsWorld::getGravity() const {
return mGravity;
@ -312,26 +286,6 @@ inline uint DynamicsWorld::getNbContactManifolds() const {
return mContactManifolds.size();
}
// Return a start iterator on the constraint list
inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsBeginIterator() {
return mConstraints.begin();
}
// Return a end iterator on the constraint list
inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsEndIterator() {
return mConstraints.end();
}
// Return a start iterator on the contact manifolds list
inline std::vector<ContactManifold*>::iterator DynamicsWorld::getContactManifoldsBeginIterator() {
return mContactManifolds.begin();
}
// Return a end iterator on the contact manifolds list
inline std::vector<ContactManifold*>::iterator DynamicsWorld::getContactManifoldsEndIterator() {
return mContactManifolds.end();
}
}
#endif