diff --git a/sources/reactphysics3d/body/Body.cpp b/sources/reactphysics3d/body/Body.cpp index 2481bccc..14726272 100644 --- a/sources/reactphysics3d/body/Body.cpp +++ b/sources/reactphysics3d/body/Body.cpp @@ -24,11 +24,11 @@ using namespace reactphysics3d; // Constructor -Body::Body(Kilogram mass) throw(std::invalid_argument) : mass(mass) { - // Check if the mass is different from zero - if (mass.getValue() == 0) { +Body::Body(double mass) throw(std::invalid_argument) : mass(mass) { + // Check if the mass is not larger than zero + if (mass <= 0.0) { // We throw an exception - throw std::invalid_argument("Exception in Body constructor : the mass has to be different from zero"); + throw std::invalid_argument("Exception in Body constructor : the mass has to be different larger than zero"); } } diff --git a/sources/reactphysics3d/body/Body.h b/sources/reactphysics3d/body/Body.h index fb1cd2ff..d67bc2bd 100644 --- a/sources/reactphysics3d/body/Body.h +++ b/sources/reactphysics3d/body/Body.h @@ -22,7 +22,6 @@ // Libraries #include -#include "../physics/physics.h" // Namespace reactphysics3d namespace reactphysics3d { @@ -35,17 +34,17 @@ namespace reactphysics3d { */ class Body { protected : - Kilogram mass; // Mass of the body - bool isMotionEnabled; // True if the body is able to move - bool isCollisionEnabled; // True if the body can collide with others bodies + double mass; // Mass of the body + bool isMotionEnabled; // True if the body is able to move + bool isCollisionEnabled; // True if the body can collide with others bodies public : - Body(Kilogram mass) throw(std::invalid_argument); // Constructor + Body(double mass) throw(std::invalid_argument); // Constructor Body(const Body& body); // Copy-constructor virtual ~Body(); // Destructor - Kilogram getMass() const; // Return the mass of the body - void setMass(Kilogram mass); // Set the mass of the body + double getMass() const; // Return the mass of the body + void setMass(double mass); // Set the mass of the body bool getIsMotionEnabled() const; // Return if the rigid body can move void setIsMotionEnabled(bool isMotionEnabled); // Set the value to true if the body can move bool getIsCollisionEnabled() const; // Return true if the body can collide with others bodies @@ -55,7 +54,7 @@ class Body { // --- Inlines function --- // // Method that return the mass of the body -inline Kilogram Body::getMass() const { +inline double Body::getMass() const { return mass; }; @@ -70,7 +69,7 @@ inline void Body::setIsMotionEnabled(bool isMotionEnabled) { } // Method that set the mass of the body -inline void Body::setMass(Kilogram mass) { +inline void Body::setMass(double mass) { this->mass = mass; } diff --git a/sources/reactphysics3d/body/RigidBody.cpp b/sources/reactphysics3d/body/RigidBody.cpp index aac6cc7f..531bbb6f 100644 --- a/sources/reactphysics3d/body/RigidBody.cpp +++ b/sources/reactphysics3d/body/RigidBody.cpp @@ -24,9 +24,9 @@ using namespace reactphysics3d; // Constructor - RigidBody::RigidBody(const Vector3D& position, const Quaternion& orientation, const Kilogram& mass, const Matrix3x3& inertiaTensorLocal, const OBB& obb) - : Body(mass), inertiaTensorLocal(inertiaTensorLocal), currentBodyState(position, orientation, inertiaTensorLocal.getInverse(), Kilogram(1.0/mass.getValue())), - previousBodyState(position, orientation, inertiaTensorLocal.getInverse(), Kilogram(1.0/mass.getValue())), obb(obb) { + RigidBody::RigidBody(const Vector3D& position, const Quaternion& orientation, double mass, const Matrix3x3& inertiaTensorLocal, const OBB& obb) + : Body(mass), position(position), orientation(orientation.getUnit()), inertiaTensorLocal(inertiaTensorLocal), inertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), + massInverse(1.0/mass), oldPosition(position), oldOrientation(orientation), obb(obb) { restitution = 1.0; isMotionEnabled = true; @@ -40,36 +40,7 @@ this->obb.setBodyPointer(this); } - // Copy-constructor - RigidBody::RigidBody(const RigidBody& rigidBody) : Body(rigidBody), inertiaTensorLocal(rigidBody.inertiaTensorLocal), - currentBodyState(rigidBody.currentBodyState), previousBodyState(rigidBody.previousBodyState), obb(rigidBody.obb) { - this->isMotionEnabled = rigidBody.isMotionEnabled; - this->isCollisionEnabled = rigidBody.isCollisionEnabled; - interpolationFactor = rigidBody.interpolationFactor; - } - // Destructor RigidBody::~RigidBody() { -}; - -// Compute the linear interpolation state between the previous body state and the current body state -// This is used to avoid visual stuttering when the display and physics framerates are out of synchronization -BodyState RigidBody::getInterpolatedState() const { - - // Get the interpolation factor - double alpha = interpolationFactor; - - // Compute the linear interpolation state - BodyState interpolatedState = currentBodyState; // Used to take massInverse, inertiaTensorInverse - interpolatedState.setPosition(previousBodyState.getPosition() * (1-alpha) + currentBodyState.getPosition() * alpha); - interpolatedState.setLinearMomentum(previousBodyState.getLinearMomentum() * (1-alpha) + currentBodyState.getLinearMomentum() * alpha); - interpolatedState.setOrientation(Quaternion::slerp(previousBodyState.getOrientation(), currentBodyState.getOrientation(), alpha)); - interpolatedState.setAngularMomentum(previousBodyState.getAngularMomentum() * (1-alpha) + currentBodyState.getAngularMomentum() * alpha); - - // Recalculate the secondary state values - interpolatedState.recalculate(); - - // Return the interpolated state - return interpolatedState; -} +}; \ No newline at end of file diff --git a/sources/reactphysics3d/body/RigidBody.h b/sources/reactphysics3d/body/RigidBody.h index 351a5c9a..75fac7cc 100644 --- a/sources/reactphysics3d/body/RigidBody.h +++ b/sources/reactphysics3d/body/RigidBody.h @@ -23,7 +23,6 @@ // Libraries #include #include "Body.h" - #include "BodyState.h" #include "OBB.h" #include "../mathematics/mathematics.h" #include "../physics/physics.h" @@ -31,9 +30,6 @@ // Namespace reactphysics3d namespace reactphysics3d { -// TODO : Check if it is possible to put all the attributes from BodyState class in the RigidBody class (to do this, the -// class RungeKutta have to be changed a lot - /* ------------------------------------------------------------------- Class RigidBody : This class represents a rigid body of the physics @@ -43,36 +39,129 @@ namespace reactphysics3d { */ class RigidBody : public Body { protected : + Vector3D position; // Position of the center of mass of the body + Vector3D oldPosition; // Old position used to compute the interpolated position + Quaternion orientation; // Orientation quaternion of the body + Quaternion oldOrientation; // Old orientation used to compute the interpolated orientation + Vector3D linearVelocity; // Linear velocity of the body + Vector3D angularVelocity; // Angular velocity of the body + Vector3D externalForce; // Current external force on the body + Vector3D externalTorque; // Current external torque on the body Matrix3x3 inertiaTensorLocal; // Local inertia tensor of the body (in body coordinates) - BodyState currentBodyState; // Current body state - BodyState previousBodyState; // Previous body state + Matrix3x3 inertiaTensorLocalInverse; // Inverse of the inertia tensor of the body (in body coordinates) + double massInverse; // Inverse of the mass of the body + double interpolationFactor; // Interpolation factor used for the state interpolation double restitution; // Coefficient of restitution (between 0 and 1), 1 for a very boucing body OBB obb; // Oriented bounding box that contains the rigid body public : - RigidBody(const Vector3D& position, const Quaternion& orientation, const Kilogram& mass, const Matrix3x3& inertiaTensorLocal, const OBB& obb); // Constructor - RigidBody(const RigidBody& rigidBody); // Copy-constructor - virtual ~RigidBody(); // Destructor + RigidBody(const Vector3D& position, const Quaternion& orientation, double mass, const Matrix3x3& inertiaTensorLocal, const OBB& obb); // Constructor // Copy-constructor + virtual ~RigidBody(); // Destructor + Vector3D getPosition() const; // Return the position of the body + void setPosition(const Vector3D& position); // Set the position of the body + Quaternion getOrientation() const; // Return the orientation quaternion + void setOrientation(const Quaternion& orientation); // Set the orientation quaternion + Vector3D getLinearVelocity() const; // Return the linear velocity + void setLinearVelocity(const Vector3D& linearVelocity); // TODO : Delete this + Vector3D getAngularVelocity() const; // Return the angular velocity + void setAngularVelocity(const Vector3D& angularVelocity); // Set the angular velocity + void setMassInverse(double massInverse); // Set the inverse of the mass + Vector3D getExternalForce() const; // Return the current external force of the body + void setExternalForce(const Vector3D& force); // Set the current external force on the body + Vector3D getExternalTorque() const; // Return the current external torque of the body + void setExternalTorque(const Vector3D& torque); // Set the current external torque of the body + double getMassInverse() const; // Return the inverse of the mass of the body Matrix3x3 getInertiaTensorLocal() const; // Return the local inertia tensor of the body (in body coordinates) void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal); // Set the local inertia tensor of the body (in body coordinates) + Matrix3x3 getInertiaTensorLocalInverse() const; // Get the inverse of the inertia tensor Matrix3x3 getInertiaTensorWorld() const; // Return the inertia tensor in world coordinates Matrix3x3 getInertiaTensorInverseWorld() const; // Return the inverse of the inertia tensor in world coordinates - BodyState& getCurrentBodyState(); // Return a reference to the current state of the body - BodyState& getPreviousBodyState(); // TODO : DELETE THIS void setInterpolationFactor(double factor); // Set the interpolation factor of the body - BodyState getInterpolatedState() const; // Compute and return the interpolated state - void setLinearVelocity(const Vector3D& linearVelocity); // Set the linear velocity of the rigid body + Vector3D getInterpolatedPosition() const; // Return the interpolated position + Quaternion getInterpolatedOrientation() const; // Return the interpolated orientation double getRestitution() const; // Get the restitution coefficient void setRestitution(double restitution) throw(std::invalid_argument); // Set the restitution coefficient - void updatePreviousBodyState(); // Update the previous body state of the body + void updateOldPositionAndOrientation(); // Update the previous position and orientation of the body const OBB* const getOBB() const; // Return the oriented bounding box of the rigid body void update(); // Update the rigid body in order to reflect a change in the body state }; // --- Inline functions --- // +// Return the position of the body +inline Vector3D RigidBody::getPosition() const { + return position; +} + +// Set the position of the body +inline void RigidBody::setPosition(const Vector3D& position) { + this->position = position; +} + +// Return the orientation quaternion of the body +inline Quaternion RigidBody::getOrientation() const { + return orientation; +} + +// Set the orientation quaternion +inline void RigidBody::setOrientation(const Quaternion& orientation) { + this->orientation = orientation; + + // Normalize the orientation quaternion + orientation.getUnit(); +} + +// Return the linear velocity +inline Vector3D RigidBody::getLinearVelocity() const { + return linearVelocity; +} + +// Return the angular velocity of the body +inline Vector3D RigidBody::getAngularVelocity() const { + return angularVelocity; +} + +inline void RigidBody::setAngularVelocity(const Vector3D& angularVelocity) { + this->angularVelocity = angularVelocity; +} + +// Set the inverse of the mass +inline void RigidBody::setMassInverse(double massInverse) { + this->massInverse = massInverse; +} + +// Get the inverse of the inertia tensor +inline Matrix3x3 RigidBody::getInertiaTensorLocalInverse() const { + return inertiaTensorLocalInverse; +} + +// Return the external force on the body +inline Vector3D RigidBody::getExternalForce() const { + return externalForce; +} + +// Set the external force on the body +inline void RigidBody::setExternalForce(const Vector3D& force) { + this->externalForce = force; +} + +// Return the current external torque on the body +inline Vector3D RigidBody::getExternalTorque() const { + return externalTorque; +} + + // Set the current external torque on the body +inline void RigidBody::setExternalTorque(const Vector3D& torque) { + this->externalTorque = torque; +} + +// Return the inverse of the mass of the body +inline double RigidBody::getMassInverse() const { + return massInverse; +} + // Return the local inertia tensor of the body (in body coordinates) inline Matrix3x3 RigidBody::getInertiaTensorLocal() const { return inertiaTensorLocal; @@ -89,7 +178,7 @@ inline void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal // where R is the rotation matrix (and R^T its transpose) of the current orientation quaternion of the body inline Matrix3x3 RigidBody::getInertiaTensorWorld() const { // Compute and return the inertia tensor in world coordinates - return currentBodyState.getOrientation().getMatrix() * inertiaTensorLocal * currentBodyState.getOrientation().getMatrix().getTranspose(); + return orientation.getMatrix() * inertiaTensorLocal * orientation.getMatrix().getTranspose(); } // Return the inverse of the inertia tensor in world coordinates @@ -98,21 +187,9 @@ inline Matrix3x3 RigidBody::getInertiaTensorWorld() const { // where R is the rotation matrix (and R^T its transpose) of the current orientation quaternion of the body inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { // Compute and return the inertia tensor in world coordinates - return currentBodyState.getOrientation().getMatrix() * currentBodyState.getInertiaTensorInverse() * currentBodyState.getOrientation().getMatrix().getTranspose(); + return orientation.getMatrix() * inertiaTensorLocalInverse * orientation.getMatrix().getTranspose(); } -// Return a reference to the current state of the body -// This way the currentBodyState could be modify outside the rigid body -inline BodyState& RigidBody::getCurrentBodyState() { - return currentBodyState; -} - -// TODO : DELETE THIS -inline BodyState& RigidBody::getPreviousBodyState() { - return previousBodyState; -} - - // Set the interpolation factor of the body inline void RigidBody::setInterpolationFactor(double factor) { //assert(factor >= 0.0 && factor <= 1.0); // TODO : Remove the comment here @@ -121,15 +198,24 @@ inline void RigidBody::setInterpolationFactor(double factor) { interpolationFactor = factor; } +// Return the interpolated position +inline Vector3D RigidBody::getInterpolatedPosition() const { + // Compute the interpolated position + return oldPosition * (1-interpolationFactor) + position * interpolationFactor; +} + + // Return the interpolated orientation +inline Quaternion RigidBody::getInterpolatedOrientation() const { + // Compute the interpolated orientation + return Quaternion::slerp(oldOrientation, orientation, interpolationFactor); +} + // Set the linear velocity of the rigid body inline void RigidBody::setLinearVelocity(const Vector3D& linearVelocity) { // If the body is able to move if (isMotionEnabled) { // Update the linear velocity of the current body state - currentBodyState.setLinearVelocity(linearVelocity); - - // Update the linear momentum of the current body state - currentBodyState.setLinearMomentum(linearVelocity * (1.0/currentBodyState.getMassInverse().getValue())); + this->linearVelocity = linearVelocity; } } @@ -150,9 +236,10 @@ inline void RigidBody::setRestitution(double restitution) throw(std::invalid_arg } // Update the previous body state of the body -inline void RigidBody::updatePreviousBodyState() { - // The current body state becomes the previous body state - previousBodyState = currentBodyState; +// This is used to compute the interpolated position and orientation of the body +inline void RigidBody::updateOldPositionAndOrientation() { + oldPosition = position; + oldOrientation = orientation; } // Return the oriented bounding box of the rigid body @@ -163,7 +250,7 @@ inline const OBB* const RigidBody::getOBB() const { // Update the rigid body in order to reflect a change in the body state inline void RigidBody::update() { // Update the orientation of the corresponding bounding volume of the rigid body - obb.updateOrientation(currentBodyState.getPosition(), currentBodyState.getOrientation()); + obb.updateOrientation(position, orientation); } } // End of the ReactPhyscis3D namespace diff --git a/sources/reactphysics3d/engine/ConstraintSolver.cpp b/sources/reactphysics3d/engine/ConstraintSolver.cpp index 057d44cc..7f90bd42 100644 --- a/sources/reactphysics3d/engine/ConstraintSolver.cpp +++ b/sources/reactphysics3d/engine/ConstraintSolver.cpp @@ -183,8 +183,8 @@ void ConstraintSolver::fillInMatrices() { assert(rigidBody != 0); // Compute the vector V1 with initial velocities values - v.fillInSubVector(0, rigidBody->getCurrentBodyState().getLinearVelocity()); - v.fillInSubVector(3, rigidBody->getCurrentBodyState().getAngularVelocity()); + v.fillInSubVector(0, rigidBody->getLinearVelocity()); + v.fillInSubVector(3, rigidBody->getAngularVelocity()); V1[bodyNumber].changeSize(6); V1[bodyNumber] = v; @@ -193,8 +193,8 @@ void ConstraintSolver::fillInMatrices() { Vconstraint[bodyNumber].initWithValue(0.0); // Compute the vector with forces and torques values - f.fillInSubVector(0, rigidBody->getCurrentBodyState().getExternalForce()); - f.fillInSubVector(3, rigidBody->getCurrentBodyState().getExternalTorque()); + f.fillInSubVector(0, rigidBody->getExternalForce()); + f.fillInSubVector(3, rigidBody->getExternalTorque()); Fext[bodyNumber].changeSize(6); Fext[bodyNumber] = f; @@ -202,7 +202,7 @@ void ConstraintSolver::fillInMatrices() { Matrix mInv(6,6); mInv.initWithValue(0.0); if (rigidBody->getIsMotionEnabled()) { - mInv.fillInSubMatrix(0, 0, rigidBody->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3)); + mInv.fillInSubMatrix(0, 0, rigidBody->getMassInverse() * Matrix::identity(3)); mInv.fillInSubMatrix(3, 3, rigidBody->getInertiaTensorInverseWorld()); } Minv_sp[bodyNumber].changeSize(6, 6); diff --git a/sources/reactphysics3d/engine/PhysicsEngine.cpp b/sources/reactphysics3d/engine/PhysicsEngine.cpp index e6d04160..59b03940 100644 --- a/sources/reactphysics3d/engine/PhysicsEngine.cpp +++ b/sources/reactphysics3d/engine/PhysicsEngine.cpp @@ -104,12 +104,12 @@ void PhysicsEngine::updateAllBodiesMotion() { // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the // external forces and torques. - newLinearVelocity = newLinearVelocity + dt * rigidBody->getCurrentBodyState().getMassInverse().getValue() * rigidBody->getCurrentBodyState().getExternalForce(); - newAngularVelocity = newAngularVelocity + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getCurrentBodyState().getExternalTorque(); + newLinearVelocity = newLinearVelocity + dt * rigidBody->getMassInverse() * rigidBody->getExternalForce(); + newAngularVelocity = newAngularVelocity + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); // Add the velocity V1 to the new velocity - newLinearVelocity = newLinearVelocity + rigidBody->getCurrentBodyState().getLinearVelocity(); - newAngularVelocity = newAngularVelocity + rigidBody->getCurrentBodyState().getAngularVelocity(); + newLinearVelocity = newLinearVelocity + rigidBody->getLinearVelocity(); + newAngularVelocity = newAngularVelocity + rigidBody->getAngularVelocity(); // Update the position and the orientation of the body according to the new velocity updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity); @@ -133,21 +133,19 @@ void PhysicsEngine::updateAllBodiesMotion() { RigidBody* rigidBody = dynamic_cast(body); assert(rigidBody); - // The current body state of the body becomes the previous body state - rigidBody->updatePreviousBodyState(); - - BodyState& bodyState = rigidBody->getCurrentBodyState(); + // Update the old position and orientation of the body + rigidBody->updateOldPositionAndOrientation(); // Normalize the orientation quaternion - bodyState.setOrientation(bodyState.getOrientation().getUnit()); + rigidBody->setOrientation(rigidBody->getOrientation().getUnit()); // Update the linear and angular velocity of the body - bodyState.setLinearVelocity(newLinVelocity); - bodyState.setAngularVelocity(newAngVelocity); + rigidBody->setLinearVelocity(newLinVelocity); + rigidBody->setAngularVelocity(newAngVelocity); // Update the position and the orientation of the body - bodyState.setPosition(bodyState.getPosition() + newLinVelocity * dt); - bodyState.setOrientation(bodyState.getOrientation() + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * rigidBody->getCurrentBodyState().getOrientation() * 0.5 * dt); + rigidBody->setPosition(rigidBody->getPosition() + newLinVelocity * dt); + rigidBody->setOrientation(rigidBody->getOrientation() + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * rigidBody->getOrientation() * 0.5 * dt); } // Apply the gravity force to all bodies of the physics world @@ -162,7 +160,7 @@ void PhysicsEngine::applyGravity() { // If the gravity force is on if(world->getIsGravityOn()) { // Apply the current gravity force to the body - rigidBody->getCurrentBodyState().setExternalForce(world->getGravity()); + rigidBody->setExternalForce(rigidBody->getMass() * world->getGravity()); } } } diff --git a/sources/reactphysics3d/engine/PhysicsEngine.h b/sources/reactphysics3d/engine/PhysicsEngine.h index 571e8567..c64e5c3d 100644 --- a/sources/reactphysics3d/engine/PhysicsEngine.h +++ b/sources/reactphysics3d/engine/PhysicsEngine.h @@ -42,8 +42,7 @@ class PhysicsEngine { Timer timer; // Timer of the physics engine CollisionDetection collisionDetection; // Collision detection ConstraintSolver constraintSolver; // Constraint solver - - void updateBodyState(RigidBody* const rigidBody, const Time& timeStep); // Update the state of a rigid body // TODO : Delete this + // Update the state of a rigid body // TODO : Delete this void updateAllBodiesMotion(); // Compute the motion of all bodies and update their positions and orientations void updatePositionAndOrientationOfBody(Body* body, const Vector3D& newLinVelocity, const Vector3D& newAngVelocity); // Update the position and orientation of a body void applyGravity(); // Apply the gravity force to all bodies