From 9333d0e690077063e96c44ef5d0f1c2ab393f17f Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Wed, 25 Jan 2012 22:57:27 +0000 Subject: [PATCH] Use a customize algorithm for Sphere vs Sphere collision detection instead of using GJK git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@463 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- src/body/Body.cpp | 2 +- src/body/Body.h | 23 ++++-- src/colliders/BoxCollider.cpp | 2 +- src/colliders/Collider.cpp | 2 +- src/colliders/Collider.h | 17 ++++- src/colliders/ConeCollider..cpp | 2 +- src/colliders/CylinderCollider.cpp | 3 +- src/colliders/SphereCollider.cpp | 2 +- src/collision/CollisionDetection.cpp | 15 ++-- src/collision/CollisionDetection.h | 52 ++++++++----- src/collision/ContactInfo.h | 6 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 2 + .../narrowphase/NarrowPhaseAlgorithm.h | 4 +- .../narrowphase/SphereVsSphereAlgorithm.cpp | 74 +++++++++++++++++++ .../narrowphase/SphereVsSphereAlgorithm.h | 59 +++++++++++++++ src/configuration.h | 3 +- src/constraint/Contact.h | 2 +- src/engine/ConstraintSolver.h | 1 - src/engine/PhysicsEngine.cpp | 3 +- src/engine/PhysicsEngine.h | 12 +-- src/mathematics/Transform.h | 4 +- 21 files changed, 233 insertions(+), 57 deletions(-) create mode 100644 src/collision/narrowphase/SphereVsSphereAlgorithm.cpp create mode 100644 src/collision/narrowphase/SphereVsSphereAlgorithm.h diff --git a/src/body/Body.cpp b/src/body/Body.cpp index db642792..9abae89a 100644 --- a/src/body/Body.cpp +++ b/src/body/Body.cpp @@ -32,7 +32,7 @@ using namespace reactphysics3d; // Constructor Body::Body(const Transform& transform, Collider* collider, decimal mass, long unsigned int id) - : collider(collider), mass(mass), transform(transform), id(id) { + : collider(collider), mass(mass), transform(transform), isActive(true), id(id) { assert(mass > 0.0); assert(collider); diff --git a/src/body/Body.h b/src/body/Body.h index cd295ffc..c6f67dd0 100644 --- a/src/body/Body.h +++ b/src/body/Body.h @@ -51,6 +51,7 @@ class Body { Transform transform; // Position and orientation of the body Transform oldTransform; // Last position and orientation of the body decimal interpolationFactor; // Interpolation factor used for the state interpolation + bool isActive; // True if the body is active (not sleeping because of deactivation) bool isMotionEnabled; // True if the body is able to move bool isCollisionEnabled; // True if the body can collide with others bodies AABB* aabb; // Axis-Aligned Bounding Box for Broad-Phase collision detection @@ -65,6 +66,8 @@ class Body { void setCollider(Collider* collider); // Set the collision collider decimal getMass() const; // Return the mass of the body void setMass(decimal mass); // Set the mass of the body + bool getIsActive() const; // Return true if the body is active + void setIsActive(bool isActive); // Set the isActive variable const Transform& getTransform() const; // Return the current position and orientation void setTransform(const Transform& transform); // Set the current position and orientation const AABB* getAABB() const; // Return the AAABB of the body @@ -100,6 +103,21 @@ inline decimal Body::getMass() const { return mass; }; +// Method that set the mass of the body +inline void Body::setMass(decimal mass) { + this->mass = mass; +} + +// Return true if the body is active +inline bool Body::getIsActive() const { + return isActive; +} + +// Set the isActive variable +inline void Body::setIsActive(bool isActive) { + this->isActive = isActive; +} + // Return the interpolated transform for rendering inline Transform Body::getInterpolatedTransform() const { return Transform::interpolateTransforms(oldTransform, transform, interpolationFactor); @@ -121,11 +139,6 @@ inline void Body::setIsMotionEnabled(bool isMotionEnabled) { this->isMotionEnabled = isMotionEnabled; } -// Method that set the mass of the body -inline void Body::setMass(decimal mass) { - this->mass = mass; -} - // Return the current position and orientation inline const Transform& Body::getTransform() const { return transform; diff --git a/src/colliders/BoxCollider.cpp b/src/colliders/BoxCollider.cpp index 7e355500..3c01639f 100644 --- a/src/colliders/BoxCollider.cpp +++ b/src/colliders/BoxCollider.cpp @@ -46,7 +46,7 @@ using namespace reactphysics3d; using namespace std; // Constructor -BoxCollider::BoxCollider(const Vector3& extent) : extent(extent) { +BoxCollider::BoxCollider(const Vector3& extent) : Collider(BOX), extent(extent) { } diff --git a/src/colliders/Collider.cpp b/src/colliders/Collider.cpp index a6450553..6ecab1a2 100644 --- a/src/colliders/Collider.cpp +++ b/src/colliders/Collider.cpp @@ -30,7 +30,7 @@ using namespace reactphysics3d; // Constructor -Collider::Collider() { +Collider::Collider(ColliderType type) : type(type) { } diff --git a/src/colliders/Collider.h b/src/colliders/Collider.h index 1dafa038..35a48578 100644 --- a/src/colliders/Collider.h +++ b/src/colliders/Collider.h @@ -33,6 +33,9 @@ // ReactPhysics3D namespace namespace reactphysics3d { + +// Enumeration +enum ColliderType {BOX, SPHERE, CONE, CYLINDER}; // Type of the collider // Declarations class Body; @@ -44,15 +47,18 @@ class Body; ------------------------------------------------------------------- */ class Collider { + protected : Body* bodyPointer; // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable) - + ColliderType type; // Type of the collider + public : - Collider(); // Constructor - virtual ~Collider(); // Destructor + Collider(ColliderType type); // Constructor + virtual ~Collider(); // Destructor Body* getBodyPointer() const; // Return the body pointer void setBodyPointer(Body* bodyPointer); // Set the body pointer + ColliderType getType() const; // Return the type of the collider virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const=0; // Return a local support point in a given direction virtual Vector3 getLocalExtents(decimal margin=0.0) const=0; // Return the local extents in x,y and z direction virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0; // Return the local inertia tensor of the collider @@ -69,6 +75,11 @@ inline void Collider::setBodyPointer(Body* bodyPointer) { this->bodyPointer = bodyPointer; } +// Return the type of the collider +inline ColliderType Collider::getType() const { + return type; +} + } #endif \ No newline at end of file diff --git a/src/colliders/ConeCollider..cpp b/src/colliders/ConeCollider..cpp index 9bc4be26..8886cd17 100644 --- a/src/colliders/ConeCollider..cpp +++ b/src/colliders/ConeCollider..cpp @@ -44,7 +44,7 @@ using namespace reactphysics3d; // Constructor -ConeCollider::ConeCollider(decimal radius, decimal height) : radius(radius), halfHeight(height/2.0) { +ConeCollider::ConeCollider(decimal radius, decimal height) : Collider(CONE), radius(radius), halfHeight(height/2.0) { assert(radius > 0.0); assert(halfHeight > 0.0); diff --git a/src/colliders/CylinderCollider.cpp b/src/colliders/CylinderCollider.cpp index 001b15c8..e34d4eec 100644 --- a/src/colliders/CylinderCollider.cpp +++ b/src/colliders/CylinderCollider.cpp @@ -43,7 +43,8 @@ using namespace reactphysics3d; // Constructor -CylinderCollider::CylinderCollider(decimal radius, decimal height) : radius(radius), halfHeight(height/2.0) { +CylinderCollider::CylinderCollider(decimal radius, decimal height) + : Collider(CYLINDER), radius(radius), halfHeight(height/2.0) { } diff --git a/src/colliders/SphereCollider.cpp b/src/colliders/SphereCollider.cpp index cdc473f0..c852928c 100644 --- a/src/colliders/SphereCollider.cpp +++ b/src/colliders/SphereCollider.cpp @@ -45,7 +45,7 @@ using namespace reactphysics3d; using namespace std; // Constructor -SphereCollider::SphereCollider(decimal radius) : radius(radius) { +SphereCollider::SphereCollider(decimal radius) : Collider(SPHERE), radius(radius) { } diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 1d94331d..3c7b1b4f 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -26,7 +26,6 @@ // Libraries #include "CollisionDetection.h" #include "broadphase/SweepAndPruneAlgorithm.h" -#include "narrowphase/GJK/GJKAlgorithm.h" #include "../body/Body.h" #include "../colliders/BoxCollider.h" #include "../body/RigidBody.h" @@ -42,13 +41,14 @@ using namespace std; // Constructor CollisionDetection::CollisionDetection(PhysicsWorld* world) - : world(world), memoryPoolContacts(NB_MAX_CONTACTS), memoryPoolOverlappingPairs(NB_MAX_COLLISION_PAIRS) { + : world(world), narrowPhaseGJKAlgorithm(*this), narrowPhaseSphereVsSphereAlgorithm(*this), + memoryPoolContacts(NB_MAX_CONTACTS), memoryPoolOverlappingPairs(NB_MAX_COLLISION_PAIRS) { // Create the broad-phase algorithm that will be used (Sweep and Prune with AABB) broadPhaseAlgorithm = new SweepAndPruneAlgorithm(*this); - // Create the narrow-phase algorithm that will be used (Separating axis algorithm) - narrowPhaseAlgorithm = new GJKAlgorithm(*this); + // Create the narrow-phase algorithm that will be used + //narrowPhaseAlgorithm = new GJKAlgorithm(*this); } // Destructor @@ -118,9 +118,12 @@ bool CollisionDetection::computeNarrowPhase() { // Update the contact cache of the overlapping pair (*it).second->update(); + // Select the narrow phase algorithm to use according to the two colliders + NarrowPhaseAlgorithm& narrowPhaseAlgorithm = SelectNarrowPhaseAlgorithm(body1->getCollider(), body2->getCollider()); + // Use the narrow-phase collision detection algorithm to check if there really are a contact - if (narrowPhaseAlgorithm->testCollision(body1->getCollider(), body1->getTransform(), - body2->getCollider(), body2->getTransform(), contactInfo)) { + if (narrowPhaseAlgorithm.testCollision(body1->getCollider(), body1->getTransform(), + body2->getCollider(), body2->getTransform(), contactInfo)) { assert(contactInfo); collisionExists = true; diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index d163a4cf..bcd5091e 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -31,6 +31,8 @@ #include "OverlappingPair.h" #include "../engine/PhysicsWorld.h" #include "../memory/MemoryPool.h" +#include "narrowphase/GJK/GJKAlgorithm.h" +#include "narrowphase/SphereVsSphereAlgorithm.h" #include "ContactInfo.h" #include #include @@ -43,37 +45,39 @@ namespace reactphysics3d { // Declarations class BroadPhaseAlgorithm; -class NarrowPhaseAlgorithm; /* ------------------------------------------------------------------- Class CollisionDetection : This class computes the collision detection algorithms. We first - perfom a broad-phase algorithm to know which pairs of bodies can + perform a broad-phase algorithm to know which pairs of bodies can collide and then we run a narrow-phase algorithm to compute the collision contacts between bodies. ------------------------------------------------------------------- */ class CollisionDetection { private : - PhysicsWorld* world; // Pointer to the physics world - std::map, OverlappingPair*> overlappingPairs; // Broad-phase overlapping pairs of bodies - std::set > currentStepOverlappingPairs; // Overlapping pairs of bodies at the current collision detection step - std::set > lastStepOverlappingPairs; // Overlapping pairs of bodies at the last collision detection step - BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm - NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm - MemoryPool memoryPoolContacts; // Memory pool for the contacts - MemoryPool memoryPoolOverlappingPairs; // Memory pool for the overlapping pairs - - void computeBroadPhase(); // Compute the broad-phase collision detection - bool computeNarrowPhase(); // Compute the narrow-phase collision detection + PhysicsWorld* world; // Pointer to the physics world + std::map, OverlappingPair*> overlappingPairs; // Broad-phase overlapping pairs of bodies + std::set > currentStepOverlappingPairs; // Overlapping pairs of bodies at the current collision detection step + std::set > lastStepOverlappingPairs; // Overlapping pairs of bodies at the last collision detection step + BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm + GJKAlgorithm narrowPhaseGJKAlgorithm; // Narrow-phase GJK algorithm + SphereVsSphereAlgorithm narrowPhaseSphereVsSphereAlgorithm; // Narrow-phase Sphere vs Sphere algorithm + MemoryPool memoryPoolContacts; // Memory pool for the contacts + MemoryPool memoryPoolOverlappingPairs; // Memory pool for the overlapping pairs + void computeBroadPhase(); // Compute the broad-phase collision detection + bool computeNarrowPhase(); // Compute the narrow-phase collision detection + NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(Collider* collider1, + Collider* collider2); // Select the narrow phase algorithm to use given two colliders + public : - CollisionDetection(PhysicsWorld* physicsWorld); // Constructor - ~CollisionDetection(); // Destructor + CollisionDetection(PhysicsWorld* physicsWorld); // Constructor + ~CollisionDetection(); // Destructor - OverlappingPair* getOverlappingPair(luint body1ID, luint body2ID); // Return an overlapping pair or null - bool computeCollisionDetection(); // Compute the collision detection - void broadPhaseNotifyOverlappingPair(Body* body1, Body* body2); // Allow the broadphase to notify the collision detection about an overlapping pair + OverlappingPair* getOverlappingPair(luint body1ID, luint body2ID); // Return an overlapping pair or null + bool computeCollisionDetection(); // Compute the collision detection + void broadPhaseNotifyOverlappingPair(Body* body1, Body* body2); // Allow the broadphase to notify the collision detection about an overlapping pair }; // Return an overlapping pair of bodies according to the given bodies ID @@ -86,6 +90,18 @@ inline OverlappingPair* CollisionDetection::getOverlappingPair(luint body1ID, lu return NULL; } +// Select the narrow-phase collision algorithm to use given two colliders +inline NarrowPhaseAlgorithm& CollisionDetection::SelectNarrowPhaseAlgorithm(Collider* collider1, Collider* collider2) { + + // Sphere vs Sphere algorithm + if (collider1->getType() == SPHERE && collider2->getType() == SPHERE) { + return narrowPhaseSphereVsSphereAlgorithm; + } + else { // GJK algorithm + return narrowPhaseGJKAlgorithm; + } +} + } // End of the ReactPhysics3D namespace #endif diff --git a/src/collision/ContactInfo.h b/src/collision/ContactInfo.h index b93c2979..87056f47 100644 --- a/src/collision/ContactInfo.h +++ b/src/collision/ContactInfo.h @@ -36,7 +36,7 @@ namespace reactphysics3d { /* ------------------------------------------------------------------- Structure ContactInfo : This structure contains informations about a collision contact - computed durring the narow phase collision detection. Those + computed during the narrow-phase collision detection. Those informations are use to compute the contact set for a contact between two bodies. ------------------------------------------------------------------- @@ -46,7 +46,7 @@ struct ContactInfo { Body* const body1; // Pointer to the first body of the contact Body* const body2; // Pointer to the second body of the contact const Vector3 normal; // Normal vector the the collision contact in world space - const decimal penetrationDepth; // Penetration depth of the contact + const decimal penetrationDepth; // Penetration depth of the contact const Vector3 localPoint1; // Contact point of body 1 in local space of body 1 const Vector3 localPoint2; // Contact point of body 2 in local space of body 2 const Vector3 worldPoint1; // Contact point of body 1 in world space @@ -54,7 +54,7 @@ struct ContactInfo { ContactInfo(Body* body1, Body* body2, const Vector3& normal, decimal penetrationDepth, const Vector3& localPoint1, const Vector3& localPoint2, - const Transform& transform1, const Transform& transform2); // Constructor for GJK + const Transform& transform1, const Transform& transform2); // Constructor }; } // End of the ReactPhysics3D namespace diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 8895b254..787afee0 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -28,6 +28,8 @@ #include "Simplex.h" #include "../../../constraint/Contact.h" #include "../../../configuration.h" +#include "../../OverlappingPair.h" +#include "../../CollisionDetection.h" #include #include #include diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index 351258bc..a1e08ba0 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -29,10 +29,12 @@ // Libraries #include "../../body/Body.h" #include "../ContactInfo.h" -#include "../CollisionDetection.h" // Namespace ReactPhysics3D namespace reactphysics3d { + +// Class declarations +class CollisionDetection; /* ------------------------------------------------------------------- Class NarrowPhaseAlgorithm : diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp new file mode 100644 index 00000000..9c3ace60 --- /dev/null +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -0,0 +1,74 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2012 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 "../../colliders/SphereCollider.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +SphereVsSphereAlgorithm::SphereVsSphereAlgorithm(CollisionDetection& collisionDetection) + :NarrowPhaseAlgorithm(collisionDetection) { + +} + +// Destructor +SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() { + +} + +bool SphereVsSphereAlgorithm::testCollision(const Collider* collider1, const Transform& transform1, + const Collider* collider2, const Transform& transform2, ContactInfo*& contactInfo) { + + assert(collider1 != collider2); + + // Get the sphere colliders + const SphereCollider* sphereCollider1 = dynamic_cast(collider1); + const SphereCollider* sphereCollider2 = dynamic_cast(collider2); + + // Compute the distance between the centers + Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); + decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare(); + + // Compute the sum of the radius + decimal sumRadius = sphereCollider1->getRadius() + sphereCollider2->getRadius(); + + // If the sphere colliders intersect + if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) { + Vector3 centerSphere2InBody1LocalSpace = transform1.inverse() * transform2.getPosition(); + Vector3 centerSphere1InBody2LocalSpace = transform2.inverse() * transform1.getPosition(); + Vector3 intersectionOnBody1 = sphereCollider1->getRadius() * centerSphere2InBody1LocalSpace.getUnit(); + Vector3 intersectionOnBody2 = sphereCollider2->getRadius() * centerSphere1InBody2LocalSpace.getUnit(); + decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); + contactInfo = new ContactInfo(collider1->getBodyPointer(), collider2->getBodyPointer(), vectorBetweenCenters.getUnit(), + penetrationDepth, intersectionOnBody1, intersectionOnBody2, transform1, transform2); + + return true; + } + + return false; +} \ No newline at end of file diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h new file mode 100644 index 00000000..f4d0b8dc --- /dev/null +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -0,0 +1,59 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2012 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 SPHERE_VS_SPHERE_ALGORITHM_H +#define SPHERE_VS_SPHERE_ALGORITHM_H + +// Libraries +#include "../../body/Body.h" +#include "../ContactInfo.h" +#include "NarrowPhaseAlgorithm.h" + + +// Namespace ReactPhysics3D +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class SphereVsSphereAlgorithm : + This class is used to compute the narrow-phase collision detection + between two sphere colliders. + ------------------------------------------------------------------- +*/ +class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { + protected : + + public : + SphereVsSphereAlgorithm(CollisionDetection& collisionDetection); // Constructor + virtual ~SphereVsSphereAlgorithm(); // Destructor + + virtual bool testCollision(const Collider* collider1, const Transform& transform1, + const Collider* collider2, const Transform& transform2, + ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide +}; + +} // End of reactphysics3d namespace + +#endif + diff --git a/src/configuration.h b/src/configuration.h index 7f0f0cf1..ebbd631c 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -52,7 +52,8 @@ const reactphysics3d::decimal DECIMAL_INFINITY = std::numeric_limits 0.0); } diff --git a/src/engine/PhysicsEngine.h b/src/engine/PhysicsEngine.h index 5f9e5807..23b00873 100644 --- a/src/engine/PhysicsEngine.h +++ b/src/engine/PhysicsEngine.h @@ -44,12 +44,12 @@ namespace reactphysics3d { ------------------------------------------------------------------- */ class PhysicsEngine { - protected : + private : PhysicsWorld* world; // Pointer to the physics world of the physics engine Timer timer; // Timer of the physics engine CollisionDetection collisionDetection; // Collision detection ConstraintSolver constraintSolver; // Constraint solver - + bool isDeactivationActive; // True if the deactivation (sleeping) of inactive bodies is enabled void updateAllBodiesMotion(); // Compute the motion of all bodies and update their positions and orientations void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity, const Vector3& newAngVelocity, const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection); // Update the position and orientation of a body @@ -63,7 +63,6 @@ public : void start(); // Start the physics simulation void stop(); // Stop the physics simulation void update(); // Update the physics simulation - void setPenetrationFactor(decimal factor); // Set the penetration factor for the constraint solver void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver void setIsErrorCorrectionActive(bool isErrorCorrectionActive); // Set the isErrorCorrectionActive value @@ -78,12 +77,7 @@ inline void PhysicsEngine::start() { inline void PhysicsEngine::stop() { timer.stop(); -} - -// Set the penetration factor for the constraint solver -inline void PhysicsEngine::setPenetrationFactor(decimal factor) { - constraintSolver.setPenetrationFactor(factor); -} +} // Set the number of iterations of the LCP solver inline void PhysicsEngine::setNbLCPIterations(uint nbIterations) { diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h index 87b34035..7e0e2364 100644 --- a/src/mathematics/Transform.h +++ b/src/mathematics/Transform.h @@ -63,8 +63,8 @@ class Transform { const Transform& newTransform, decimal interpolationFactor); // Return an interpolated transform - Vector3 operator*(const Vector3& vector) const; // Return the transformed vector - Transform operator*(const Transform& transform2) const; // Operator of multiplication of a transform with another one + Vector3 operator*(const Vector3& vector) const; // Return the transformed vector + Transform operator*(const Transform& transform2) const; // Operator of multiplication of a transform with another one }; // Return the position of the transform