From 2b2143d41e1bd5472c220ce5bf3d180902e041a6 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 24 Apr 2013 19:24:28 +0200 Subject: [PATCH] Continue the implementation of the constraint solver --- src/collision/CollisionDetection.cpp | 12 ++- src/collision/CollisionDetection.h | 2 +- src/collision/ContactInfo.cpp | 38 --------- src/collision/ContactInfo.h | 81 ------------------- .../narrowphase/EPA/EPAAlgorithm.cpp | 4 +- src/collision/narrowphase/EPA/EPAAlgorithm.h | 4 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 12 +-- src/collision/narrowphase/GJK/GJKAlgorithm.h | 6 +- .../narrowphase/NarrowPhaseAlgorithm.h | 6 +- .../narrowphase/SphereVsSphereAlgorithm.cpp | 4 +- .../SphereVsSphereAlgorithm.cpp.autosave | 81 +++++++++++++++++++ .../narrowphase/SphereVsSphereAlgorithm.h | 4 +- src/constraint/BallAndSocketJoint.cpp | 5 +- src/constraint/BallAndSocketJoint.h | 36 ++++++++- src/constraint/Constraint.cpp | 8 +- src/constraint/Constraint.h | 39 ++++++++- src/constraint/ContactPoint.cpp | 15 ++-- src/constraint/ContactPoint.h | 56 ++++++++++++- src/engine/CollisionWorld.cpp | 7 +- src/engine/CollisionWorld.h | 2 +- src/engine/ConstraintSolver.cpp | 8 +- src/engine/ConstraintSolver.h | 25 +++++- src/engine/ContactSolver.cpp | 15 ++-- src/engine/ContactSolver.h | 11 +-- src/engine/DynamicsWorld.cpp | 72 +++++++++++++---- src/engine/DynamicsWorld.h | 78 ++++-------------- 26 files changed, 373 insertions(+), 258 deletions(-) delete mode 100644 src/collision/ContactInfo.cpp delete mode 100644 src/collision/ContactInfo.h create mode 100644 src/collision/narrowphase/SphereVsSphereAlgorithm.cpp.autosave diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index e5359860..46773a1c 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -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(body1); + contactInfo->body2 = dynamic_cast(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)); } } } diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 897103f9..2cb0879a 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -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 #include #include diff --git a/src/collision/ContactInfo.cpp b/src/collision/ContactInfo.cpp deleted file mode 100644 index b3d7486a..00000000 --- a/src/collision/ContactInfo.cpp +++ /dev/null @@ -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) { - -} diff --git a/src/collision/ContactInfo.h b/src/collision/ContactInfo.h deleted file mode 100644 index a9d8245a..00000000 --- a/src/collision/ContactInfo.h +++ /dev/null @@ -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 - diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp index d14aff7b..2439979a 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp @@ -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); diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.h b/src/collision/narrowphase/EPA/EPAAlgorithm.h index 5b11a2bf..8f13b8a5 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.h +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.h @@ -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 diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index b03f3953..16e8f671 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -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; diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index 5e387b68..5ce89f2e 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -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); }; } diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index 998583a3..79ac19d9 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -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 diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 2f950e04..1a67782a 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -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(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); diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp.autosave b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp.autosave new file mode 100644 index 00000000..a4165d64 --- /dev/null +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp.autosave @@ -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(collisionShape1); + const SphereShape* sphereShape2 = dynamic_cast(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; +} diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h index e9320619..d47f55aa 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -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); }; } diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 51b017c7..17de606d 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -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){ } diff --git a/src/constraint/BallAndSocketJoint.h b/src/constraint/BallAndSocketJoint.h index 7908dc8f..e7e3f5ea 100644 --- a/src/constraint/BallAndSocketJoint.h +++ b/src/constraint/BallAndSocketJoint.h @@ -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 diff --git a/src/constraint/Constraint.cpp b/src/constraint/Constraint.cpp index a1bd2b76..7ecded94 100644 --- a/src/constraint/Constraint.cpp +++ b/src/constraint/Constraint.cpp @@ -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 diff --git a/src/constraint/Constraint.h b/src/constraint/Constraint.h index 9399d84b..de09c956 100644 --- a/src/constraint/Constraint.h +++ b/src/constraint/Constraint.h @@ -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 diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index df38e269..93a7ec3d 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -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); diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index 8ad4eff9..14852282 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -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 { diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 3a048d09..80e73e87 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -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()); } diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index a120956b..89f38c5c 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -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); diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index a0131bac..5fc41c2a 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -29,7 +29,13 @@ using namespace reactphysics3d; // Constructor -ConstraintSolver::ConstraintSolver() { +ConstraintSolver::ConstraintSolver(std::set& joints, + std::vector& constrainedLinearVelocities, + std::vector& constrainedAngularVelocities, + const std::map& mapBodyToVelocityIndex) + : mJoints(joints), mConstrainedLinearVelocities(constrainedLinearVelocities), + mConstrainedAngularVelocities(constrainedAngularVelocities), + mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex) { } diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 227c4c9a..5dce0686 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -27,6 +27,11 @@ #define REACTPHYSICS3D_CONSTRAINT_SOLVER_H // Libraries +#include "../configuration.h" +#include "mathematics/mathematics.h" +#include "../constraint/Constraint.h" +#include +#include namespace reactphysics3d { @@ -105,6 +110,20 @@ class ConstraintSolver { // -------------------- Attributes -------------------- // + /// Reference to all the joints of the world + std::set& mJoints; + + /// Reference to the array of constrained linear velocities (state of the linear velocities + /// after solving the constraints) + std::vector& mConstrainedLinearVelocities; + + /// Reference to the array of constrained angular velocities (state of the angular velocities + /// after solving the constraints) + std::vector& mConstrainedAngularVelocities; + + /// Reference to the map of rigid body to their index in the constrained velocities array + const std::map& mMapBodyToConstrainedVelocityIndex; + /// Number of iterations of the contact solver uint mNbIterations; @@ -116,11 +135,13 @@ class ConstraintSolver { // -------------------- Methods -------------------- // /// Constructor - ConstraintSolver(); + ConstraintSolver(std::set& joints, + std::vector& constrainedLinearVelocities, + std::vector& constrainedAngularVelocities, + const std::map& mapBodyToVelocityIndex); /// Destructor ~ConstraintSolver(); - }; } diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 7d786588..94d58943 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -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& constrainedLinearVelocities, +ContactSolver::ContactSolver(std::vector& contactManifolds, + std::vector& constrainedLinearVelocities, std::vector& constrainedAngularVelocities, const std::map& 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::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); diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 1153f306..97ec39b5 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -342,8 +342,8 @@ class ContactSolver { // -------------------- Attributes -------------------- // - /// Reference to the world - DynamicsWorld& mWorld; + /// Reference to all the contact manifold of the world + std::vector& mContactManifolds; /// Number of iterations of the contact solver uint mNbIterations; @@ -366,11 +366,11 @@ class ContactSolver { /// Constrained bodies std::set 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& 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& mConstrainedAngularVelocities; @@ -452,7 +452,8 @@ class ContactSolver { // -------------------- Methods -------------------- // /// Constructor - ContactSolver(DynamicsWorld& mWorld, std::vector& constrainedLinearVelocities, + ContactSolver(std::vector& contactManifolds, + std::vector& constrainedLinearVelocities, std::vector& constrainedAngularVelocities, const std::map& mapBodyToVelocityIndex); diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 13d79138..dd319483 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -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( + 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(broadPhasePair->body1); - RigidBody* const rigidBody2 = dynamic_cast(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 diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 93b87bf2..de1edd7c 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -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 mContactManifolds; - /// All the constraints (except contact constraints) - std::vector mConstraints; + /// All the joints of the world + std::set mJoints; + + /// All the bodies that are part of contacts or constraints + std::set 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::iterator getConstraintsBeginIterator(); - - /// Return a end iterator on the constraint list - std::vector::iterator getConstraintsEndIterator(); - - /// Return a start iterator on the contact manifolds list - std::vector::iterator getContactManifoldsBeginIterator(); - - /// Return a end iterator on the contact manifolds list - std::vector::iterator getContactManifoldsEndIterator(); - /// Return an iterator to the beginning of the rigid bodies of the physics world std::set::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::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::iterator DynamicsWorld::getConstraintsBeginIterator() { - return mConstraints.begin(); -} - -// Return a end iterator on the constraint list -inline std::vector::iterator DynamicsWorld::getConstraintsEndIterator() { - return mConstraints.end(); -} - -// Return a start iterator on the contact manifolds list -inline std::vector::iterator DynamicsWorld::getContactManifoldsBeginIterator() { - return mContactManifolds.begin(); -} - -// Return a end iterator on the contact manifolds list -inline std::vector::iterator DynamicsWorld::getContactManifoldsEndIterator() { - return mContactManifolds.end(); -} - } #endif