diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index edc71428..450ceb1e 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -4,6 +4,11 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6) # Set a variable for the directory of the opengl-framework SET(OPENGLFRAMEWORK_DIR "${CMAKE_CURRENT_SOURCE_DIR}/common/opengl-framework") +# If we will use FREEGLUT +IF(NOT APPLE) + ADD_DEFINITIONS(-DUSE_FREEGLUT) +ENDIF() + ADD_SUBDIRECTORY(common/) ADD_SUBDIRECTORY(cubes/) ADD_SUBDIRECTORY(joints/) diff --git a/examples/collisionshapes/CollisionShapes.cpp b/examples/collisionshapes/CollisionShapes.cpp index cf1f9fdc..6f12deeb 100644 --- a/examples/collisionshapes/CollisionShapes.cpp +++ b/examples/collisionshapes/CollisionShapes.cpp @@ -120,7 +120,6 @@ void keyboard(unsigned char key, int x, int y) { // Escape key case 27: #ifdef USE_FREEGLUT - // TODO : Check if we need to call finish() here glutLeaveMainLoop(); #endif break; diff --git a/examples/collisionshapes/Scene.cpp b/examples/collisionshapes/Scene.cpp index 923e4206..360dba9e 100644 --- a/examples/collisionshapes/Scene.cpp +++ b/examples/collisionshapes/Scene.cpp @@ -74,7 +74,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0), Box* box = new Box(BOX_SIZE, position , BOX_MASS, mDynamicsWorld); // The sphere is a moving rigid body - box->getRigidBody()->setIsMotionEnabled(true); + box->getRigidBody()->enableMotion(true); // Change the material properties of the rigid body rp3d::Material& material = box->getRigidBody()->getMaterial(); @@ -97,7 +97,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0), Sphere* sphere = new Sphere(SPHERE_RADIUS, position , BOX_MASS, mDynamicsWorld); // The sphere is a moving rigid body - sphere->getRigidBody()->setIsMotionEnabled(true); + sphere->getRigidBody()->enableMotion(true); // Change the material properties of the rigid body rp3d::Material& material = sphere->getRigidBody()->getMaterial(); @@ -120,7 +120,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0), Cone* cone = new Cone(CONE_RADIUS, CONE_HEIGHT, position , CONE_MASS, mDynamicsWorld); // The cone is a moving rigid body - cone->getRigidBody()->setIsMotionEnabled(true); + cone->getRigidBody()->enableMotion(true); // Change the material properties of the rigid body rp3d::Material& material = cone->getRigidBody()->getMaterial(); @@ -144,7 +144,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0), CYLINDER_MASS, mDynamicsWorld); // The cylinder is a moving rigid body - cylinder->getRigidBody()->setIsMotionEnabled(true); + cylinder->getRigidBody()->enableMotion(true); // Change the material properties of the rigid body rp3d::Material& material = cylinder->getRigidBody()->getMaterial(); @@ -168,7 +168,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0), CAPSULE_MASS, mDynamicsWorld); // The cylinder is a moving rigid body - capsule->getRigidBody()->setIsMotionEnabled(true); + capsule->getRigidBody()->enableMotion(true); // Change the material properties of the rigid body rp3d::Material& material = capsule->getRigidBody()->getMaterial(); @@ -191,7 +191,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0), ConvexMesh* mesh = new ConvexMesh(position, MESH_MASS, mDynamicsWorld); // The mesh is a moving rigid body - mesh->getRigidBody()->setIsMotionEnabled(true); + mesh->getRigidBody()->enableMotion(true); // Change the material properties of the rigid body rp3d::Material& material = mesh->getRigidBody()->getMaterial(); @@ -206,7 +206,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0), mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); // The floor must be a non-moving rigid body - mFloor->getRigidBody()->setIsMotionEnabled(false); + mFloor->getRigidBody()->enableMotion(false); // Change the material properties of the rigid body rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); diff --git a/examples/common/Box.cpp b/examples/common/Box.cpp index a148921b..e5a2bab1 100644 --- a/examples/common/Box.cpp +++ b/examples/common/Box.cpp @@ -121,7 +121,7 @@ void Box::render(openglframework::Shader& shader, localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); shader.setMatrix3x3Uniform("normalMatrix", normalMatrix); - // TODO : REMOVE THIS + // Set the vertex color openglframework::Vector4 color(mColor.r, mColor.g, mColor.b, mColor.a); shader.setVector4Uniform("vertexColor", color); diff --git a/examples/common/opengl-framework/CMakeLists.txt b/examples/common/opengl-framework/CMakeLists.txt index ab8192db..e8e3c5f1 100644 --- a/examples/common/opengl-framework/CMakeLists.txt +++ b/examples/common/opengl-framework/CMakeLists.txt @@ -28,24 +28,23 @@ ENDIF() # Find the GLUT/FREEGLUT library IF(APPLE) - - # Find the GLUT library - FIND_PACKAGE(GLUT REQUIRED) - IF(GLUT_FOUND) - MESSAGE("GLUT found") - ELSE(GLUT_FOUND) - MESSAGE(SEND_ERROR "GLUT not found") + + # Find the GLUT library + FIND_PACKAGE(GLUT REQUIRED) + IF(GLUT_FOUND) + MESSAGE("GLUT found") + ELSE(GLUT_FOUND) + MESSAGE(SEND_ERROR "GLUT not found") ENDIF(GLUT_FOUND) ELSE(APPLE) - # Find the FREEGLUT library - FIND_PACKAGE(FREEGLUT REQUIRED) - IF(FREEGLUT_FOUND) - MESSAGE("FREEGLUT found") - ADD_DEFINITIONS(-DUSE_FREEGLUT) - ELSE(FREEGLUT_FOUND) - MESSAGE(SEND_ERROR "FREEGLUT not found") + # Find the FREEGLUT library + FIND_PACKAGE(FREEGLUT REQUIRED) + IF(FREEGLUT_FOUND) + MESSAGE("FREEGLUT found") + ELSE(FREEGLUT_FOUND) + MESSAGE(SEND_ERROR "FREEGLUT not found") ENDIF(FREEGLUT_FOUND) ENDIF(APPLE) diff --git a/examples/cubes/Cubes.cpp b/examples/cubes/Cubes.cpp index 80c5d501..bd148434 100644 --- a/examples/cubes/Cubes.cpp +++ b/examples/cubes/Cubes.cpp @@ -120,7 +120,6 @@ void keyboard(unsigned char key, int x, int y) { // Escape key case 27: #ifdef USE_FREEGLUT - // TODO : Check if we need to call finish() here glutLeaveMainLoop(); #endif break; diff --git a/examples/cubes/Scene.cpp b/examples/cubes/Scene.cpp index e3bc109a..aa417739 100644 --- a/examples/cubes/Scene.cpp +++ b/examples/cubes/Scene.cpp @@ -70,7 +70,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0), // Create a cube and a corresponding rigid in the dynamics world Box* cube = new Box(BOX_SIZE, position , BOX_MASS, mDynamicsWorld); - cube->getRigidBody()->setIsMotionEnabled(true); + cube->getRigidBody()->enableMotion(true); // Change the material properties of the rigid body rp3d::Material& material = cube->getRigidBody()->getMaterial(); @@ -85,7 +85,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0), mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); // The floor must be a non-moving rigid body - mFloor->getRigidBody()->setIsMotionEnabled(false); + mFloor->getRigidBody()->enableMotion(false); // Change the material properties of the floor rigid body rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); diff --git a/examples/joints/Joints.cpp b/examples/joints/Joints.cpp index 75c6892a..abb58d8a 100644 --- a/examples/joints/Joints.cpp +++ b/examples/joints/Joints.cpp @@ -119,7 +119,6 @@ void keyboard(unsigned char key, int x, int y) { // Escape key case 27: #ifdef USE_FREEGLUT - // TODO : Check if we need to call finish() here glutLeaveMainLoop(); #endif break; diff --git a/examples/joints/Scene.cpp b/examples/joints/Scene.cpp index 5a566f17..a651334d 100644 --- a/examples/joints/Scene.cpp +++ b/examples/joints/Scene.cpp @@ -205,8 +205,8 @@ void Scene::createBallAndSocketJoints() { mDynamicsWorld); // The fist box cannot move - if (i == 0) mBallAndSocketJointChainBoxes[i]->getRigidBody()->setIsMotionEnabled(false); - else mBallAndSocketJointChainBoxes[i]->getRigidBody()->setIsMotionEnabled(true); + if (i == 0) mBallAndSocketJointChainBoxes[i]->getRigidBody()->enableMotion(false); + else mBallAndSocketJointChainBoxes[i]->getRigidBody()->enableMotion(true); // Add some angular velocity damping mBallAndSocketJointChainBoxes[i]->getRigidBody()->setAngularDamping(rp3d::decimal(0.2)); @@ -249,7 +249,7 @@ void Scene::createSliderJoint() { mSliderJointBottomBox = new Box(box1Dimension, positionBox1 , BOX_MASS, mDynamicsWorld); // The fist box cannot move - mSliderJointBottomBox->getRigidBody()->setIsMotionEnabled(false); + mSliderJointBottomBox->getRigidBody()->enableMotion(false); // Change the material properties of the rigid body rp3d::Material& material1 = mSliderJointBottomBox->getRigidBody()->getMaterial(); @@ -265,7 +265,7 @@ void Scene::createSliderJoint() { mSliderJointTopBox = new Box(box2Dimension, positionBox2 , BOX_MASS, mDynamicsWorld); // The second box is allowed to move - mSliderJointTopBox->getRigidBody()->setIsMotionEnabled(true); + mSliderJointTopBox->getRigidBody()->enableMotion(true); // Change the material properties of the rigid body rp3d::Material& material2 = mSliderJointTopBox->getRigidBody()->getMaterial(); @@ -304,7 +304,7 @@ void Scene::createPropellerHingeJoint() { mPropellerBox = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld); // The fist box cannot move - mPropellerBox->getRigidBody()->setIsMotionEnabled(true); + mPropellerBox->getRigidBody()->enableMotion(true); // Change the material properties of the rigid body rp3d::Material& material = mPropellerBox->getRigidBody()->getMaterial(); @@ -342,7 +342,7 @@ void Scene::createFixedJoints() { mFixedJointBox1 = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld); // The fist box cannot move - mFixedJointBox1->getRigidBody()->setIsMotionEnabled(true); + mFixedJointBox1->getRigidBody()->enableMotion(true); // Change the material properties of the rigid body rp3d::Material& material1 = mFixedJointBox1->getRigidBody()->getMaterial(); @@ -357,7 +357,7 @@ void Scene::createFixedJoints() { mFixedJointBox2 = new Box(boxDimension, positionBox2 , BOX_MASS, mDynamicsWorld); // The second box is allowed to move - mFixedJointBox2->getRigidBody()->setIsMotionEnabled(true); + mFixedJointBox2->getRigidBody()->enableMotion(true); // Change the material properties of the rigid body rp3d::Material& material2 = mFixedJointBox2->getRigidBody()->getMaterial(); @@ -396,7 +396,7 @@ void Scene::createFloor() { mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); // The floor must be a non-moving rigid body - mFloor->getRigidBody()->setIsMotionEnabled(false); + mFloor->getRigidBody()->enableMotion(false); // Change the material properties of the rigid body rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); diff --git a/src/body/Body.h b/src/body/Body.h index af8ad635..a7351cd0 100644 --- a/src/body/Body.h +++ b/src/body/Body.h @@ -83,12 +83,6 @@ class Body { /// Return the id of the body bodyindex getID() const; - /// Return true if the body has already been added in an island (for the sleeping technique) - bool isAlreadyInIsland() const; - - /// Set the value of to know if the body has already been added into an island - void setIsAlreadyInIsland(bool isAlreadyInIsland); - /// Return whether or not the body is allowed to sleep bool isAllowedToSleep() const; @@ -126,16 +120,6 @@ inline bodyindex Body::getID() const { return mID; } -// Return true if the body has already been added in an island (for the sleeping technique) -inline bool Body::isAlreadyInIsland() const { - return mIsAlreadyInIsland; -} - -// Set the value of to know if the body has already been added into an island -inline void Body::setIsAlreadyInIsland(bool isAlreadyInIsland) { - mIsAlreadyInIsland = isAlreadyInIsland; -} - // Return whether or not the body is allowed to sleep inline bool Body::isAllowedToSleep() const { return mIsAllowedToSleep; diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index 399bd971..0cd9df84 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -91,6 +91,12 @@ class CollisionBody : public Body { /// Reset the contact manifold lists void resetContactManifoldsList(MemoryAllocator& memoryAllocator); + /// Update the old transform with the current one. + void updateOldTransform(); + + /// Update the Axis-Aligned Bounding Box coordinates + void updateAABB(); + public : // -------------------- Methods -------------------- // @@ -101,12 +107,6 @@ class CollisionBody : public Body { /// Destructor virtual ~CollisionBody(); - /// Return true if the body has moved during the last frame - bool getHasMoved() const; - - /// Set the hasMoved variable (true if the body has moved during the last frame) - void setHasMoved(bool hasMoved); - /// Return the collision shape CollisionShape* getCollisionShape() const; @@ -128,23 +128,17 @@ class CollisionBody : public Body { /// Set the interpolation factor of the body void setInterpolationFactor(decimal factor); - /// Return if the rigid body can move - bool getIsMotionEnabled() const; + /// Return true if the rigid body is allowed to move + bool isMotionEnabled() const; - /// Set the value to true if the body can move - void setIsMotionEnabled(bool isMotionEnabled); + /// Enable/disable the motion of the body + void enableMotion(bool isMotionEnabled); /// Return true if the body can collide with others bodies - bool getIsCollisionEnabled() const; + bool isCollisionEnabled() const; - /// Set the isCollisionEnabled value - void setIsCollisionEnabled(bool isCollisionEnabled); - - /// Update the old transform with the current one. - void updateOldTransform(); - - /// Update the Axis-Aligned Bounding Box coordinates - void updateAABB(); + /// Enable/disable the collision with this body + void enableCollision(bool isCollisionEnabled); /// Return the first element of the linked list of contact manifolds involving this body const ContactManifoldListElement* getContactManifoldsLists() const; @@ -152,18 +146,9 @@ class CollisionBody : public Body { // -------------------- Friendship -------------------- // friend class DynamicsWorld; + friend class CollisionDetection; }; -// Return true if the body has moved during the last frame -inline bool CollisionBody::getHasMoved() const { - return mHasMoved; -} - -// Set the hasMoved variable (true if the body has moved during the last frame) -inline void CollisionBody::setHasMoved(bool hasMoved) { - mHasMoved = hasMoved; -} - // Return the collision shape inline CollisionShape* CollisionBody::getCollisionShape() const { assert(mCollisionShape); @@ -187,13 +172,13 @@ inline void CollisionBody::setInterpolationFactor(decimal factor) { mInterpolationFactor = factor; } -// Return if the rigid body can move -inline bool CollisionBody::getIsMotionEnabled() const { +// Return true if the rigid body is allowed to move +inline bool CollisionBody::isMotionEnabled() const { return mIsMotionEnabled; } -// Set the value to true if the body can move -inline void CollisionBody::setIsMotionEnabled(bool isMotionEnabled) { +// Enable/disable the motion of the body +inline void CollisionBody::enableMotion(bool isMotionEnabled) { mIsMotionEnabled = isMotionEnabled; } @@ -219,12 +204,12 @@ inline const AABB& CollisionBody::getAABB() const { } // Return true if the body can collide with others bodies -inline bool CollisionBody::getIsCollisionEnabled() const { +inline bool CollisionBody::isCollisionEnabled() const { return mIsCollisionEnabled; } -// Set the isCollisionEnabled value -inline void CollisionBody::setIsCollisionEnabled(bool isCollisionEnabled) { +// Enable/disable the collision with this body +inline void CollisionBody::enableCollision(bool isCollisionEnabled) { mIsCollisionEnabled = isCollisionEnabled; } diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index c388e214..4b7980e8 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -105,6 +105,9 @@ class RigidBody : public CollisionBody { /// Remove a joint from the joints list void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint); + /// Set the inverse of the mass + void setMassInverse(decimal massInverse); + public : // -------------------- Methods -------------------- // @@ -134,9 +137,6 @@ class RigidBody : public CollisionBody { /// Set the angular velocity void setAngularVelocity(const Vector3& angularVelocity); - /// Set the inverse of the mass - void setMassInverse(decimal massInverse); - /// Return the inverse of the mass of the body decimal getMassInverse() const; diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index cf006f5b..c98407bf 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -82,7 +82,7 @@ void CollisionDetection::computeBroadPhase() { it != mWorld->getBodiesEndIterator(); it++) { // If the body has moved - if ((*it)->getHasMoved()) { + if ((*it)->mHasMoved) { // Notify the broad-phase that the body has moved mBroadPhaseAlgorithm->updateObject(*it, (*it)->getAABB()); diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index a2aa3427..eee6f1a2 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -38,8 +38,6 @@ #include #include #include -#include // TODO : Delete this - /// ReactPhysics3D namespace namespace reactphysics3d { diff --git a/src/configuration.h b/src/configuration.h index 7141ab18..de4d293c 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -105,7 +105,7 @@ const decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03); const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0); /// Number of iterations when solving the velocity constraints of the Sequential Impulse technique -const uint DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 15; +const uint DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 10; /// Number of iterations when solving the position constraints of the Sequential Impulse technique const uint DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5; diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index c707e8cd..3db0e8ba 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -73,25 +73,25 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS // Compute the matrix K=JM^-1J^t (3x3 matrix) decimal inverseMassBodies = 0.0; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { inverseMassBodies += mBody1->getMassInverse(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { inverseMassBodies += mBody2->getMassInverse(); } Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); } // Compute the inverse mass matrix K^-1 mInverseMassMatrix.setToZero(); - if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) { mInverseMassMatrix = massMatrix.getInverse(); } @@ -123,7 +123,7 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD const decimal inverseMassBody1 = mBody1->getMassInverse(); const decimal inverseMassBody2 = mBody2->getMassInverse(); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda const Vector3 linearImpulseBody1 = -mImpulse; @@ -133,7 +133,7 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD v1 += inverseMassBody1 * linearImpulseBody1; w1 += mI1 * angularImpulseBody1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda const Vector3 linearImpulseBody2 = mImpulse; @@ -165,7 +165,7 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector); mImpulse += deltaLambda; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda const Vector3 linearImpulseBody1 = -deltaLambda; @@ -175,7 +175,7 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con v1 += inverseMassBody1 * linearImpulseBody1; w1 += mI1 * angularImpulseBody1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda const Vector3 linearImpulseBody2 = deltaLambda; @@ -218,23 +218,23 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con // Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints decimal inverseMassBodies = 0.0; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { inverseMassBodies += inverseMassBody1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { inverseMassBodies += inverseMassBody2; } Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); } mInverseMassMatrix.setToZero(); - if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) { mInverseMassMatrix = massMatrix.getInverse(); } @@ -247,7 +247,7 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con const Vector3 lambda = mInverseMassMatrix * (-constraintError); // Apply the impulse to the bodies of the joint (directly update the position/orientation) - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse const Vector3 linearImpulseBody1 = -lambda; @@ -262,7 +262,7 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1.normalize(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse const Vector3 linearImpulseBody2 = lambda; diff --git a/src/constraint/BallAndSocketJoint.h b/src/constraint/BallAndSocketJoint.h index f6bd8d7a..2030627f 100644 --- a/src/constraint/BallAndSocketJoint.h +++ b/src/constraint/BallAndSocketJoint.h @@ -56,7 +56,8 @@ struct BallAndSocketJointInfo : public JointInfo { // Class BallAndSocketJoint /** * This class represents a ball-and-socket joint that allows arbitrary rotation - * between two bodies. + * between two bodies. This joint has three degrees of freedom. It can be used to + * create a chain of bodies for instance. */ class BallAndSocketJoint : public Joint { @@ -104,16 +105,6 @@ class BallAndSocketJoint : public Joint { /// Private assignment operator BallAndSocketJoint& operator=(const BallAndSocketJoint& constraint); - public : - - // -------------------- Methods -------------------- // - - /// Constructor - BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo); - - /// Destructor - virtual ~BallAndSocketJoint(); - /// Return the number of bytes used by the joint virtual size_t getSizeInBytes() const; @@ -128,6 +119,16 @@ class BallAndSocketJoint : public Joint { /// Solve the position constraint (for position error correction) virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo); + + /// Destructor + virtual ~BallAndSocketJoint(); }; // Return the number of bytes used by the joint diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index 46f7b89d..b649d88b 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -81,25 +81,25 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints decimal inverseMassBodies = 0.0; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { inverseMassBodies += mBody1->getMassInverse(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { inverseMassBodies += mBody2->getMassInverse(); } Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); } // Compute the inverse mass matrix K^-1 for the 3 translation constraints mInverseMassMatrixTranslation.setToZero(); - if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) { mInverseMassMatrixTranslation = massMatrix.getInverse(); } @@ -113,13 +113,13 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) mInverseMassMatrixRotation.setToZero(); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { mInverseMassMatrixRotation += mI1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { mInverseMassMatrixRotation += mI2; } - if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) { mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); } @@ -154,7 +154,7 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) { const decimal inverseMassBody1 = mBody1->getMassInverse(); const decimal inverseMassBody2 = mBody2->getMassInverse(); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 3 translation constraints Vector3 linearImpulseBody1 = -mImpulseTranslation; @@ -167,7 +167,7 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) { v1 += inverseMassBody1 * linearImpulseBody1; w1 += mI1 * angularImpulseBody1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 3 translation constraints Vector3 linearImpulseBody2 = mImpulseTranslation; @@ -205,7 +205,7 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS (-JvTranslation - mBiasTranslation); mImpulseTranslation += deltaLambda; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda const Vector3 linearImpulseBody1 = -deltaLambda; @@ -215,7 +215,7 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS v1 += inverseMassBody1 * linearImpulseBody1; w1 += mI1 * angularImpulseBody1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda const Vector3 linearImpulseBody2 = deltaLambda; @@ -235,7 +235,7 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation); mImpulseRotation += deltaLambda2; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 3 rotation constraints const Vector3 angularImpulseBody1 = -deltaLambda2; @@ -243,7 +243,7 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS // Apply the impulse to the body w1 += mI1 * angularImpulseBody1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 3 rotation constraints const Vector3 angularImpulseBody2 = deltaLambda2; @@ -286,23 +286,23 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints decimal inverseMassBodies = 0.0; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { inverseMassBodies += mBody1->getMassInverse(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { inverseMassBodies += mBody2->getMassInverse(); } Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); } mInverseMassMatrixTranslation.setToZero(); - if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) { mInverseMassMatrixTranslation = massMatrix.getInverse(); } @@ -313,7 +313,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation); // Apply the impulse to the bodies of the joint - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse Vector3 linearImpulseBody1 = -lambdaTranslation; @@ -328,7 +328,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1.normalize(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse Vector3 linearImpulseBody2 = lambdaTranslation; @@ -349,13 +349,13 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) mInverseMassMatrixRotation.setToZero(); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { mInverseMassMatrixRotation += mI1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { mInverseMassMatrixRotation += mI2; } - if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) { mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); } @@ -369,7 +369,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS Vector3 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation); // Apply the impulse to the bodies of the joint - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 3 rotation constraints const Vector3 angularImpulseBody1 = -lambdaRotation; @@ -381,7 +381,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1.normalize(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse const Vector3 angularImpulseBody2 = lambdaRotation; diff --git a/src/constraint/FixedJoint.h b/src/constraint/FixedJoint.h index 1e153085..9fe3b055 100644 --- a/src/constraint/FixedJoint.h +++ b/src/constraint/FixedJoint.h @@ -116,16 +116,6 @@ class FixedJoint : public Joint { /// Private assignment operator FixedJoint& operator=(const FixedJoint& constraint); - public : - - // -------------------- Methods -------------------- // - - /// Constructor - FixedJoint(const FixedJointInfo& jointInfo); - - /// Destructor - virtual ~FixedJoint(); - /// Return the number of bytes used by the joint virtual size_t getSizeInBytes() const; @@ -140,6 +130,16 @@ class FixedJoint : public Joint { /// Solve the position constraint (for position error correction) virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + FixedJoint(const FixedJointInfo& jointInfo); + + /// Destructor + virtual ~FixedJoint(); }; // Return the number of bytes used by the joint diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 2a7a5ee0..9e6a132c 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -123,23 +123,23 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix) decimal inverseMassBodies = 0.0; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { inverseMassBodies += mBody1->getMassInverse(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { inverseMassBodies += mBody2->getMassInverse(); } Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); } mInverseMassMatrixTranslation.setToZero(); - if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) { mInverseMassMatrixTranslation = massMatrix.getInverse(); } @@ -155,11 +155,11 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat Vector3 I1C2CrossA1(0, 0, 0); Vector3 I2B2CrossA1(0, 0, 0); Vector3 I2C2CrossA1(0, 0, 0); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { I1B2CrossA1 = mI1 * mB2CrossA1; I1C2CrossA1 = mI1 * mC2CrossA1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { I2B2CrossA1 = mI2 * mB2CrossA1; I2C2CrossA1 = mI2 * mC2CrossA1; } @@ -173,7 +173,7 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat mC2CrossA1.dot(I2C2CrossA1); const Matrix2x2 matrixKRotation(el11, el12, el21, el22); mInverseMassMatrixRotation.setToZero(); - if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) { mInverseMassMatrixRotation = matrixKRotation.getInverse(); } @@ -198,10 +198,10 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) mInverseMassMatrixLimitMotor = 0.0; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { mInverseMassMatrixLimitMotor += mA1.dot(mI1 * mA1); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { mInverseMassMatrixLimitMotor += mA1.dot(mI2 * mA1); } mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? @@ -243,7 +243,7 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Compute the impulse P=J^T * lambda for the motor constraint const Vector3 motorImpulse = -mImpulseMotor * mA1; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 3 translation constraints Vector3 linearImpulseBody1 = -mImpulseTranslation; @@ -262,7 +262,7 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { v1 += inverseMassBody1 * linearImpulseBody1; w1 += mI1 * angularImpulseBody1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 3 translation constraints Vector3 linearImpulseBody2 = mImpulseTranslation; @@ -306,7 +306,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS (-JvTranslation - mBTranslation); mImpulseTranslation += deltaLambdaTranslation; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda const Vector3 linearImpulseBody1 = -deltaLambdaTranslation; @@ -316,7 +316,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS v1 += inverseMassBody1 * linearImpulseBody1; w1 += mI1 * angularImpulseBody1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda const Vector3 linearImpulseBody2 = deltaLambdaTranslation; @@ -337,7 +337,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS Vector2 deltaLambdaRotation = mInverseMassMatrixRotation * (-JvRotation - mBRotation); mImpulseRotation += deltaLambdaRotation; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 2 rotation constraints const Vector3 angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x - @@ -346,7 +346,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS // Apply the impulse to the body w1 += mI1 * angularImpulseBody1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 2 rotation constraints const Vector3 angularImpulseBody2 = mB2CrossA1 * deltaLambdaRotation.x + @@ -372,7 +372,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the lower limit constraint const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1; @@ -380,7 +380,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS // Apply the impulse to the body w1 += mI1 * angularImpulseBody1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the lower limit constraint const Vector3 angularImpulseBody2 = deltaLambdaLower * mA1; @@ -402,7 +402,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0)); deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the upper limit constraint const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1; @@ -410,7 +410,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS // Apply the impulse to the body w1 += mI1 * angularImpulseBody1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the upper limit constraint const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mA1; @@ -435,7 +435,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); deltaLambdaMotor = mImpulseMotor - lambdaTemp; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the motor const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1; @@ -443,7 +443,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS // Apply the impulse to the body w1 += mI1 * angularImpulseBody1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the motor const Vector3 angularImpulseBody2 = deltaLambdaMotor * mA1; @@ -506,23 +506,23 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints decimal inverseMassBodies = 0.0; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { inverseMassBodies += mBody1->getMassInverse(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { inverseMassBodies += mBody2->getMassInverse(); } Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); } mInverseMassMatrixTranslation.setToZero(); - if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) { mInverseMassMatrixTranslation = massMatrix.getInverse(); } @@ -533,7 +533,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation); // Apply the impulse to the bodies of the joint - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse Vector3 linearImpulseBody1 = -lambdaTranslation; @@ -548,7 +548,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1.normalize(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse Vector3 linearImpulseBody2 = lambdaTranslation; @@ -571,11 +571,11 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS Vector3 I1C2CrossA1(0, 0, 0); Vector3 I2B2CrossA1(0, 0, 0); Vector3 I2C2CrossA1(0, 0, 0); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { I1B2CrossA1 = mI1 * mB2CrossA1; I1C2CrossA1 = mI1 * mC2CrossA1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { I2B2CrossA1 = mI2 * mB2CrossA1; I2C2CrossA1 = mI2 * mC2CrossA1; } @@ -589,7 +589,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS mC2CrossA1.dot(I2C2CrossA1); const Matrix2x2 matrixKRotation(el11, el12, el21, el22); mInverseMassMatrixRotation.setToZero(); - if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) { mInverseMassMatrixRotation = matrixKRotation.getInverse(); } @@ -600,7 +600,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS Vector2 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation); // Apply the impulse to the bodies of the joint - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 3 rotation constraints const Vector3 angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x - @@ -613,7 +613,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1.normalize(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse const Vector3 angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x + @@ -635,10 +635,10 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) mInverseMassMatrixLimitMotor = 0.0; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { mInverseMassMatrixLimitMotor += mA1.dot(mI1 * mA1); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { mInverseMassMatrixLimitMotor += mA1.dot(mI2 * mA1); } mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? @@ -652,7 +652,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS decimal lambdaLowerLimit = mInverseMassMatrixLimitMotor * (-lowerLimitError ); // Apply the impulse to the bodies of the joint - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1; @@ -664,7 +664,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1.normalize(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda const Vector3 angularImpulseBody2 = lambdaLowerLimit * mA1; @@ -685,7 +685,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS decimal lambdaUpperLimit = mInverseMassMatrixLimitMotor * (-upperLimitError); // Apply the impulse to the bodies of the joint - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1; @@ -697,7 +697,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1.normalize(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mA1; diff --git a/src/constraint/HingeJoint.h b/src/constraint/HingeJoint.h index f9505992..00dacd29 100644 --- a/src/constraint/HingeJoint.h +++ b/src/constraint/HingeJoint.h @@ -109,7 +109,8 @@ struct HingeJointInfo : public JointInfo { // Class HingeJoint /** * This class represents a hinge joint that allows arbitrary rotation - * between two bodies around a single axis. + * between two bodies around a single axis. This joint has one degree of freedom. It + * can be useful to simulate doors or pendulumns. */ class HingeJoint : public Joint { @@ -246,6 +247,21 @@ class HingeJoint : public Joint { decimal computeCurrentHingeAngle(const Quaternion& orientationBody1, const Quaternion& orientationBody2); + /// Return the number of bytes used by the joint + virtual size_t getSizeInBytes() const; + + /// Initialize before solving the constraint + virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); + + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData); + + /// Solve the velocity constraint + virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); + + /// Solve the position constraint (for position error correction) + virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); + public : // -------------------- Methods -------------------- // @@ -294,21 +310,6 @@ class HingeJoint : public Joint { /// Return the intensity of the current torque applied for the joint motor decimal getMotorTorque(decimal timeStep) const; - - /// Return the number of bytes used by the joint - virtual size_t getSizeInBytes() const; - - /// Initialize before solving the constraint - virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); - - /// Warm start the constraint (apply the previous impulse at the beginning of the step) - virtual void warmstart(const ConstraintSolverData& constraintSolverData); - - /// Solve the velocity constraint - virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); - - /// Solve the position constraint (for position error correction) - virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); }; // Return true if the limits or the joint are enabled diff --git a/src/constraint/Joint.h b/src/constraint/Joint.h index 425ece73..9d57aa08 100644 --- a/src/constraint/Joint.h +++ b/src/constraint/Joint.h @@ -152,6 +152,24 @@ class Joint { /// Private assignment operator Joint& operator=(const Joint& constraint); + /// Return true if the joint has already been added into an island + bool isAlreadyInIsland() const; + + /// Return the number of bytes used by the joint + virtual size_t getSizeInBytes() const = 0; + + /// Initialize before solving the joint + virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) = 0; + + /// Warm start the joint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData) = 0; + + /// Solve the velocity constraint + virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) = 0; + + /// Solve the position constraint + virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) = 0; + public : // -------------------- Methods -------------------- // @@ -177,28 +195,11 @@ class Joint { /// Return true if the collision between the two bodies of the joint is enabled bool isCollisionEnabled() const; - /// Return true if the joint has already been added into an island - bool isAlreadyInIsland() const; - - /// Return the number of bytes used by the joint - virtual size_t getSizeInBytes() const = 0; - - /// Initialize before solving the joint - virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) = 0; - - /// Warm start the joint (apply the previous impulse at the beginning of the step) - virtual void warmstart(const ConstraintSolverData& constraintSolverData) = 0; - - /// Solve the velocity constraint - virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) = 0; - - /// Solve the position constraint - virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) = 0; - // -------------------- Friendship -------------------- // friend class DynamicsWorld; friend class Island; + friend class ConstraintSolver; }; // Return the reference to the body 1 diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 0fd912ce..9a3cdf97 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -129,12 +129,12 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa Vector3 I1R1PlusUCrossN2(0, 0, 0); Vector3 I2R2CrossN1(0, 0, 0); Vector3 I2R2CrossN2(0, 0, 0); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { sumInverseMass += mBody1->getMassInverse(); I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { sumInverseMass += mBody2->getMassInverse(); I2R2CrossN1 = mI2 * mR2CrossN1; I2R2CrossN2 = mI2 * mR2CrossN2; @@ -149,7 +149,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mR2CrossN2.dot(I2R2CrossN2); Matrix2x2 matrixKTranslation(el11, el12, el21, el22); mInverseMassMatrixTranslationConstraint.setToZero(); - if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) { mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); } @@ -165,13 +165,13 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) mInverseMassMatrixRotationConstraint.setToZero(); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { mInverseMassMatrixRotationConstraint += mI1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { mInverseMassMatrixRotationConstraint += mI2; } - if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) { mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); } @@ -188,11 +188,11 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) mInverseMassMatrixLimit = 0.0; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { mInverseMassMatrixLimit += mBody1->getMassInverse() + mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { mInverseMassMatrixLimit += mBody2->getMassInverse() + mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); } @@ -214,10 +214,10 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix) mInverseMassMatrixMotor = 0.0; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { mInverseMassMatrixMotor += mBody1->getMassInverse(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { mInverseMassMatrixMotor += mBody2->getMassInverse(); } mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ? @@ -255,7 +255,7 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Compute the impulse P=J^T * lambda for the motor constraint Vector3 impulseMotor = mImpulseMotor * mSliderAxisWorld; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 2 translation constraints Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - mN2 * mImpulseTranslation.y; @@ -276,7 +276,7 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { v1 += inverseMassBody1 * linearImpulseBody1; w1 += mI1 * angularImpulseBody1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 2 translation constraints Vector3 linearImpulseBody2 = mN1 * mImpulseTranslation.x + mN2 * mImpulseTranslation.y; @@ -325,7 +325,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation); mImpulseTranslation += deltaLambda; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 2 translation constraints const Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y; @@ -336,7 +336,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint v1 += inverseMassBody1 * linearImpulseBody1; w1 += mI1 * angularImpulseBody1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 2 translation constraints const Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y; @@ -356,7 +356,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - mBRotation); mImpulseRotation += deltaLambda2; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 3 rotation constraints const Vector3 angularImpulseBody1 = -deltaLambda2; @@ -364,7 +364,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // Apply the impulse to the body w1 += mI1 * angularImpulseBody1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 3 rotation constraints const Vector3 angularImpulseBody2 = deltaLambda2; @@ -390,7 +390,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the lower limit constraint const Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld; @@ -400,7 +400,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint v1 += inverseMassBody1 * linearImpulseBody1; w1 += mI1 * angularImpulseBody1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the lower limit constraint const Vector3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld; @@ -425,7 +425,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0)); deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the upper limit constraint const Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld; @@ -435,7 +435,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint v1 += inverseMassBody1 * linearImpulseBody1; w1 += mI1 * angularImpulseBody1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the upper limit constraint const Vector3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld; @@ -462,7 +462,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); deltaLambdaMotor = mImpulseMotor - lambdaTemp; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the motor const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld; @@ -470,7 +470,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // Apply the impulse to the body v1 += inverseMassBody1 * linearImpulseBody1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the motor const Vector3 linearImpulseBody2 = -deltaLambdaMotor * mSliderAxisWorld; @@ -540,12 +540,12 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint Vector3 I1R1PlusUCrossN2(0, 0, 0); Vector3 I2R2CrossN1(0, 0, 0); Vector3 I2R2CrossN2(0, 0, 0); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { sumInverseMass += mBody1->getMassInverse(); I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { sumInverseMass += mBody2->getMassInverse(); I2R2CrossN1 = mI2 * mR2CrossN1; I2R2CrossN2 = mI2 * mR2CrossN2; @@ -560,7 +560,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint mR2CrossN2.dot(I2R2CrossN2); Matrix2x2 matrixKTranslation(el11, el12, el21, el22); mInverseMassMatrixTranslationConstraint.setToZero(); - if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) { mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); } @@ -570,7 +570,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Compute the Lagrange multiplier lambda for the 2 translation constraints Vector2 lambdaTranslation = mInverseMassMatrixTranslationConstraint * (-translationError); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 2 translation constraints const Vector3 linearImpulseBody1 = -mN1 * lambdaTranslation.x - mN2 * lambdaTranslation.y; @@ -586,7 +586,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1.normalize(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 2 translation constraints const Vector3 linearImpulseBody2 = mN1 * lambdaTranslation.x + mN2 * lambdaTranslation.y; @@ -608,13 +608,13 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) mInverseMassMatrixRotationConstraint.setToZero(); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { mInverseMassMatrixRotationConstraint += mI1; } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { mInverseMassMatrixRotationConstraint += mI2; } - if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) { mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); } @@ -627,7 +627,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Compute the Lagrange multiplier lambda for the 3 rotation constraints Vector3 lambdaRotation = mInverseMassMatrixRotationConstraint * (-errorRotation); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 3 rotation constraints const Vector3 angularImpulseBody1 = -lambdaRotation; @@ -639,7 +639,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1.normalize(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the 3 rotation constraints const Vector3 angularImpulseBody2 = lambdaRotation; @@ -660,11 +660,11 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) mInverseMassMatrixLimit = 0.0; - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { mInverseMassMatrixLimit += mBody1->getMassInverse() + mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { mInverseMassMatrixLimit += mBody2->getMassInverse() + mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); } @@ -678,7 +678,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Compute the Lagrange multiplier lambda for the lower limit constraint decimal lambdaLowerLimit = mInverseMassMatrixLimit * (-lowerLimitError); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the lower limit constraint const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld; @@ -693,7 +693,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1.normalize(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the lower limit constraint const Vector3 linearImpulseBody2 = lambdaLowerLimit * mSliderAxisWorld; @@ -716,7 +716,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Compute the Lagrange multiplier lambda for the upper limit constraint decimal lambdaUpperLimit = mInverseMassMatrixLimit * (-upperLimitError); - if (mBody1->getIsMotionEnabled()) { + if (mBody1->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the upper limit constraint const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld; @@ -731,7 +731,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1.normalize(); } - if (mBody2->getIsMotionEnabled()) { + if (mBody2->isMotionEnabled()) { // Compute the impulse P=J^T * lambda for the upper limit constraint const Vector3 linearImpulseBody2 = -lambdaUpperLimit * mSliderAxisWorld; diff --git a/src/constraint/SliderJoint.h b/src/constraint/SliderJoint.h index 8e06fd26..645c2e47 100644 --- a/src/constraint/SliderJoint.h +++ b/src/constraint/SliderJoint.h @@ -107,7 +107,9 @@ struct SliderJointInfo : public JointInfo { // Class SliderJoint /** - * This class represents a slider joint. + * This class represents a slider joint. This joint has a one degree of freedom. + * It only allows relative translation of the bodies along a single direction and no + * rotation. */ class SliderJoint : public Joint { @@ -245,6 +247,21 @@ class SliderJoint : public Joint { /// Reset the limits void resetLimits(); + /// Return the number of bytes used by the joint + virtual size_t getSizeInBytes() const; + + /// Initialize before solving the constraint + virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); + + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData); + + /// Solve the velocity constraint + virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); + + /// Solve the position constraint (for position error correction) + virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); + public : // -------------------- Methods -------------------- // @@ -296,21 +313,6 @@ class SliderJoint : public Joint { /// Return the intensity of the current force applied for the joint motor decimal getMotorForce(decimal timeStep) const; - - /// Return the number of bytes used by the joint - virtual size_t getSizeInBytes() const; - - /// Initialize before solving the constraint - virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); - - /// Warm start the constraint (apply the previous impulse at the beginning of the step) - virtual void warmstart(const ConstraintSolverData& constraintSolverData); - - /// Solve the velocity constraint - virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); - - /// Solve the position constraint (for position error correction) - virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); }; // Return true if the limits or the joint are enabled diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index b40eb8f2..3bb660b3 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -96,8 +96,8 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); - internalManifold.isBody1Moving = body1->getIsMotionEnabled(); - internalManifold.isBody2Moving = body2->getIsMotionEnabled(); + internalManifold.isBody1Moving = body1->isMotionEnabled(); + internalManifold.isBody2Moving = body2->isMotionEnabled(); internalManifold.massInverseBody1 = body1->getMassInverse(); internalManifold.massInverseBody2 = body2->getMassInverse(); internalManifold.nbContacts = externalManifold->getNbContactPoints(); diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 68073562..7688c5ee 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -174,7 +174,7 @@ void DynamicsWorld::integrateRigidBodiesPositions() { for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { // If the body is allowed to move - if (bodies[b]->getIsMotionEnabled()) { + if (bodies[b]->isMotionEnabled()) { // Get the constrained velocity uint indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; @@ -220,7 +220,7 @@ void DynamicsWorld::updateRigidBodiesAABB() { for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { // If the body has moved - if ((*it)->getHasMoved()) { + if ((*it)->mHasMoved) { // Update the AABB of the rigid body (*it)->updateAABB(); @@ -314,7 +314,7 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0)); // If the body is allowed to move - if (bodies[b]->getIsMotionEnabled()) { + if (bodies[b]->isMotionEnabled()) { // Integrate the external force to get the new velocity of the body mConstrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() + @@ -528,12 +528,9 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { removeCollisionShape(rigidBody->getCollisionShape()); // Destroy all the joints in which the rigid body to be destroyed is involved - // TODO : Iterate on the mJointList of the rigid body instead over all the joints of the world - bodyindex idToRemove = rigidBody->getID(); - for (std::set::iterator it = mJoints.begin(); it != mJoints.end(); ++it) { - if ((*it)->getBody1()->getID() == idToRemove || (*it)->getBody2()->getID() == idToRemove) { - destroyJoint(*it); - } + JointListElement* element; + for (element = rigidBody->mJointsList; element != NULL; element = element->next) { + destroyJoint(element->joint); } // Reset the contact manifold list of the body @@ -752,19 +749,17 @@ void DynamicsWorld::computeIslands() { size_t nbBytesStack = sizeof(RigidBody*) * nbBodies; RigidBody** stackBodiesToVisit = (RigidBody**)mMemoryAllocator.allocate(nbBytesStack); - uint idIsland = 0; // TODO : REMOVE THIS - // For each rigid body of the world for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { RigidBody* body = *it; // If the body has already been added to an island, we go to the next body - if (body->isAlreadyInIsland()) continue; + if (body->mIsAlreadyInIsland) continue; // If the body is not moving, we go to the next body // TODO : When we will use STATIC bodies, we will need to take care of this case here - if (!body->getIsMotionEnabled()) continue; + if (!body->isMotionEnabled()) continue; // If the body is sleeping or inactive, we go to the next body if (body->isSleeping() || !body->isActive()) continue; @@ -777,9 +772,8 @@ void DynamicsWorld::computeIslands() { // Create the new island void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island)); - mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(idIsland, nbBodies,mContactManifolds.size(), + mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies,mContactManifolds.size(), mJoints.size(), mMemoryAllocator); - idIsland++; // While there are still some bodies to visit in the stack while (stackIndex > 0) { @@ -797,7 +791,7 @@ void DynamicsWorld::computeIslands() { // If the current body is not moving, we do not want to perform the DFS // search across that body - if (!bodyToVisit->getIsMotionEnabled()) continue; + if (!bodyToVisit->isMotionEnabled()) continue; // For each contact manifold in which the current body is involded ContactManifoldListElement* contactElement; @@ -819,7 +813,7 @@ void DynamicsWorld::computeIslands() { RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1; // Check if the other body has already been added to the island - if (otherBody->isAlreadyInIsland()) continue; + if (otherBody->mIsAlreadyInIsland) continue; // Insert the other body into the stack of bodies to visit stackBodiesToVisit[stackIndex] = otherBody; @@ -847,7 +841,7 @@ void DynamicsWorld::computeIslands() { RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1; // Check if the other body has already been added to the island - if (otherBody->isAlreadyInIsland()) continue; + if (otherBody->mIsAlreadyInIsland) continue; // Insert the other body into the stack of bodies to visit stackBodiesToVisit[stackIndex] = otherBody; @@ -860,7 +854,7 @@ void DynamicsWorld::computeIslands() { // can also be included in the other islands for (uint i=0; i < mIslands[mNbIslands]->mNbBodies; i++) { - if (!mIslands[mNbIslands]->mBodies[i]->getIsMotionEnabled()) { + if (!mIslands[mNbIslands]->mBodies[i]->isMotionEnabled()) { mIslands[mNbIslands]->mBodies[i]->mIsAlreadyInIsland = false; } } @@ -893,7 +887,7 @@ void DynamicsWorld::updateSleepingBodies() { for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { // Skip static bodies - if (!bodies[b]->getIsMotionEnabled()) continue; + if (!bodies[b]->isMotionEnabled()) continue; // If the body is velocity is large enough to stay awake if (bodies[b]->getLinearVelocity().lengthSquare() > sleepLinearVelocitySquare || diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 36f6ab23..d1a9b158 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -194,7 +194,7 @@ class DynamicsWorld : public CollisionWorld { virtual void notifyNewContact(const BroadPhasePair* pair, const ContactPointInfo* contactInfo); -public : + public : // -------------------- Methods -------------------- // @@ -246,8 +246,8 @@ public : /// Add the joint to the list of joints of the two bodies involved in the joint void addJointToBody(Joint* joint); - //// Add a contact manifold to the linked list of contact manifolds of the two bodies involed - //// in the corresponding contact. + /// Add a contact manifold to the linked list of contact manifolds of the two bodies + /// involed in the corresponding contact. void addContactManifoldToBody(ContactManifold* contactManifold, CollisionBody *body1, CollisionBody *body2); @@ -310,12 +310,6 @@ public : /// Set an event listener object to receive events callbacks. void setEventListener(EventListener* eventListener); - - // TODO : REMOVE THIS - Island** getIslands() { return mIslands;} - - // TODO : REMOVE THIS - uint getNbIslands() const {return mNbIslands;} }; // Reset the external force and torque applied to the bodies @@ -384,7 +378,7 @@ inline void DynamicsWorld::resetBodiesMovementVariable() { it != getRigidBodiesEndIterator(); it++) { // Set the hasMoved variable to false - (*it)->setHasMoved(false); + (*it)->mHasMoved = false; } } diff --git a/src/engine/Island.cpp b/src/engine/Island.cpp index 5f5b36a2..116f652a 100644 --- a/src/engine/Island.cpp +++ b/src/engine/Island.cpp @@ -29,9 +29,9 @@ using namespace reactphysics3d; // Constructor -Island::Island(uint id, uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, +Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, MemoryAllocator& memoryAllocator) - : mID(id), mBodies(NULL), mContactManifolds(NULL), mJoints(NULL), mNbBodies(0), + : mBodies(NULL), mContactManifolds(NULL), mJoints(NULL), mNbBodies(0), mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) { // Allocate memory for the arrays diff --git a/src/engine/Island.h b/src/engine/Island.h index 283e532d..2aa067f3 100644 --- a/src/engine/Island.h +++ b/src/engine/Island.h @@ -45,9 +45,6 @@ class Island { // -------------------- Attributes -------------------- // - // TODO : REMOVE THIS - uint mID; - /// Array with all the bodies of the island RigidBody** mBodies; @@ -91,7 +88,7 @@ class Island { // -------------------- Methods -------------------- // /// Constructor - Island(uint id, uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, + Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, MemoryAllocator& memoryAllocator); /// Destructor @@ -124,9 +121,6 @@ class Island { /// Return a pointer to the array of joints Joint** getJoints(); - // TODO : REMOVE THIS - uint getID() const {return mID;} - // -------------------- Friendship -------------------- // friend class DynamicsWorld; diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index cf85da5e..4724eab0 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -26,7 +26,7 @@ /******************************************************************************** * ReactPhysics3D * -* Version 0.3.0 * +* Version 0.4.0 * * http://code.google.com/p/reactphysics3d/ * * Daniel Chappuis * ********************************************************************************/