diff --git a/CMakeLists.txt b/CMakeLists.txt index dce9b270..9c0a8403 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -145,7 +145,6 @@ SET (REACTPHYSICS3D_HEADERS "src/components/RigidBodyComponents.h" "src/components/TransformComponents.h" "src/components/ProxyShapeComponents.h" - "src/components/DynamicsComponents.h" "src/collision/CollisionCallback.h" "src/collision/OverlapCallback.h" "src/mathematics/mathematics.h" @@ -237,7 +236,6 @@ SET (REACTPHYSICS3D_SOURCES "src/components/RigidBodyComponents.cpp" "src/components/TransformComponents.cpp" "src/components/ProxyShapeComponents.cpp" - "src/components/DynamicsComponents.cpp" "src/collision/CollisionCallback.cpp" "src/collision/OverlapCallback.cpp" "src/mathematics/mathematics_functions.cpp" diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 98843732..2799758d 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -39,15 +39,10 @@ using namespace reactphysics3d; * @param world The world where the body has been added * @param id The ID of the body */ -RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, Entity entity) +RigidBody::RigidBody(CollisionWorld& world, Entity entity) : CollisionBody(world, entity), mMaterial(world.mConfig), mJointsList(nullptr), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) { - // Compute the inverse mass - mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity)); - - // Update the world inverse inertia tensor - updateInertiaTensorInverseWorld(); } // Destructor @@ -94,15 +89,15 @@ void RigidBody::setType(BodyType type) { if (type == BodyType::STATIC || type == BodyType::KINEMATIC) { // Reset the inverse mass and inverse inertia tensor to zero - mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(0)); - mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero()); - mWorld.mDynamicsComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero()); + mWorld.mRigidBodyComponents.setMassInverse(mEntity, decimal(0)); + mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero()); + mWorld.mRigidBodyComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero()); } else { // If it is a dynamic body - mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity)); + mWorld.mRigidBodyComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mRigidBodyComponents.getInitMass(mEntity)); if (mIsInertiaTensorSetByUser) { - mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse); + mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse); } } @@ -117,8 +112,8 @@ void RigidBody::setType(BodyType type) { askForBroadPhaseCollisionCheck(); // Reset the force and torque on the body - mWorld.mDynamicsComponents.setExternalForce(mEntity, Vector3::zero()); - mWorld.mDynamicsComponents.setExternalTorque(mEntity, Vector3::zero()); + mWorld.mRigidBodyComponents.setExternalForce(mEntity, Vector3::zero()); + mWorld.mRigidBodyComponents.setExternalTorque(mEntity, Vector3::zero()); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, "Body " + std::to_string(mEntity.id) + ": Set type=" + @@ -127,7 +122,7 @@ void RigidBody::setType(BodyType type) { // Get the inverse local inertia tensor of the body (in body coordinates) const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const { - return mWorld.mDynamicsComponents.getInertiaTensorLocalInverse(mEntity); + return mWorld.mRigidBodyComponents.getInertiaTensorLocalInverse(mEntity); } // Return the inverse of the inertia tensor in world coordinates. @@ -143,7 +138,7 @@ const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const { Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { // Compute and return the inertia tensor in world coordinates - return mWorld.mDynamicsComponents.getInertiaTensorWorldInverse(mEntity); + return mWorld.mRigidBodyComponents.getInertiaTensorWorldInverse(mEntity); } // Method that return the mass of the body @@ -151,7 +146,7 @@ Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { * @return The mass (in kilograms) of the body */ decimal RigidBody::getMass() const { - return mWorld.mDynamicsComponents.getInitMass(mEntity); + return mWorld.mRigidBodyComponents.getInitMass(mEntity); } // Apply an external force to the body at a given point (in world-space coordinates). @@ -176,13 +171,13 @@ void RigidBody::applyForce(const Vector3& force, const Vector3& point) { } // Add the force - const Vector3& externalForce = mWorld.mDynamicsComponents.getExternalForce(mEntity); - mWorld.mDynamicsComponents.setExternalForce(mEntity, externalForce + force); + const Vector3& externalForce = mWorld.mRigidBodyComponents.getExternalForce(mEntity); + mWorld.mRigidBodyComponents.setExternalForce(mEntity, externalForce + force); // Add the torque - const Vector3& externalTorque = mWorld.mDynamicsComponents.getExternalTorque(mEntity); - const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity); - mWorld.mDynamicsComponents.setExternalTorque(mEntity, externalTorque + (point - centerOfMassWorld).cross(force)); + const Vector3& externalTorque = mWorld.mRigidBodyComponents.getExternalTorque(mEntity); + const Vector3& centerOfMassWorld = mWorld.mRigidBodyComponents.getCenterOfMassWorld(mEntity); + mWorld.mRigidBodyComponents.setExternalTorque(mEntity, externalTorque + (point - centerOfMassWorld).cross(force)); } // Set the local inertia tensor of the body (in local-space coordinates) @@ -200,7 +195,7 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) { if (mWorld.mRigidBodyComponents.getBodyType(mEntity) != BodyType::DYNAMIC) return; // Compute the inverse local inertia tensor - mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse); + mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse); // Update the world inverse inertia tensor updateInertiaTensorInverseWorld(); @@ -228,8 +223,8 @@ void RigidBody::applyForceToCenterOfMass(const Vector3& force) { } // Add the force - const Vector3& externalForce = mWorld.mDynamicsComponents.getExternalForce(mEntity); - mWorld.mDynamicsComponents.setExternalForce(mEntity, externalForce + force); + const Vector3& externalForce = mWorld.mRigidBodyComponents.getExternalForce(mEntity); + mWorld.mRigidBodyComponents.setExternalForce(mEntity, externalForce + force); } // Return the linear velocity damping factor @@ -237,7 +232,7 @@ void RigidBody::applyForceToCenterOfMass(const Vector3& force) { * @return The linear damping factor of this body */ decimal RigidBody::getLinearDamping() const { - return mWorld.mDynamicsComponents.getLinearDamping(mEntity); + return mWorld.mRigidBodyComponents.getLinearDamping(mEntity); } // Return the angular velocity damping factor @@ -245,7 +240,7 @@ decimal RigidBody::getLinearDamping() const { * @return The angular damping factor of this body */ decimal RigidBody::getAngularDamping() const { - return mWorld.mDynamicsComponents.getAngularDamping(mEntity); + return mWorld.mRigidBodyComponents.getAngularDamping(mEntity); } // Set the inverse local inertia tensor of the body (in local-space coordinates) @@ -263,7 +258,7 @@ void RigidBody::setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTens if (mWorld.mRigidBodyComponents.getBodyType(mEntity) != BodyType::DYNAMIC) return; // Compute the inverse local inertia tensor - mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse); + mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse); // Update the world inverse inertia tensor updateInertiaTensorInverseWorld(); @@ -287,17 +282,17 @@ void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) { mIsCenterOfMassSetByUser = true; - const Vector3 oldCenterOfMass = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity); - mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, centerOfMassLocal); + const Vector3 oldCenterOfMass = mWorld.mRigidBodyComponents.getCenterOfMassWorld(mEntity); + mWorld.mRigidBodyComponents.setCenterOfMassLocal(mEntity, centerOfMassLocal); // Compute the center of mass in world-space coordinates - const Vector3& updatedCenterOfMassLocal = mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity); - mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, mWorld.mTransformComponents.getTransform(mEntity) * updatedCenterOfMassLocal); + const Vector3& updatedCenterOfMassLocal = mWorld.mRigidBodyComponents.getCenterOfMassLocal(mEntity); + mWorld.mRigidBodyComponents.setCenterOfMassWorld(mEntity, mWorld.mTransformComponents.getTransform(mEntity) * updatedCenterOfMassLocal); // Update the linear velocity of the center of mass Vector3 linearVelocity = mWorld.mRigidBodyComponents.getAngularVelocity(mEntity); const Vector3& angularVelocity = mWorld.mRigidBodyComponents.getAngularVelocity(mEntity); - const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity); + const Vector3& centerOfMassWorld = mWorld.mRigidBodyComponents.getCenterOfMassWorld(mEntity); linearVelocity += angularVelocity.cross(centerOfMassWorld - oldCenterOfMass); mWorld.mRigidBodyComponents.setLinearVelocity(mEntity, linearVelocity); @@ -313,14 +308,14 @@ void RigidBody::setMass(decimal mass) { if (mWorld.mRigidBodyComponents.getBodyType(mEntity) != BodyType::DYNAMIC) return; - mWorld.mDynamicsComponents.setInitMass(mEntity, mass); + mWorld.mRigidBodyComponents.setInitMass(mEntity, mass); - if (mWorld.mDynamicsComponents.getInitMass(mEntity) > decimal(0.0)) { - mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity)); + if (mWorld.mRigidBodyComponents.getInitMass(mEntity) > decimal(0.0)) { + mWorld.mRigidBodyComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mRigidBodyComponents.getInitMass(mEntity)); } else { - mWorld.mDynamicsComponents.setInitMass(mEntity, decimal(1.0)); - mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0)); + mWorld.mRigidBodyComponents.setInitMass(mEntity, decimal(1.0)); + mWorld.mRigidBodyComponents.setMassInverse(mEntity, decimal(1.0)); } RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, @@ -368,8 +363,8 @@ void RigidBody::updateInertiaTensorInverseWorld() { // TODO : Make sure we do this in a system Matrix3x3 orientation = mWorld.mTransformComponents.getTransform(mEntity).getOrientation().getMatrix(); - const Matrix3x3& inverseInertiaLocalTensor = mWorld.mDynamicsComponents.getInertiaTensorLocalInverse(mEntity); - mWorld.mDynamicsComponents.setInverseInertiaTensorWorld(mEntity, orientation * inverseInertiaLocalTensor * orientation.getTranspose()); + const Matrix3x3& inverseInertiaLocalTensor = mWorld.mRigidBodyComponents.getInertiaTensorLocalInverse(mEntity); + mWorld.mRigidBodyComponents.setInverseInertiaTensorWorld(mEntity, orientation * inverseInertiaLocalTensor * orientation.getTranspose()); } // Add a collision shape to the body. @@ -470,7 +465,7 @@ void RigidBody::removeCollisionShape(ProxyShape* proxyShape) { * @param isEnabled True if you want the gravity to be applied to this body */ void RigidBody::enableGravity(bool isEnabled) { - mWorld.mDynamicsComponents.setIsGravityEnabled(mEntity, isEnabled); + mWorld.mRigidBodyComponents.setIsGravityEnabled(mEntity, isEnabled); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, "Body " + std::to_string(mEntity.id) + ": Set isGravityEnabled=" + @@ -484,7 +479,7 @@ void RigidBody::enableGravity(bool isEnabled) { */ void RigidBody::setLinearDamping(decimal linearDamping) { assert(linearDamping >= decimal(0.0)); - mWorld.mDynamicsComponents.setLinearDamping(mEntity, linearDamping); + mWorld.mRigidBodyComponents.setLinearDamping(mEntity, linearDamping); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, "Body " + std::to_string(mEntity.id) + ": Set linearDamping=" + std::to_string(linearDamping)); @@ -497,7 +492,7 @@ void RigidBody::setLinearDamping(decimal linearDamping) { */ void RigidBody::setAngularDamping(decimal angularDamping) { assert(angularDamping >= decimal(0.0)); - mWorld.mDynamicsComponents.setAngularDamping(mEntity, angularDamping); + mWorld.mRigidBodyComponents.setAngularDamping(mEntity, angularDamping); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, "Body " + std::to_string(mEntity.id) + ": Set angularDamping=" + std::to_string(angularDamping)); @@ -510,8 +505,8 @@ void RigidBody::updateTransformWithCenterOfMass() { // Translate the body according to the translation of the center of mass position Transform& transform = mWorld.mTransformComponents.getTransform(mEntity); - const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity); - const Vector3& centerOfMassLocal = mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity); + const Vector3& centerOfMassWorld = mWorld.mRigidBodyComponents.getCenterOfMassWorld(mEntity); + const Vector3& centerOfMassLocal = mWorld.mRigidBodyComponents.getCenterOfMassLocal(mEntity); transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal); } @@ -577,16 +572,16 @@ void RigidBody::setAngularVelocity(const Vector3& angularVelocity) { */ void RigidBody::setTransform(const Transform& transform) { - const Vector3 oldCenterOfMass = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity); + const Vector3 oldCenterOfMass = mWorld.mRigidBodyComponents.getCenterOfMassWorld(mEntity); // Compute the new center of mass in world-space coordinates - const Vector3& centerOfMassLocal = mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity); - mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform * centerOfMassLocal); + const Vector3& centerOfMassLocal = mWorld.mRigidBodyComponents.getCenterOfMassLocal(mEntity); + mWorld.mRigidBodyComponents.setCenterOfMassWorld(mEntity, transform * centerOfMassLocal); // Update the linear velocity of the center of mass Vector3 linearVelocity = mWorld.mRigidBodyComponents.getLinearVelocity(mEntity); const Vector3& angularVelocity = mWorld.mRigidBodyComponents.getAngularVelocity(mEntity); - const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity); + const Vector3& centerOfMassWorld = mWorld.mRigidBodyComponents.getCenterOfMassWorld(mEntity); linearVelocity += angularVelocity.cross(centerOfMassWorld - oldCenterOfMass); mWorld.mRigidBodyComponents.setLinearVelocity(mEntity, linearVelocity); @@ -606,11 +601,11 @@ void RigidBody::setTransform(const Transform& transform) { // the collision shapes attached to the body. void RigidBody::recomputeMassInformation() { - mWorld.mDynamicsComponents.setInitMass(mEntity, decimal(0.0)); - mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(0.0)); - if (!mIsInertiaTensorSetByUser) mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero()); - if (!mIsInertiaTensorSetByUser) mWorld.mDynamicsComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero()); - if (!mIsCenterOfMassSetByUser) mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, Vector3::zero()); + mWorld.mRigidBodyComponents.setInitMass(mEntity, decimal(0.0)); + mWorld.mRigidBodyComponents.setMassInverse(mEntity, decimal(0.0)); + if (!mIsInertiaTensorSetByUser) mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero()); + if (!mIsInertiaTensorSetByUser) mWorld.mRigidBodyComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero()); + if (!mIsCenterOfMassSetByUser) mWorld.mRigidBodyComponents.setCenterOfMassLocal(mEntity, Vector3::zero()); Matrix3x3 inertiaTensorLocal; inertiaTensorLocal.setToZero(); @@ -619,7 +614,7 @@ void RigidBody::recomputeMassInformation() { // If it is a STATIC or a KINEMATIC body BodyType type = mWorld.mRigidBodyComponents.getBodyType(mEntity); if (type == BodyType::STATIC || type == BodyType::KINEMATIC) { - mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform.getPosition()); + mWorld.mRigidBodyComponents.setCenterOfMassWorld(mEntity, transform.getPosition()); return; } @@ -629,30 +624,30 @@ void RigidBody::recomputeMassInformation() { const List& proxyShapesEntities = mWorld.mCollisionBodyComponents.getProxyShapes(mEntity); for (uint i=0; i < proxyShapesEntities.size(); i++) { ProxyShape* proxyShape = mWorld.mProxyShapesComponents.getProxyShape(proxyShapesEntities[i]); - mWorld.mDynamicsComponents.setInitMass(mEntity, mWorld.mDynamicsComponents.getInitMass(mEntity) + proxyShape->getMass()); + mWorld.mRigidBodyComponents.setInitMass(mEntity, mWorld.mRigidBodyComponents.getInitMass(mEntity) + proxyShape->getMass()); if (!mIsCenterOfMassSetByUser) { - mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity) + + mWorld.mRigidBodyComponents.setCenterOfMassLocal(mEntity, mWorld.mRigidBodyComponents.getCenterOfMassLocal(mEntity) + proxyShape->getLocalToBodyTransform().getPosition() * proxyShape->getMass()); } } - if (mWorld.mDynamicsComponents.getInitMass(mEntity) > decimal(0.0)) { - mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity)); + if (mWorld.mRigidBodyComponents.getInitMass(mEntity) > decimal(0.0)) { + mWorld.mRigidBodyComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mRigidBodyComponents.getInitMass(mEntity)); } else { - mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform.getPosition()); + mWorld.mRigidBodyComponents.setCenterOfMassWorld(mEntity, transform.getPosition()); return; } // Compute the center of mass - const Vector3 oldCenterOfMass = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity); + const Vector3 oldCenterOfMass = mWorld.mRigidBodyComponents.getCenterOfMassWorld(mEntity); if (!mIsCenterOfMassSetByUser) { - mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity) * mWorld.mDynamicsComponents.getMassInverse(mEntity)); + mWorld.mRigidBodyComponents.setCenterOfMassLocal(mEntity, mWorld.mRigidBodyComponents.getCenterOfMassLocal(mEntity) * mWorld.mRigidBodyComponents.getMassInverse(mEntity)); } - mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform * mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity)); + mWorld.mRigidBodyComponents.setCenterOfMassWorld(mEntity, transform * mWorld.mRigidBodyComponents.getCenterOfMassLocal(mEntity)); if (!mIsInertiaTensorSetByUser) { @@ -673,7 +668,7 @@ void RigidBody::recomputeMassInformation() { // Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape // center into a inertia tensor w.r.t to the body origin. - Vector3 offset = shapeTransform.getPosition() - mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity); + Vector3 offset = shapeTransform.getPosition() - mWorld.mRigidBodyComponents.getCenterOfMassLocal(mEntity); decimal offsetSquare = offset.lengthSquare(); Matrix3x3 offsetMatrix; offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0)); @@ -688,7 +683,7 @@ void RigidBody::recomputeMassInformation() { } // Compute the local inverse inertia tensor - mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, inertiaTensorLocal.getInverse()); + mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, inertiaTensorLocal.getInverse()); } // Update the world inverse inertia tensor @@ -697,7 +692,7 @@ void RigidBody::recomputeMassInformation() { // Update the linear velocity of the center of mass Vector3 linearVelocity = mWorld.mRigidBodyComponents.getLinearVelocity(mEntity); Vector3 angularVelocity = mWorld.mRigidBodyComponents.getAngularVelocity(mEntity); - linearVelocity += angularVelocity.cross(mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity) - oldCenterOfMass); + linearVelocity += angularVelocity.cross(mWorld.mRigidBodyComponents.getCenterOfMassWorld(mEntity) - oldCenterOfMass); mWorld.mRigidBodyComponents.setLinearVelocity(mEntity, linearVelocity); } @@ -722,7 +717,7 @@ Vector3 RigidBody::getAngularVelocity() const { * @return True if the gravity is applied to the body */ bool RigidBody::isGravityEnabled() const { - return mWorld.mDynamicsComponents.getIsGravityEnabled(mEntity); + return mWorld.mRigidBodyComponents.getIsGravityEnabled(mEntity); } // Apply an external torque to the body. @@ -744,8 +739,8 @@ void RigidBody::applyTorque(const Vector3& torque) { } // Add the torque - const Vector3& externalTorque = mWorld.mDynamicsComponents.getExternalTorque(mEntity); - mWorld.mDynamicsComponents.setExternalTorque(mEntity, externalTorque + torque); + const Vector3& externalTorque = mWorld.mRigidBodyComponents.getExternalTorque(mEntity); + mWorld.mRigidBodyComponents.setExternalTorque(mEntity, externalTorque + torque); } // Set the variable to know whether or not the body is sleeping @@ -779,8 +774,8 @@ void RigidBody::setIsSleeping(bool isSleeping) { mWorld.mRigidBodyComponents.setLinearVelocity(mEntity, Vector3::zero()); mWorld.mRigidBodyComponents.setAngularVelocity(mEntity, Vector3::zero()); - mWorld.mDynamicsComponents.setExternalForce(mEntity, Vector3::zero()); - mWorld.mDynamicsComponents.setExternalTorque(mEntity, Vector3::zero()); + mWorld.mRigidBodyComponents.setExternalForce(mEntity, Vector3::zero()); + mWorld.mRigidBodyComponents.setExternalTorque(mEntity, Vector3::zero()); } RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 0a9c45c9..f676755a 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -90,7 +90,7 @@ class RigidBody : public CollisionBody { // -------------------- Methods -------------------- // /// Constructor - RigidBody(const Transform& transform, CollisionWorld& world, Entity entity); + RigidBody(CollisionWorld& world, Entity entity); /// Destructor virtual ~RigidBody() override; diff --git a/src/components/DynamicsComponents.cpp b/src/components/DynamicsComponents.cpp deleted file mode 100644 index de9c78f5..00000000 --- a/src/components/DynamicsComponents.cpp +++ /dev/null @@ -1,289 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2018 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "DynamicsComponents.h" -#include "engine/EntityManager.h" -#include -#include - -// We want to use the ReactPhysics3D namespace -using namespace reactphysics3d; - -// Constructor -DynamicsComponents::DynamicsComponents(MemoryAllocator& allocator) - :Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + - sizeof(Vector3) + sizeof(Vector3) + sizeof(decimal) + - sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(Matrix3x3) + sizeof(Matrix3x3) + - sizeof(Vector3) + sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) + sizeof(bool) + - sizeof(bool)) { - - // Allocate memory for the components data - allocate(INIT_NB_ALLOCATED_COMPONENTS); -} - -// Allocate memory for a given number of components -void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { - - assert(nbComponentsToAllocate > mNbAllocatedComponents); - - // Size for the data of a single component (in bytes) - const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize; - - // Allocate memory - void* newBuffer = mMemoryAllocator.allocate(totalSizeBytes); - assert(newBuffer != nullptr); - - // New pointers to components data - Entity* newBodies = static_cast(newBuffer); - Vector3* newConstrainedLinearVelocities = reinterpret_cast(newBodies + nbComponentsToAllocate); - Vector3* newConstrainedAngularVelocities = reinterpret_cast(newConstrainedLinearVelocities + nbComponentsToAllocate); - Vector3* newSplitLinearVelocities = reinterpret_cast(newConstrainedAngularVelocities + nbComponentsToAllocate); - Vector3* newSplitAngularVelocities = reinterpret_cast(newSplitLinearVelocities + nbComponentsToAllocate); - Vector3* newExternalForces = reinterpret_cast(newSplitAngularVelocities + nbComponentsToAllocate); - Vector3* newExternalTorques = reinterpret_cast(newExternalForces + nbComponentsToAllocate); - decimal* newLinearDampings = reinterpret_cast(newExternalTorques + nbComponentsToAllocate); - decimal* newAngularDampings = reinterpret_cast(newLinearDampings + nbComponentsToAllocate); - decimal* newInitMasses = reinterpret_cast(newAngularDampings + nbComponentsToAllocate); - decimal* newInverseMasses = reinterpret_cast(newInitMasses + nbComponentsToAllocate); - Matrix3x3* newInertiaTensorLocalInverses = reinterpret_cast(newInverseMasses + nbComponentsToAllocate); - Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast(newInertiaTensorLocalInverses + nbComponentsToAllocate); - Vector3* newConstrainedPositions = reinterpret_cast(newInertiaTensorWorldInverses + nbComponentsToAllocate); - Quaternion* newConstrainedOrientations = reinterpret_cast(newConstrainedPositions + nbComponentsToAllocate); - Vector3* newCentersOfMassLocal = reinterpret_cast(newConstrainedOrientations + nbComponentsToAllocate); - Vector3* newCentersOfMassWorld = reinterpret_cast(newCentersOfMassLocal + nbComponentsToAllocate); - bool* newIsGravityEnabled = reinterpret_cast(newCentersOfMassWorld + nbComponentsToAllocate); - bool* newIsAlreadyInIsland = reinterpret_cast(newIsGravityEnabled + nbComponentsToAllocate); - - // If there was already components before - if (mNbComponents > 0) { - - // Copy component data from the previous buffer to the new one - memcpy(newBodies, mBodies, mNbComponents * sizeof(Entity)); - memcpy(newConstrainedLinearVelocities, mConstrainedLinearVelocities, mNbComponents * sizeof(Vector3)); - memcpy(newConstrainedAngularVelocities, mConstrainedAngularVelocities, mNbComponents * sizeof(Vector3)); - memcpy(newSplitLinearVelocities, mSplitLinearVelocities, mNbComponents * sizeof(Vector3)); - memcpy(newSplitAngularVelocities, mSplitAngularVelocities, mNbComponents * sizeof(Vector3)); - memcpy(newExternalForces, mExternalForces, mNbComponents * sizeof(Vector3)); - memcpy(newExternalTorques, mExternalTorques, mNbComponents * sizeof(Vector3)); - memcpy(newLinearDampings, mLinearDampings, mNbComponents * sizeof(decimal)); - memcpy(newAngularDampings, mAngularDampings, mNbComponents * sizeof(decimal)); - memcpy(newInitMasses, mInitMasses, mNbComponents * sizeof(decimal)); - memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal)); - memcpy(newInertiaTensorLocalInverses, mInverseInertiaTensorsLocal, mNbComponents * sizeof(Matrix3x3)); - memcpy(newInertiaTensorWorldInverses, mInverseInertiaTensorsWorld, mNbComponents * sizeof(Matrix3x3)); - memcpy(newConstrainedPositions, mConstrainedPositions, mNbComponents * sizeof(Vector3)); - memcpy(newConstrainedOrientations, mConstrainedOrientations, mNbComponents * sizeof(Quaternion)); - memcpy(newCentersOfMassLocal, mCentersOfMassLocal, mNbComponents * sizeof(Vector3)); - memcpy(newCentersOfMassWorld, mCentersOfMassWorld, mNbComponents * sizeof(Vector3)); - memcpy(newIsGravityEnabled, mIsGravityEnabled, mNbComponents * sizeof(bool)); - memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool)); - - // Deallocate previous memory - mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); - } - - mBuffer = newBuffer; - mBodies = newBodies; - mConstrainedLinearVelocities = newConstrainedLinearVelocities; - mConstrainedAngularVelocities = newConstrainedAngularVelocities; - mSplitLinearVelocities = newSplitLinearVelocities; - mSplitAngularVelocities = newSplitAngularVelocities; - mExternalForces = newExternalForces; - mExternalTorques = newExternalTorques; - mLinearDampings = newLinearDampings; - mAngularDampings = newAngularDampings; - mInitMasses = newInitMasses; - mInverseMasses = newInverseMasses; - mInverseInertiaTensorsLocal = newInertiaTensorLocalInverses; - mInverseInertiaTensorsWorld = newInertiaTensorWorldInverses; - mConstrainedPositions = newConstrainedPositions; - mConstrainedOrientations = newConstrainedOrientations; - mCentersOfMassLocal = newCentersOfMassLocal; - mCentersOfMassWorld = newCentersOfMassWorld; - mIsGravityEnabled = newIsGravityEnabled; - mIsAlreadyInIsland = newIsAlreadyInIsland; - - mNbAllocatedComponents = nbComponentsToAllocate; -} - -// Add a component -void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const DynamicsComponent& component) { - - // Prepare to add new component (allocate memory if necessary and compute insertion index) - uint32 index = prepareAddComponent(isSleeping); - - // Insert the new component data - new (mBodies + index) Entity(bodyEntity); - new (mConstrainedLinearVelocities + index) Vector3(0, 0, 0); - new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0); - new (mSplitLinearVelocities + index) Vector3(0, 0, 0); - new (mSplitAngularVelocities + index) Vector3(0, 0, 0); - new (mExternalForces + index) Vector3(0, 0, 0); - new (mExternalTorques + index) Vector3(0, 0, 0); - mLinearDampings[index] = decimal(0.0); - mAngularDampings[index] = decimal(0.0); - mInitMasses[index] = decimal(1.0); - mInverseMasses[index] = decimal(1.0); - new (mInverseInertiaTensorsLocal + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); - new (mInverseInertiaTensorsWorld + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); - new (mConstrainedPositions + index) Vector3(0, 0, 0); - new (mConstrainedOrientations + index) Quaternion(0, 0, 0, 1); - new (mCentersOfMassLocal + index) Vector3(0, 0, 0); - new (mCentersOfMassWorld + index) Vector3(component.worldPosition); - mIsGravityEnabled[index] = true; - mIsAlreadyInIsland[index] = false; - - // Map the entity with the new component lookup index - mMapEntityToComponentIndex.add(Pair(bodyEntity, index)); - - mNbComponents++; - - assert(mDisabledStartIndex <= mNbComponents); - assert(mNbComponents == static_cast(mMapEntityToComponentIndex.size())); -} - -// Move a component from a source to a destination index in the components array -// The destination location must contain a constructed object -void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) { - - const Entity entity = mBodies[srcIndex]; - - // Copy the data of the source component to the destination location - new (mBodies + destIndex) Entity(mBodies[srcIndex]); - new (mConstrainedLinearVelocities + destIndex) Vector3(mConstrainedLinearVelocities[srcIndex]); - new (mConstrainedAngularVelocities + destIndex) Vector3(mConstrainedAngularVelocities[srcIndex]); - new (mSplitLinearVelocities + destIndex) Vector3(mSplitLinearVelocities[srcIndex]); - new (mSplitAngularVelocities + destIndex) Vector3(mSplitAngularVelocities[srcIndex]); - new (mExternalForces + destIndex) Vector3(mExternalForces[srcIndex]); - new (mExternalTorques + destIndex) Vector3(mExternalTorques[srcIndex]); - mLinearDampings[destIndex] = mLinearDampings[srcIndex]; - mAngularDampings[destIndex] = mAngularDampings[srcIndex]; - mInitMasses[destIndex] = mInitMasses[srcIndex]; - mInverseMasses[destIndex] = mInverseMasses[srcIndex]; - new (mInverseInertiaTensorsLocal + destIndex) Matrix3x3(mInverseInertiaTensorsLocal[srcIndex]); - new (mInverseInertiaTensorsWorld + destIndex) Matrix3x3(mInverseInertiaTensorsWorld[srcIndex]); - new (mConstrainedPositions + destIndex) Vector3(mConstrainedPositions[srcIndex]); - new (mConstrainedOrientations + destIndex) Quaternion(mConstrainedOrientations[srcIndex]); - new (mCentersOfMassLocal + destIndex) Vector3(mCentersOfMassLocal[srcIndex]); - new (mCentersOfMassWorld + destIndex) Vector3(mCentersOfMassWorld[srcIndex]); - mIsGravityEnabled[destIndex] = mIsGravityEnabled[srcIndex]; - mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex]; - - // Destroy the source component - destroyComponent(srcIndex); - - assert(!mMapEntityToComponentIndex.containsKey(entity)); - - // Update the entity to component index mapping - mMapEntityToComponentIndex.add(Pair(entity, destIndex)); - - assert(mMapEntityToComponentIndex[mBodies[destIndex]] == destIndex); -} - -// Swap two components in the array -void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) { - - assert(mMapEntityToComponentIndex[mBodies[index1]] == index1); - assert(mMapEntityToComponentIndex[mBodies[index2]] == index2); - - // Copy component 1 data - Entity entity1(mBodies[index1]); - Vector3 constrainedLinearVelocity1(mConstrainedLinearVelocities[index1]); - Vector3 constrainedAngularVelocity1(mConstrainedAngularVelocities[index1]); - Vector3 splitLinearVelocity1(mSplitLinearVelocities[index1]); - Vector3 splitAngularVelocity1(mSplitAngularVelocities[index1]); - Vector3 externalForce1(mExternalForces[index1]); - Vector3 externalTorque1(mExternalTorques[index1]); - decimal linearDamping1 = mLinearDampings[index1]; - decimal angularDamping1 = mAngularDampings[index1]; - decimal initMass1 = mInitMasses[index1]; - decimal inverseMass1 = mInverseMasses[index1]; - Matrix3x3 inertiaTensorLocalInverse1 = mInverseInertiaTensorsLocal[index1]; - Matrix3x3 inertiaTensorWorldInverse1 = mInverseInertiaTensorsWorld[index1]; - Vector3 constrainedPosition1 = mConstrainedPositions[index1]; - Quaternion constrainedOrientation1 = mConstrainedOrientations[index1]; - Vector3 centerOfMassLocal1 = mCentersOfMassLocal[index1]; - Vector3 centerOfMassWorld1 = mCentersOfMassWorld[index1]; - bool isGravityEnabled1 = mIsGravityEnabled[index1]; - bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1]; - - // Destroy component 1 - destroyComponent(index1); - - moveComponentToIndex(index2, index1); - - // Reconstruct component 1 at component 2 location - new (mBodies + index2) Entity(entity1); - new (mConstrainedLinearVelocities + index2) Vector3(constrainedLinearVelocity1); - new (mConstrainedAngularVelocities + index2) Vector3(constrainedAngularVelocity1); - new (mSplitLinearVelocities + index2) Vector3(splitLinearVelocity1); - new (mSplitAngularVelocities + index2) Vector3(splitAngularVelocity1); - new (mExternalForces + index2) Vector3(externalForce1); - new (mExternalTorques + index2) Vector3(externalTorque1); - mLinearDampings[index2] = linearDamping1; - mAngularDampings[index2] = angularDamping1; - mInitMasses[index2] = initMass1; - mInverseMasses[index2] = inverseMass1; - mInverseInertiaTensorsLocal[index2] = inertiaTensorLocalInverse1; - mInverseInertiaTensorsWorld[index2] = inertiaTensorWorldInverse1; - mConstrainedPositions[index2] = constrainedPosition1; - mConstrainedOrientations[index2] = constrainedOrientation1; - mCentersOfMassLocal[index2] = centerOfMassLocal1; - mCentersOfMassWorld[index2] = centerOfMassWorld1; - mIsGravityEnabled[index2] = isGravityEnabled1; - mIsAlreadyInIsland[index2] = isAlreadyInIsland1; - - // Update the entity to component index mapping - mMapEntityToComponentIndex.add(Pair(entity1, index2)); - - assert(mMapEntityToComponentIndex[mBodies[index1]] == index1); - assert(mMapEntityToComponentIndex[mBodies[index2]] == index2); - assert(mNbComponents == static_cast(mMapEntityToComponentIndex.size())); -} - -// Destroy a component at a given index -void DynamicsComponents::destroyComponent(uint32 index) { - - Components::destroyComponent(index); - - assert(mMapEntityToComponentIndex[mBodies[index]] == index); - - mMapEntityToComponentIndex.remove(mBodies[index]); - - mBodies[index].~Entity(); - mConstrainedLinearVelocities[index].~Vector3(); - mConstrainedAngularVelocities[index].~Vector3(); - mSplitLinearVelocities[index].~Vector3(); - mSplitAngularVelocities[index].~Vector3(); - mExternalForces[index].~Vector3(); - mExternalTorques[index].~Vector3(); - mInverseInertiaTensorsLocal[index].~Matrix3x3(); - mInverseInertiaTensorsWorld[index].~Matrix3x3(); - mConstrainedPositions[index].~Vector3(); - mConstrainedOrientations[index].~Quaternion(); - mCentersOfMassLocal[index].~Vector3(); - mCentersOfMassWorld[index].~Vector3(); -} diff --git a/src/components/DynamicsComponents.h b/src/components/DynamicsComponents.h deleted file mode 100644 index 820d4c44..00000000 --- a/src/components/DynamicsComponents.h +++ /dev/null @@ -1,560 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2018 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_DYNAMICS_COMPONENTS_H -#define REACTPHYSICS3D_DYNAMICS_COMPONENTS_H - -// Libraries -#include "mathematics/Transform.h" -#include "engine/Entity.h" -#include "components/Components.h" -#include "mathematics/Matrix3x3.h" -#include "containers/Map.h" - -// ReactPhysics3D namespace -namespace reactphysics3d { - -// Class declarations -class MemoryAllocator; -class EntityManager; - -// Class DynamicsComponents -/** - * This class represent the component of the ECS that contains the variables concerning dynamics - * like velocities. A rigid body that is not static always has a dynamics component. A rigid body - * that is static does not have one because it is not simulated by dynamics. - */ -class DynamicsComponents : public Components { - - private: - - // -------------------- Attributes -------------------- // - - /// Array of body entities of each component - Entity* mBodies; - - /// Array with the constrained linear velocity of each component - Vector3* mConstrainedLinearVelocities; - - /// Array with the constrained angular velocity of each component - Vector3* mConstrainedAngularVelocities; - - /// Array with the split linear velocity of each component - Vector3* mSplitLinearVelocities; - - /// Array with the split angular velocity of each component - Vector3* mSplitAngularVelocities; - - /// Array with the external force of each component - Vector3* mExternalForces; - - /// Array with the external torque of each component - Vector3* mExternalTorques; - - /// Array with the linear damping factor of each component - decimal* mLinearDampings; - - /// Array with the angular damping factor of each component - decimal* mAngularDampings; - - /// Array with the initial mass of each component - decimal* mInitMasses; - - /// Array with the inverse mass of each component - decimal* mInverseMasses; - - /// Array with the inverse of the inertia tensor of each component - Matrix3x3* mInverseInertiaTensorsLocal; - - /// Array with the inverse of the world inertia tensor of each component - Matrix3x3* mInverseInertiaTensorsWorld; - - /// Array with the constrained position of each component (for position error correction) - Vector3* mConstrainedPositions; - - /// Array of constrained orientation for each component (for position error correction) - Quaternion* mConstrainedOrientations; - - /// Array of center of mass of each component (in local-space coordinates) - Vector3* mCentersOfMassLocal; - - /// Array of center of mass of each component (in world-space coordinates) - Vector3* mCentersOfMassWorld; - - /// True if the gravity needs to be applied to this component - bool* mIsGravityEnabled; - - /// Array with the boolean value to know if the body has already been added into an island - bool* mIsAlreadyInIsland; - - // -------------------- Methods -------------------- // - - /// Allocate memory for a given number of components - virtual void allocate(uint32 nbComponentsToAllocate) override; - - /// Destroy a component at a given index - virtual void destroyComponent(uint32 index) override; - - /// Move a component from a source to a destination index in the components array - virtual void moveComponentToIndex(uint32 srcIndex, uint32 destIndex) override; - - /// Swap two components in the array - virtual void swapComponents(uint32 index1, uint32 index2) override; - - public: - - /// Structure for the data of a transform component - struct DynamicsComponent { - - const Vector3& worldPosition; - - /// Constructor - DynamicsComponent(const Vector3& worldPosition) - : worldPosition(worldPosition) { - - } - }; - - // -------------------- Methods -------------------- // - - /// Constructor - DynamicsComponents(MemoryAllocator& allocator); - - /// Destructor - virtual ~DynamicsComponents() override = default; - - /// Add a component - void addComponent(Entity bodyEntity, bool isSleeping, const DynamicsComponent& component); - - /// Return the constrained linear velocity of an entity - const Vector3& getConstrainedLinearVelocity(Entity bodyEntity) const; - - /// Return the constrained angular velocity of an entity - const Vector3& getConstrainedAngularVelocity(Entity bodyEntity) const; - - /// Return the split linear velocity of an entity - const Vector3& getSplitLinearVelocity(Entity bodyEntity) const; - - /// Return the split angular velocity of an entity - const Vector3& getSplitAngularVelocity(Entity bodyEntity) const; - - /// Return the external force of an entity - const Vector3& getExternalForce(Entity bodyEntity) const; - - /// Return the external torque of an entity - const Vector3& getExternalTorque(Entity bodyEntity) const; - - /// Return the linear damping factor of an entity - decimal getLinearDamping(Entity bodyEntity) const; - - /// Return the angular damping factor of an entity - decimal getAngularDamping(Entity bodyEntity) const; - - /// Return the initial mass of an entity - decimal getInitMass(Entity bodyEntity) const; - - /// Return the mass inverse of an entity - decimal getMassInverse(Entity bodyEntity) const; - - /// Return the inverse local inertia tensor of an entity - const Matrix3x3& getInertiaTensorLocalInverse(Entity bodyEntity); - - /// Return the inverse world inertia tensor of an entity - const Matrix3x3& getInertiaTensorWorldInverse(Entity bodyEntity); - - /// Return the constrained position of an entity - const Vector3& getConstrainedPosition(Entity bodyEntity); - - /// Return the constrained orientation of an entity - const Quaternion& getConstrainedOrientation(Entity bodyEntity); - - /// Return the local center of mass of an entity - const Vector3& getCenterOfMassLocal(Entity bodyEntity); - - /// Return the world center of mass of an entity - const Vector3& getCenterOfMassWorld(Entity bodyEntity); - - /// Return true if gravity is enabled for this entity - bool getIsGravityEnabled(Entity bodyEntity) const; - - /// Return true if the entity is already in an island - bool getIsAlreadyInIsland(Entity bodyEntity) const; - - /// Set the constrained linear velocity of an entity - void setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity); - - /// Set the constrained angular velocity of an entity - void setConstrainedAngularVelocity(Entity bodyEntity, const Vector3& constrainedAngularVelocity); - - /// Set the split linear velocity of an entity - void setSplitLinearVelocity(Entity bodyEntity, const Vector3& splitLinearVelocity); - - /// Set the split angular velocity of an entity - void setSplitAngularVelocity(Entity bodyEntity, const Vector3& splitAngularVelocity); - - /// Set the external force of an entity - void setExternalForce(Entity bodyEntity, const Vector3& externalForce); - - /// Set the external force of an entity - void setExternalTorque(Entity bodyEntity, const Vector3& externalTorque); - - /// Set the linear damping factor of an entity - void setLinearDamping(Entity bodyEntity, decimal linearDamping); - - /// Set the angular damping factor of an entity - void setAngularDamping(Entity bodyEntity, decimal angularDamping); - - /// Set the initial mass of an entity - void setInitMass(Entity bodyEntity, decimal initMass); - - /// Set the inverse mass of an entity - void setMassInverse(Entity bodyEntity, decimal inverseMass); - - /// Set the inverse local inertia tensor of an entity - void setInverseInertiaTensorLocal(Entity bodyEntity, const Matrix3x3& inertiaTensorLocalInverse); - - /// Set the inverse world inertia tensor of an entity - void setInverseInertiaTensorWorld(Entity bodyEntity, const Matrix3x3& inertiaTensorWorldInverse); - - /// Set the constrained position of an entity - void setConstrainedPosition(Entity bodyEntity, const Vector3& constrainedPosition); - - /// Set the constrained orientation of an entity - void setConstrainedOrientation(Entity bodyEntity, const Quaternion& constrainedOrientation); - - /// Set the local center of mass of an entity - void setCenterOfMassLocal(Entity bodyEntity, const Vector3& centerOfMassLocal); - - /// Set the world center of mass of an entity - void setCenterOfMassWorld(Entity bodyEntity, const Vector3& centerOfMassWorld); - - /// Set the value to know if the gravity is enabled for this entity - void setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled); - - /// Set the value to know if the entity is already in an island - void setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland); - - // -------------------- Friendship -------------------- // - - friend class BroadPhaseSystem; - friend class DynamicsWorld; - friend class ContactSolver; - friend class BallAndSocketJoint; - friend class FixedJoint; - friend class HingeJoint; - friend class SliderJoint; -}; - -// Return the constrained linear velocity of an entity -inline const Vector3& DynamicsComponents::getConstrainedLinearVelocity(Entity bodyEntity) const { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mConstrainedLinearVelocities[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Return the constrained angular velocity of an entity -inline const Vector3& DynamicsComponents::getConstrainedAngularVelocity(Entity bodyEntity) const { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mConstrainedAngularVelocities[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Return the split linear velocity of an entity -inline const Vector3& DynamicsComponents::getSplitLinearVelocity(Entity bodyEntity) const { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mSplitLinearVelocities[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Return the split angular velocity of an entity -inline const Vector3& DynamicsComponents::getSplitAngularVelocity(Entity bodyEntity) const { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mSplitAngularVelocities[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Return the external force of an entity -inline const Vector3& DynamicsComponents::getExternalForce(Entity bodyEntity) const { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mExternalForces[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Return the external torque of an entity -inline const Vector3& DynamicsComponents::getExternalTorque(Entity bodyEntity) const { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mExternalTorques[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Return the linear damping factor of an entity -inline decimal DynamicsComponents::getLinearDamping(Entity bodyEntity) const { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mLinearDampings[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Return the angular damping factor of an entity -inline decimal DynamicsComponents::getAngularDamping(Entity bodyEntity) const { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mAngularDampings[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Return the initial mass of an entity -inline decimal DynamicsComponents::getInitMass(Entity bodyEntity) const { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mInitMasses[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Return the inverse mass of an entity -inline decimal DynamicsComponents::getMassInverse(Entity bodyEntity) const { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mInverseMasses[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Return the inverse local inertia tensor of an entity -inline const Matrix3x3& DynamicsComponents::getInertiaTensorLocalInverse(Entity bodyEntity) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Return the inverse world inertia tensor of an entity -inline const Matrix3x3& DynamicsComponents::getInertiaTensorWorldInverse(Entity bodyEntity) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Return the constrained position of an entity -inline const Vector3& DynamicsComponents::getConstrainedPosition(Entity bodyEntity) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mConstrainedPositions[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Return the constrained orientation of an entity -inline const Quaternion& DynamicsComponents::getConstrainedOrientation(Entity bodyEntity) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mConstrainedOrientations[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Return the local center of mass of an entity -inline const Vector3& DynamicsComponents::getCenterOfMassLocal(Entity bodyEntity) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mCentersOfMassLocal[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Return the world center of mass of an entity -inline const Vector3& DynamicsComponents::getCenterOfMassWorld(Entity bodyEntity) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mCentersOfMassWorld[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Set the constrained linear velocity of an entity -inline void DynamicsComponents::setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - mConstrainedLinearVelocities[mMapEntityToComponentIndex[bodyEntity]] = constrainedLinearVelocity; -} - -// Set the constrained angular velocity of an entity -inline void DynamicsComponents::setConstrainedAngularVelocity(Entity bodyEntity, const Vector3& constrainedAngularVelocity) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - mConstrainedAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = constrainedAngularVelocity; -} - -// Set the split linear velocity of an entity -inline void DynamicsComponents::setSplitLinearVelocity(Entity bodyEntity, const Vector3& splitLinearVelocity) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - mSplitLinearVelocities[mMapEntityToComponentIndex[bodyEntity]] = splitLinearVelocity; -} - -// Set the split angular velocity of an entity -inline void DynamicsComponents::setSplitAngularVelocity(Entity bodyEntity, const Vector3& splitAngularVelocity) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - mSplitAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = splitAngularVelocity; -} - -// Set the external force of an entity -inline void DynamicsComponents::setExternalForce(Entity bodyEntity, const Vector3& externalForce) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - mExternalForces[mMapEntityToComponentIndex[bodyEntity]] = externalForce; -} - -// Set the external force of an entity -inline void DynamicsComponents::setExternalTorque(Entity bodyEntity, const Vector3& externalTorque) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - mExternalTorques[mMapEntityToComponentIndex[bodyEntity]] = externalTorque; -} - -// Set the linear damping factor of an entity -inline void DynamicsComponents::setLinearDamping(Entity bodyEntity, decimal linearDamping) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - mLinearDampings[mMapEntityToComponentIndex[bodyEntity]] = linearDamping; -} - -// Set the angular damping factor of an entity -inline void DynamicsComponents::setAngularDamping(Entity bodyEntity, decimal angularDamping) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - mAngularDampings[mMapEntityToComponentIndex[bodyEntity]] = angularDamping; -} - -// Set the initial mass of an entity -inline void DynamicsComponents::setInitMass(Entity bodyEntity, decimal initMass) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - mInitMasses[mMapEntityToComponentIndex[bodyEntity]] = initMass; -} - -// Set the mass inverse of an entity -inline void DynamicsComponents::setMassInverse(Entity bodyEntity, decimal inverseMass) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - mInverseMasses[mMapEntityToComponentIndex[bodyEntity]] = inverseMass; -} - -// Set the inverse local inertia tensor of an entity -inline void DynamicsComponents::setInverseInertiaTensorLocal(Entity bodyEntity, const Matrix3x3& inertiaTensorLocalInverse) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorLocalInverse; -} - -// Set the inverse world inertia tensor of an entity -inline void DynamicsComponents::setInverseInertiaTensorWorld(Entity bodyEntity, const Matrix3x3& inertiaTensorWorldInverse) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorWorldInverse; -} - -// Set the constrained position of an entity -inline void DynamicsComponents::setConstrainedPosition(Entity bodyEntity, const Vector3& constrainedPosition) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - mConstrainedPositions[mMapEntityToComponentIndex[bodyEntity]] = constrainedPosition; -} - -// Set the constrained orientation of an entity -inline void DynamicsComponents::setConstrainedOrientation(Entity bodyEntity, const Quaternion& constrainedOrientation) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - mConstrainedOrientations[mMapEntityToComponentIndex[bodyEntity]] = constrainedOrientation; -} - -// Set the local center of mass of an entity -inline void DynamicsComponents::setCenterOfMassLocal(Entity bodyEntity, const Vector3& centerOfMassLocal) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - mCentersOfMassLocal[mMapEntityToComponentIndex[bodyEntity]] = centerOfMassLocal; -} - -// Set the world center of mass of an entity -inline void DynamicsComponents::setCenterOfMassWorld(Entity bodyEntity, const Vector3& centerOfMassWorld) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - mCentersOfMassWorld[mMapEntityToComponentIndex[bodyEntity]] = centerOfMassWorld; -} - -// Return true if gravity is enabled for this entity -inline bool DynamicsComponents::getIsGravityEnabled(Entity bodyEntity) const { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mIsGravityEnabled[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Return true if the entity is already in an island -inline bool DynamicsComponents::getIsAlreadyInIsland(Entity bodyEntity) const { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Set the value to know if the gravity is enabled for this entity -inline void DynamicsComponents::setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - mIsGravityEnabled[mMapEntityToComponentIndex[bodyEntity]] = isGravityEnabled; -} - -// Set the value to know if the entity is already in an island -inline void DynamicsComponents::setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]] = isAlreadyInIsland; -} - -} - -#endif diff --git a/src/components/RigidBodyComponents.cpp b/src/components/RigidBodyComponents.cpp index dfa2fdbf..7ad67fb9 100644 --- a/src/components/RigidBodyComponents.cpp +++ b/src/components/RigidBodyComponents.cpp @@ -37,7 +37,13 @@ using namespace reactphysics3d; RigidBodyComponents::RigidBodyComponents(MemoryAllocator& allocator) :Components(allocator, sizeof(Entity) + sizeof(RigidBody*) + sizeof(bool) + sizeof(bool) + sizeof(decimal) + sizeof(BodyType) + - sizeof(Vector3) + sizeof(Vector3)) { + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + + sizeof(Vector3) + sizeof(decimal) + sizeof(decimal) + + sizeof(decimal) + sizeof(decimal) + sizeof(Matrix3x3) + + sizeof(Matrix3x3) + sizeof(Vector3) + sizeof(Vector3) + + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + + sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) + + sizeof(bool) + sizeof(bool)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -64,6 +70,24 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { BodyType* newBodyTypes = reinterpret_cast(newSleepTimes + nbComponentsToAllocate); Vector3* newLinearVelocities = reinterpret_cast(newBodyTypes + nbComponentsToAllocate); Vector3* newAngularVelocities = reinterpret_cast(newLinearVelocities + nbComponentsToAllocate); + Vector3* newExternalForces = reinterpret_cast(newAngularVelocities + nbComponentsToAllocate); + Vector3* newExternalTorques = reinterpret_cast(newExternalForces + nbComponentsToAllocate); + decimal* newLinearDampings = reinterpret_cast(newExternalTorques + nbComponentsToAllocate); + decimal* newAngularDampings = reinterpret_cast(newLinearDampings + nbComponentsToAllocate); + decimal* newInitMasses = reinterpret_cast(newAngularDampings + nbComponentsToAllocate); + decimal* newInverseMasses = reinterpret_cast(newInitMasses + nbComponentsToAllocate); + Matrix3x3* newInertiaTensorLocalInverses = reinterpret_cast(newInverseMasses + nbComponentsToAllocate); + Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast(newInertiaTensorLocalInverses + nbComponentsToAllocate); + Vector3* newConstrainedLinearVelocities = reinterpret_cast(newInertiaTensorWorldInverses + nbComponentsToAllocate); + Vector3* newConstrainedAngularVelocities = reinterpret_cast(newConstrainedLinearVelocities + nbComponentsToAllocate); + Vector3* newSplitLinearVelocities = reinterpret_cast(newConstrainedAngularVelocities + nbComponentsToAllocate); + Vector3* newSplitAngularVelocities = reinterpret_cast(newSplitLinearVelocities + nbComponentsToAllocate); + Vector3* newConstrainedPositions = reinterpret_cast(newSplitAngularVelocities + nbComponentsToAllocate); + Quaternion* newConstrainedOrientations = reinterpret_cast(newConstrainedPositions + nbComponentsToAllocate); + Vector3* newCentersOfMassLocal = reinterpret_cast(newConstrainedOrientations + nbComponentsToAllocate); + Vector3* newCentersOfMassWorld = reinterpret_cast(newCentersOfMassLocal + nbComponentsToAllocate); + bool* newIsGravityEnabled = reinterpret_cast(newCentersOfMassWorld + nbComponentsToAllocate); + bool* newIsAlreadyInIsland = reinterpret_cast(newIsGravityEnabled + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -77,6 +101,24 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newBodyTypes, mBodyTypes, mNbComponents * sizeof(BodyType)); memcpy(newLinearVelocities, mLinearVelocities, mNbComponents * sizeof(Vector3)); memcpy(newAngularVelocities, mAngularVelocities, mNbComponents * sizeof(Vector3)); + memcpy(newExternalForces, mExternalForces, mNbComponents * sizeof(Vector3)); + memcpy(newExternalTorques, mExternalTorques, mNbComponents * sizeof(Vector3)); + memcpy(newLinearDampings, mLinearDampings, mNbComponents * sizeof(decimal)); + memcpy(newAngularDampings, mAngularDampings, mNbComponents * sizeof(decimal)); + memcpy(newInitMasses, mInitMasses, mNbComponents * sizeof(decimal)); + memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal)); + memcpy(newInertiaTensorLocalInverses, mInverseInertiaTensorsLocal, mNbComponents * sizeof(Matrix3x3)); + memcpy(newInertiaTensorWorldInverses, mInverseInertiaTensorsWorld, mNbComponents * sizeof(Matrix3x3)); + memcpy(newConstrainedLinearVelocities, mConstrainedLinearVelocities, mNbComponents * sizeof(Vector3)); + memcpy(newConstrainedAngularVelocities, mConstrainedAngularVelocities, mNbComponents * sizeof(Vector3)); + memcpy(newSplitLinearVelocities, mSplitLinearVelocities, mNbComponents * sizeof(Vector3)); + memcpy(newSplitAngularVelocities, mSplitAngularVelocities, mNbComponents * sizeof(Vector3)); + memcpy(newConstrainedPositions, mConstrainedPositions, mNbComponents * sizeof(Vector3)); + memcpy(newConstrainedOrientations, mConstrainedOrientations, mNbComponents * sizeof(Quaternion)); + memcpy(newCentersOfMassLocal, mCentersOfMassLocal, mNbComponents * sizeof(Vector3)); + memcpy(newCentersOfMassWorld, mCentersOfMassWorld, mNbComponents * sizeof(Vector3)); + memcpy(newIsGravityEnabled, mIsGravityEnabled, mNbComponents * sizeof(bool)); + memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool)); // Deallocate previous memory mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); @@ -92,6 +134,24 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { mBodyTypes = newBodyTypes; mLinearVelocities = newLinearVelocities; mAngularVelocities = newAngularVelocities; + mExternalForces = newExternalForces; + mExternalTorques = newExternalTorques; + mLinearDampings = newLinearDampings; + mAngularDampings = newAngularDampings; + mInitMasses = newInitMasses; + mInverseMasses = newInverseMasses; + mInverseInertiaTensorsLocal = newInertiaTensorLocalInverses; + mInverseInertiaTensorsWorld = newInertiaTensorWorldInverses; + mConstrainedLinearVelocities = newConstrainedLinearVelocities; + mConstrainedAngularVelocities = newConstrainedAngularVelocities; + mSplitLinearVelocities = newSplitLinearVelocities; + mSplitAngularVelocities = newSplitAngularVelocities; + mConstrainedPositions = newConstrainedPositions; + mConstrainedOrientations = newConstrainedOrientations; + mCentersOfMassLocal = newCentersOfMassLocal; + mCentersOfMassWorld = newCentersOfMassWorld; + mIsGravityEnabled = newIsGravityEnabled; + mIsAlreadyInIsland = newIsAlreadyInIsland; } // Add a component @@ -109,6 +169,24 @@ void RigidBodyComponents::addComponent(Entity bodyEntity, bool isSleeping, const mBodyTypes[index] = component.bodyType; new (mLinearVelocities + index) Vector3(0, 0, 0); new (mAngularVelocities + index) Vector3(0, 0, 0); + new (mExternalForces + index) Vector3(0, 0, 0); + new (mExternalTorques + index) Vector3(0, 0, 0); + mLinearDampings[index] = decimal(0.0); + mAngularDampings[index] = decimal(0.0); + mInitMasses[index] = decimal(1.0); + mInverseMasses[index] = decimal(1.0); + new (mInverseInertiaTensorsLocal + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); + new (mInverseInertiaTensorsWorld + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); + new (mConstrainedLinearVelocities + index) Vector3(0, 0, 0); + new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0); + new (mSplitLinearVelocities + index) Vector3(0, 0, 0); + new (mSplitAngularVelocities + index) Vector3(0, 0, 0); + new (mConstrainedPositions + index) Vector3(0, 0, 0); + new (mConstrainedOrientations + index) Quaternion(0, 0, 0, 1); + new (mCentersOfMassLocal + index) Vector3(0, 0, 0); + new (mCentersOfMassWorld + index) Vector3(component.worldPosition); + mIsGravityEnabled[index] = true; + mIsAlreadyInIsland[index] = false; // Map the entity with the new component lookup index mMapEntityToComponentIndex.add(Pair(bodyEntity, index)); @@ -134,6 +212,24 @@ void RigidBodyComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex mBodyTypes[destIndex] = mBodyTypes[srcIndex]; new (mLinearVelocities + destIndex) Vector3(mLinearVelocities[srcIndex]); new (mAngularVelocities + destIndex) Vector3(mAngularVelocities[srcIndex]); + new (mExternalForces + destIndex) Vector3(mExternalForces[srcIndex]); + new (mExternalTorques + destIndex) Vector3(mExternalTorques[srcIndex]); + mLinearDampings[destIndex] = mLinearDampings[srcIndex]; + mAngularDampings[destIndex] = mAngularDampings[srcIndex]; + mInitMasses[destIndex] = mInitMasses[srcIndex]; + mInverseMasses[destIndex] = mInverseMasses[srcIndex]; + new (mInverseInertiaTensorsLocal + destIndex) Matrix3x3(mInverseInertiaTensorsLocal[srcIndex]); + new (mInverseInertiaTensorsWorld + destIndex) Matrix3x3(mInverseInertiaTensorsWorld[srcIndex]); + new (mConstrainedLinearVelocities + destIndex) Vector3(mConstrainedLinearVelocities[srcIndex]); + new (mConstrainedAngularVelocities + destIndex) Vector3(mConstrainedAngularVelocities[srcIndex]); + new (mSplitLinearVelocities + destIndex) Vector3(mSplitLinearVelocities[srcIndex]); + new (mSplitAngularVelocities + destIndex) Vector3(mSplitAngularVelocities[srcIndex]); + new (mConstrainedPositions + destIndex) Vector3(mConstrainedPositions[srcIndex]); + new (mConstrainedOrientations + destIndex) Quaternion(mConstrainedOrientations[srcIndex]); + new (mCentersOfMassLocal + destIndex) Vector3(mCentersOfMassLocal[srcIndex]); + new (mCentersOfMassWorld + destIndex) Vector3(mCentersOfMassWorld[srcIndex]); + mIsGravityEnabled[destIndex] = mIsGravityEnabled[srcIndex]; + mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex]; // Destroy the source component destroyComponent(srcIndex); @@ -158,6 +254,24 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) { BodyType bodyType1 = mBodyTypes[index1]; Vector3 linearVelocity1(mLinearVelocities[index1]); Vector3 angularVelocity1(mAngularVelocities[index1]); + Vector3 externalForce1(mExternalForces[index1]); + Vector3 externalTorque1(mExternalTorques[index1]); + decimal linearDamping1 = mLinearDampings[index1]; + decimal angularDamping1 = mAngularDampings[index1]; + decimal initMass1 = mInitMasses[index1]; + decimal inverseMass1 = mInverseMasses[index1]; + Matrix3x3 inertiaTensorLocalInverse1 = mInverseInertiaTensorsLocal[index1]; + Matrix3x3 inertiaTensorWorldInverse1 = mInverseInertiaTensorsWorld[index1]; + Vector3 constrainedLinearVelocity1(mConstrainedLinearVelocities[index1]); + Vector3 constrainedAngularVelocity1(mConstrainedAngularVelocities[index1]); + Vector3 splitLinearVelocity1(mSplitLinearVelocities[index1]); + Vector3 splitAngularVelocity1(mSplitAngularVelocities[index1]); + Vector3 constrainedPosition1 = mConstrainedPositions[index1]; + Quaternion constrainedOrientation1 = mConstrainedOrientations[index1]; + Vector3 centerOfMassLocal1 = mCentersOfMassLocal[index1]; + Vector3 centerOfMassWorld1 = mCentersOfMassWorld[index1]; + bool isGravityEnabled1 = mIsGravityEnabled[index1]; + bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1]; // Destroy component 1 destroyComponent(index1); @@ -173,6 +287,24 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) { mBodyTypes[index2] = bodyType1; new (mLinearVelocities + index2) Vector3(linearVelocity1); new (mAngularVelocities + index2) Vector3(angularVelocity1); + new (mExternalForces + index2) Vector3(externalForce1); + new (mExternalTorques + index2) Vector3(externalTorque1); + mLinearDampings[index2] = linearDamping1; + mAngularDampings[index2] = angularDamping1; + mInitMasses[index2] = initMass1; + mInverseMasses[index2] = inverseMass1; + mInverseInertiaTensorsLocal[index2] = inertiaTensorLocalInverse1; + mInverseInertiaTensorsWorld[index2] = inertiaTensorWorldInverse1; + new (mConstrainedLinearVelocities + index2) Vector3(constrainedLinearVelocity1); + new (mConstrainedAngularVelocities + index2) Vector3(constrainedAngularVelocity1); + new (mSplitLinearVelocities + index2) Vector3(splitLinearVelocity1); + new (mSplitAngularVelocities + index2) Vector3(splitAngularVelocity1); + mConstrainedPositions[index2] = constrainedPosition1; + mConstrainedOrientations[index2] = constrainedOrientation1; + mCentersOfMassLocal[index2] = centerOfMassLocal1; + mCentersOfMassWorld[index2] = centerOfMassWorld1; + mIsGravityEnabled[index2] = isGravityEnabled1; + mIsAlreadyInIsland[index2] = isAlreadyInIsland1; // Update the entity to component index mapping mMapEntityToComponentIndex.add(Pair(entity1, index2)); @@ -195,4 +327,16 @@ void RigidBodyComponents::destroyComponent(uint32 index) { mRigidBodies[index] = nullptr; mLinearVelocities[index].~Vector3(); mAngularVelocities[index].~Vector3(); + mExternalForces[index].~Vector3(); + mExternalTorques[index].~Vector3(); + mInverseInertiaTensorsLocal[index].~Matrix3x3(); + mInverseInertiaTensorsWorld[index].~Matrix3x3(); + mConstrainedLinearVelocities[index].~Vector3(); + mConstrainedAngularVelocities[index].~Vector3(); + mSplitLinearVelocities[index].~Vector3(); + mSplitAngularVelocities[index].~Vector3(); + mConstrainedPositions[index].~Vector3(); + mConstrainedOrientations[index].~Quaternion(); + mCentersOfMassLocal[index].~Vector3(); + mCentersOfMassWorld[index].~Vector3(); } diff --git a/src/components/RigidBodyComponents.h b/src/components/RigidBodyComponents.h index 1a69fcb8..d7f4d174 100644 --- a/src/components/RigidBodyComponents.h +++ b/src/components/RigidBodyComponents.h @@ -28,6 +28,7 @@ // Libraries #include "mathematics/Transform.h" +#include "mathematics/Matrix3x3.h" #include "engine/Entity.h" #include "components/Components.h" #include "containers/Map.h" @@ -87,6 +88,60 @@ class RigidBodyComponents : public Components { /// Array with the angular velocity of each component Vector3* mAngularVelocities; + /// Array with the external force of each component + Vector3* mExternalForces; + + /// Array with the external torque of each component + Vector3* mExternalTorques; + + /// Array with the linear damping factor of each component + decimal* mLinearDampings; + + /// Array with the angular damping factor of each component + decimal* mAngularDampings; + + /// Array with the initial mass of each component + decimal* mInitMasses; + + /// Array with the inverse mass of each component + decimal* mInverseMasses; + + /// Array with the inverse of the inertia tensor of each component + Matrix3x3* mInverseInertiaTensorsLocal; + + /// Array with the inverse of the world inertia tensor of each component + Matrix3x3* mInverseInertiaTensorsWorld; + + /// Array with the constrained linear velocity of each component + Vector3* mConstrainedLinearVelocities; + + /// Array with the constrained angular velocity of each component + Vector3* mConstrainedAngularVelocities; + + /// Array with the split linear velocity of each component + Vector3* mSplitLinearVelocities; + + /// Array with the split angular velocity of each component + Vector3* mSplitAngularVelocities; + + /// Array with the constrained position of each component (for position error correction) + Vector3* mConstrainedPositions; + + /// Array of constrained orientation for each component (for position error correction) + Quaternion* mConstrainedOrientations; + + /// Array of center of mass of each component (in local-space coordinates) + Vector3* mCentersOfMassLocal; + + /// Array of center of mass of each component (in world-space coordinates) + Vector3* mCentersOfMassWorld; + + /// True if the gravity needs to be applied to this component + bool* mIsGravityEnabled; + + /// Array with the boolean value to know if the body has already been added into an island + bool* mIsAlreadyInIsland; + // -------------------- Methods -------------------- // /// Allocate memory for a given number of components @@ -108,9 +163,11 @@ class RigidBodyComponents : public Components { RigidBody* body; BodyType bodyType; + const Vector3& worldPosition; /// Constructor - RigidBodyComponent(RigidBody* body, BodyType bodyType) : body(body), bodyType(bodyType) { + RigidBodyComponent(RigidBody* body, BodyType bodyType, const Vector3& worldPosition) + : body(body), bodyType(bodyType), worldPosition(worldPosition) { } }; @@ -164,6 +221,123 @@ class RigidBodyComponents : public Components { /// Set the angular velocity of an entity void setAngularVelocity(Entity bodyEntity, const Vector3& angularVelocity); + + /// Return the external force of an entity + const Vector3& getExternalForce(Entity bodyEntity) const; + + /// Return the external torque of an entity + const Vector3& getExternalTorque(Entity bodyEntity) const; + + /// Return the linear damping factor of an entity + decimal getLinearDamping(Entity bodyEntity) const; + + /// Return the angular damping factor of an entity + decimal getAngularDamping(Entity bodyEntity) const; + + /// Return the initial mass of an entity + decimal getInitMass(Entity bodyEntity) const; + + /// Return the mass inverse of an entity + decimal getMassInverse(Entity bodyEntity) const; + + /// Return the inverse local inertia tensor of an entity + const Matrix3x3& getInertiaTensorLocalInverse(Entity bodyEntity); + + /// Return the inverse world inertia tensor of an entity + const Matrix3x3& getInertiaTensorWorldInverse(Entity bodyEntity); + + /// Set the external force of an entity + void setExternalForce(Entity bodyEntity, const Vector3& externalForce); + + /// Set the external force of an entity + void setExternalTorque(Entity bodyEntity, const Vector3& externalTorque); + + /// Set the linear damping factor of an entity + void setLinearDamping(Entity bodyEntity, decimal linearDamping); + + /// Set the angular damping factor of an entity + void setAngularDamping(Entity bodyEntity, decimal angularDamping); + + /// Set the initial mass of an entity + void setInitMass(Entity bodyEntity, decimal initMass); + + /// Set the inverse mass of an entity + void setMassInverse(Entity bodyEntity, decimal inverseMass); + + /// Set the inverse local inertia tensor of an entity + void setInverseInertiaTensorLocal(Entity bodyEntity, const Matrix3x3& inertiaTensorLocalInverse); + + /// Set the inverse world inertia tensor of an entity + void setInverseInertiaTensorWorld(Entity bodyEntity, const Matrix3x3& inertiaTensorWorldInverse); + + /// Return the constrained linear velocity of an entity + const Vector3& getConstrainedLinearVelocity(Entity bodyEntity) const; + + /// Return the constrained angular velocity of an entity + const Vector3& getConstrainedAngularVelocity(Entity bodyEntity) const; + + /// Return the split linear velocity of an entity + const Vector3& getSplitLinearVelocity(Entity bodyEntity) const; + + /// Return the split angular velocity of an entity + const Vector3& getSplitAngularVelocity(Entity bodyEntity) const; + + /// Return the constrained position of an entity + const Vector3& getConstrainedPosition(Entity bodyEntity); + + /// Return the constrained orientation of an entity + const Quaternion& getConstrainedOrientation(Entity bodyEntity); + + /// Return the local center of mass of an entity + const Vector3& getCenterOfMassLocal(Entity bodyEntity); + + /// Return the world center of mass of an entity + const Vector3& getCenterOfMassWorld(Entity bodyEntity); + + /// Return true if gravity is enabled for this entity + bool getIsGravityEnabled(Entity bodyEntity) const; + + /// Return true if the entity is already in an island + bool getIsAlreadyInIsland(Entity bodyEntity) const; + + /// Set the constrained linear velocity of an entity + void setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity); + + /// Set the constrained angular velocity of an entity + void setConstrainedAngularVelocity(Entity bodyEntity, const Vector3& constrainedAngularVelocity); + + /// Set the split linear velocity of an entity + void setSplitLinearVelocity(Entity bodyEntity, const Vector3& splitLinearVelocity); + + /// Set the split angular velocity of an entity + void setSplitAngularVelocity(Entity bodyEntity, const Vector3& splitAngularVelocity); + + /// Set the constrained position of an entity + void setConstrainedPosition(Entity bodyEntity, const Vector3& constrainedPosition); + + /// Set the constrained orientation of an entity + void setConstrainedOrientation(Entity bodyEntity, const Quaternion& constrainedOrientation); + + /// Set the local center of mass of an entity + void setCenterOfMassLocal(Entity bodyEntity, const Vector3& centerOfMassLocal); + + /// Set the world center of mass of an entity + void setCenterOfMassWorld(Entity bodyEntity, const Vector3& centerOfMassWorld); + + /// Set the value to know if the gravity is enabled for this entity + void setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled); + + /// Set the value to know if the entity is already in an island + void setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland); + + // -------------------- Friendship -------------------- // + + friend class DynamicsWorld; + friend class ContactSolver; + friend class BallAndSocketJoint; + friend class FixedJoint; + friend class HingeJoint; + friend class SliderJoint; }; // Return a pointer to a body rigid @@ -270,6 +444,293 @@ inline void RigidBodyComponents::setAngularVelocity(Entity bodyEntity, const Vec mAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = angularVelocity; } +// Return the external force of an entity +inline const Vector3& RigidBodyComponents::getExternalForce(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mExternalForces[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the external torque of an entity +inline const Vector3& RigidBodyComponents::getExternalTorque(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mExternalTorques[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the linear damping factor of an entity +inline decimal RigidBodyComponents::getLinearDamping(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mLinearDampings[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the angular damping factor of an entity +inline decimal RigidBodyComponents::getAngularDamping(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mAngularDampings[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the initial mass of an entity +inline decimal RigidBodyComponents::getInitMass(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mInitMasses[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the inverse mass of an entity +inline decimal RigidBodyComponents::getMassInverse(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mInverseMasses[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the inverse local inertia tensor of an entity +inline const Matrix3x3& RigidBodyComponents::getInertiaTensorLocalInverse(Entity bodyEntity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the inverse world inertia tensor of an entity +inline const Matrix3x3& RigidBodyComponents::getInertiaTensorWorldInverse(Entity bodyEntity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Set the external force of an entity +inline void RigidBodyComponents::setExternalForce(Entity bodyEntity, const Vector3& externalForce) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mExternalForces[mMapEntityToComponentIndex[bodyEntity]] = externalForce; +} + +// Set the external force of an entity +inline void RigidBodyComponents::setExternalTorque(Entity bodyEntity, const Vector3& externalTorque) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mExternalTorques[mMapEntityToComponentIndex[bodyEntity]] = externalTorque; +} + +// Set the linear damping factor of an entity +inline void RigidBodyComponents::setLinearDamping(Entity bodyEntity, decimal linearDamping) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mLinearDampings[mMapEntityToComponentIndex[bodyEntity]] = linearDamping; +} + +// Set the angular damping factor of an entity +inline void RigidBodyComponents::setAngularDamping(Entity bodyEntity, decimal angularDamping) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mAngularDampings[mMapEntityToComponentIndex[bodyEntity]] = angularDamping; +} + +// Set the initial mass of an entity +inline void RigidBodyComponents::setInitMass(Entity bodyEntity, decimal initMass) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mInitMasses[mMapEntityToComponentIndex[bodyEntity]] = initMass; +} + +// Set the mass inverse of an entity +inline void RigidBodyComponents::setMassInverse(Entity bodyEntity, decimal inverseMass) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mInverseMasses[mMapEntityToComponentIndex[bodyEntity]] = inverseMass; +} + +// Set the inverse local inertia tensor of an entity +inline void RigidBodyComponents::setInverseInertiaTensorLocal(Entity bodyEntity, const Matrix3x3& inertiaTensorLocalInverse) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorLocalInverse; +} + +// Set the inverse world inertia tensor of an entity +inline void RigidBodyComponents::setInverseInertiaTensorWorld(Entity bodyEntity, const Matrix3x3& inertiaTensorWorldInverse) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorWorldInverse; +} + +// Return the constrained linear velocity of an entity +inline const Vector3& RigidBodyComponents::getConstrainedLinearVelocity(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mConstrainedLinearVelocities[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the constrained angular velocity of an entity +inline const Vector3& RigidBodyComponents::getConstrainedAngularVelocity(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mConstrainedAngularVelocities[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the split linear velocity of an entity +inline const Vector3& RigidBodyComponents::getSplitLinearVelocity(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mSplitLinearVelocities[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the split angular velocity of an entity +inline const Vector3& RigidBodyComponents::getSplitAngularVelocity(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mSplitAngularVelocities[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the constrained position of an entity +inline const Vector3& RigidBodyComponents::getConstrainedPosition(Entity bodyEntity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mConstrainedPositions[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the constrained orientation of an entity +inline const Quaternion& RigidBodyComponents::getConstrainedOrientation(Entity bodyEntity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mConstrainedOrientations[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the local center of mass of an entity +inline const Vector3& RigidBodyComponents::getCenterOfMassLocal(Entity bodyEntity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mCentersOfMassLocal[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the world center of mass of an entity +inline const Vector3& RigidBodyComponents::getCenterOfMassWorld(Entity bodyEntity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mCentersOfMassWorld[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Set the constrained linear velocity of an entity +inline void RigidBodyComponents::setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mConstrainedLinearVelocities[mMapEntityToComponentIndex[bodyEntity]] = constrainedLinearVelocity; +} + +// Set the constrained angular velocity of an entity +inline void RigidBodyComponents::setConstrainedAngularVelocity(Entity bodyEntity, const Vector3& constrainedAngularVelocity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mConstrainedAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = constrainedAngularVelocity; +} + +// Set the split linear velocity of an entity +inline void RigidBodyComponents::setSplitLinearVelocity(Entity bodyEntity, const Vector3& splitLinearVelocity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mSplitLinearVelocities[mMapEntityToComponentIndex[bodyEntity]] = splitLinearVelocity; +} + +// Set the split angular velocity of an entity +inline void RigidBodyComponents::setSplitAngularVelocity(Entity bodyEntity, const Vector3& splitAngularVelocity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mSplitAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = splitAngularVelocity; +} + +// Set the constrained position of an entity +inline void RigidBodyComponents::setConstrainedPosition(Entity bodyEntity, const Vector3& constrainedPosition) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mConstrainedPositions[mMapEntityToComponentIndex[bodyEntity]] = constrainedPosition; +} + +// Set the constrained orientation of an entity +inline void RigidBodyComponents::setConstrainedOrientation(Entity bodyEntity, const Quaternion& constrainedOrientation) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mConstrainedOrientations[mMapEntityToComponentIndex[bodyEntity]] = constrainedOrientation; +} + +// Set the local center of mass of an entity +inline void RigidBodyComponents::setCenterOfMassLocal(Entity bodyEntity, const Vector3& centerOfMassLocal) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mCentersOfMassLocal[mMapEntityToComponentIndex[bodyEntity]] = centerOfMassLocal; +} + +// Set the world center of mass of an entity +inline void RigidBodyComponents::setCenterOfMassWorld(Entity bodyEntity, const Vector3& centerOfMassWorld) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mCentersOfMassWorld[mMapEntityToComponentIndex[bodyEntity]] = centerOfMassWorld; +} + +// Return true if gravity is enabled for this entity +inline bool RigidBodyComponents::getIsGravityEnabled(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mIsGravityEnabled[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return true if the entity is already in an island +inline bool RigidBodyComponents::getIsAlreadyInIsland(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Set the value to know if the gravity is enabled for this entity +inline void RigidBodyComponents::setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mIsGravityEnabled[mMapEntityToComponentIndex[bodyEntity]] = isGravityEnabled; +} + +// Set the value to know if the entity is already in an island +inline void RigidBodyComponents::setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]] = isAlreadyInIsland; +} } diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 40c21b9a..171f39b9 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -26,7 +26,6 @@ // Libraries #include "BallAndSocketJoint.h" #include "engine/ConstraintSolver.h" -#include "components/DynamicsComponents.h" #include "components/RigidBodyComponents.h" using namespace reactphysics3d; @@ -47,8 +46,8 @@ BallAndSocketJoint::BallAndSocketJoint(uint id, const BallAndSocketJointInfo& jo void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { // Get the bodies center of mass and orientations - const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity); - const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity); + const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody1Entity); + const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody2Entity); const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); @@ -65,8 +64,8 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); // Compute the matrix K=JM^-1J^t (3x3 matrix) - decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity()); - decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity()); + decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1->getEntity()); + decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2->getEntity()); decimal inverseMassBodies = body1MassInverse + body2MassInverse; Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, @@ -98,42 +97,42 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS // Warm start the constraint (apply the previous impulse at the beginning of the step) void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverData) { - uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity); - uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity); + uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity); // Get the velocities - Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; - Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; - Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; - Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; + Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; + Vector3& v2 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; + Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; + Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Compute the impulse P=J^T * lambda for the body 1 const Vector3 linearImpulseBody1 = -mImpulse; const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World); // Apply the impulse to the body 1 - v1 += constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity) * linearImpulseBody1; + v1 += constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity) * linearImpulseBody1; w1 += mI1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the body 2 const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World); // Apply the impulse to the body to the body 2 - v2 += constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity) * mImpulse; + v2 += constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity) * mImpulse; w2 += mI2 * angularImpulseBody2; } // Solve the velocity constraint void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { - uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity); - uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity); + uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity); // Get the velocities - Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; - Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; - Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; - Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; + Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; + Vector3& v2 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; + Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; + Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Compute J*v const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); @@ -147,14 +146,14 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); // Apply the impulse to the body 1 - v1 += constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity) * linearImpulseBody1; + v1 += constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity) * linearImpulseBody1; w1 += mI1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the body 2 const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); // Apply the impulse to the body 2 - v2 += constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity) * deltaLambda; + v2 += constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity) * deltaLambda; w2 += mI2 * angularImpulseBody2; } @@ -166,14 +165,14 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies center of mass and orientations - Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity); - Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity); - Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity); - Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity); + Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody1Entity); + Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody2Entity); + Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody1Entity); + Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody2Entity); // Get the inverse mass and inverse inertia tensors of the bodies - const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); - const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); + const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); + const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); // Recompute the inverse inertia tensors mI1 = mBody1->getInertiaTensorInverseWorld(); @@ -232,9 +231,9 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2.normalize(); - constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1); - constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2); - constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1); - constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2); + constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody1Entity, x1); + constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody2Entity, x2); + constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody1Entity, q1); + constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody2Entity, q2); } diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index c67b9862..f954da20 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -26,7 +26,6 @@ // Libraries #include "FixedJoint.h" #include "engine/ConstraintSolver.h" -#include "components/DynamicsComponents.h" #include "components/RigidBodyComponents.h" using namespace reactphysics3d; @@ -62,8 +61,8 @@ FixedJoint::FixedJoint(uint id, const FixedJointInfo& jointInfo) void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { // Get the bodies positions and orientations - const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity); - const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity); + const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody1Entity); + const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody2Entity); const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); @@ -80,8 +79,8 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints - const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity()); - const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity()); + const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1->getEntity()); + const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2->getEntity()); const decimal inverseMassBodies = body1MassInverse + body2MassInverse; Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, @@ -129,18 +128,18 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Warm start the constraint (apply the previous impulse at the beginning of the step) void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) { - uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity); - uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity); + uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity); // Get the velocities - Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; - Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; - Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; - Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; + Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; + Vector3& v2 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; + Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; + Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass of the bodies - const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); - const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); + const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); + const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1 Vector3 linearImpulseBody1 = -mImpulseTranslation; @@ -167,18 +166,18 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Solve the velocity constraint void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { - uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity); - uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity); + uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity); // Get the velocities - Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; - Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; - Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; - Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; + Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; + Vector3& v2 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; + Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; + Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass of the bodies - decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); - decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); + decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); + decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); // --------------- Translation Constraints --------------- // @@ -232,14 +231,14 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies positions and orientations - Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity); - Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity); - Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity); - Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity); + Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody1Entity); + Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody2Entity); + Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody1Entity); + Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody2Entity); // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); - decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); + decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); + decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); // Recompute the inverse inertia tensors mI1 = mBody1->getInertiaTensorInverseWorld(); @@ -355,9 +354,9 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2.normalize(); - constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1); - constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2); - constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1); - constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2); + constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody1Entity, x1); + constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody2Entity, x2); + constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody1Entity, q1); + constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody2Entity, q2); } diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 29f29879..469fc124 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -26,7 +26,6 @@ // Libraries #include "HingeJoint.h" #include "engine/ConstraintSolver.h" -#include "components/DynamicsComponents.h" #include "components/RigidBodyComponents.h" using namespace reactphysics3d; @@ -69,8 +68,8 @@ HingeJoint::HingeJoint(uint id, const HingeJointInfo& jointInfo) void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { // Get the bodies positions and orientations - const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity); - const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity); + const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody1Entity); + const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody2Entity); const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); @@ -114,8 +113,8 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix) - decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity()); - decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity()); + decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1->getEntity()); + decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2->getEntity()); decimal inverseMassBodies = body1MassInverse + body2MassInverse; Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, @@ -198,18 +197,18 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Warm start the constraint (apply the previous impulse at the beginning of the step) void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { - uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity); - uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity); + uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity); // Get the velocities - Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; - Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; - Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; - Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; + Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; + Vector3& v2 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; + Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; + Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); - const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); + const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); + const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); // Compute the impulse P=J^T * lambda for the 2 rotation constraints Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y; @@ -257,18 +256,18 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Solve the velocity constraint void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { - uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity); - uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity); + uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity); // Get the velocities - Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; - Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; - Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; - Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; + Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; + Vector3& v2 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; + Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; + Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); - decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); + decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); + decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); // --------------- Translation Constraints --------------- // @@ -411,14 +410,14 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies positions and orientations - Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity); - Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity); - Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity); - Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity); + Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody1Entity); + Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody2Entity); + Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody1Entity); + Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody2Entity); // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); - decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); + decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); + decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); // Recompute the inverse inertia tensors mI1 = mBody1->getInertiaTensorInverseWorld(); @@ -454,8 +453,8 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // --------------- Translation Constraints --------------- // // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints - const decimal body1InverseMass = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); - const decimal body2InverseMass = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); + const decimal body1InverseMass = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); + const decimal body2InverseMass = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); decimal inverseMassBodies = body1InverseMass + body2InverseMass; Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, @@ -612,10 +611,10 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS } } - constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1); - constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2); - constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1); - constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2); + constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody1Entity, x1); + constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody2Entity, x2); + constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody1Entity, q1); + constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody2Entity, q2); } diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index e782d489..02026d30 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -26,7 +26,6 @@ // Libraries #include "SliderJoint.h" #include "engine/ConstraintSolver.h" -#include "components/DynamicsComponents.h" #include "components/RigidBodyComponents.h" using namespace reactphysics3d; @@ -77,8 +76,8 @@ SliderJoint::SliderJoint(uint id, const SliderJointInfo& jointInfo) void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { // Get the bodies positions and orientations - const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity); - const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity); + const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody1Entity); + const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody2Entity); const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); @@ -125,8 +124,8 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // constraints (2x2 matrix) - const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity()); - const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity()); + const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1->getEntity()); + const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2->getEntity()); const decimal sumInverseMass = body1MassInverse + body2MassInverse; Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; @@ -216,18 +215,18 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Warm start the constraint (apply the previous impulse at the beginning of the step) void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { - uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity); - uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity); + uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity); // Get the velocities - Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; - Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; - Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; - Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; + Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; + Vector3& v2 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; + Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; + Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); - const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); + const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); + const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1 decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit; @@ -278,18 +277,18 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Solve the velocity constraint void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { - uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity); - uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity); + uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity); // Get the velocities - Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; - Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; - Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; - Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; + Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; + Vector3& v2 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; + Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; + Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); - decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); + decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); + decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); // --------------- Translation Constraints --------------- // @@ -443,14 +442,14 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies positions and orientations - Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity); - Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity); - Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity); - Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity); + Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody1Entity); + Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody2Entity); + Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody1Entity); + Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody2Entity); // Get the inverse mass and inverse inertia tensors of the bodies - const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); - const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); + const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); + const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); // Recompute the inertia tensor of bodies mI1 = mBody1->getInertiaTensorInverseWorld(); @@ -489,8 +488,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // constraints (2x2 matrix) - const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); - const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); + const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); + const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); decimal sumInverseMass = body1MassInverse + body2MassInverse; Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; @@ -611,8 +610,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint if (mIsLowerLimitViolated || mIsUpperLimitViolated) { // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) - const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); - const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); + const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); + const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); mInverseMassMatrixLimit = body1MassInverse + body2MassInverse + mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) + mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); @@ -687,10 +686,10 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint } } - constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1); - constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2); - constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1); - constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2); + constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody1Entity, x1); + constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody2Entity, x2); + constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody1Entity, q1); + constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody2Entity, q2); } // Enable/Disable the limits of the joint diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index fdd15bf4..73266988 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -40,7 +40,6 @@ CollisionWorld::CollisionWorld(const WorldSettings& worldSettings, Logger* logge : mConfig(worldSettings), mEntityManager(mMemoryManager.getPoolAllocator()), mCollisionBodyComponents(mMemoryManager.getBaseAllocator()), mRigidBodyComponents(mMemoryManager.getBaseAllocator()), mTransformComponents(mMemoryManager.getBaseAllocator()), mProxyShapesComponents(mMemoryManager.getBaseAllocator()), - mDynamicsComponents(mMemoryManager.getBaseAllocator()), mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mRigidBodyComponents, mMemoryManager), mBodies(mMemoryManager.getPoolAllocator()), mEventListener(nullptr), mName(worldSettings.worldName), mIsProfilerCreatedByUser(profiler != nullptr), @@ -216,11 +215,11 @@ void CollisionWorld::notifyBodyDisabled(Entity bodyEntity, bool isDisabled) { // Notify all the components mCollisionBodyComponents.setIsEntityDisabled(bodyEntity, isDisabled); - mRigidBodyComponents.setIsEntityDisabled(bodyEntity, isDisabled); mTransformComponents.setIsEntityDisabled(bodyEntity, isDisabled); - if (mDynamicsComponents.hasComponent(bodyEntity)) { - mDynamicsComponents.setIsEntityDisabled(bodyEntity, isDisabled); + if (mRigidBodyComponents.hasComponent(bodyEntity)) { + mRigidBodyComponents.setIsEntityDisabled(bodyEntity, isDisabled); + mRigidBodyComponents.setIsEntityDisabled(bodyEntity, isDisabled); } // For each proxy-shape of the body diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 587b5eaa..c109ae5d 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -37,7 +37,6 @@ #include "components/RigidBodyComponents.h" #include "components/TransformComponents.h" #include "components/ProxyShapeComponents.h" -#include "components/DynamicsComponents.h" #include "collision/CollisionCallback.h" #include "collision/OverlapCallback.h" @@ -89,9 +88,6 @@ class CollisionWorld { /// Proxy-Shapes Components ProxyShapeComponents mProxyShapesComponents; - /// Dynamics components of the bodies (linear, angular velocities) - DynamicsComponents mDynamicsComponents; - /// Reference to the collision detection CollisionDetection mCollisionDetection; diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 2d95e041..14979cff 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -31,8 +31,8 @@ using namespace reactphysics3d; // Constructor -ConstraintSolver::ConstraintSolver(Islands& islands, DynamicsComponents& dynamicsComponents) - : mIsWarmStartingActive(true), mIslands(islands), mConstraintSolverData(dynamicsComponents) { +ConstraintSolver::ConstraintSolver(Islands& islands, RigidBodyComponents& rigidBodyComponents) + : mIsWarmStartingActive(true), mIslands(islands), mConstraintSolverData(rigidBodyComponents) { #ifdef IS_PROFILING_ACTIVE diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 0a4ae48c..069e8b89 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -37,6 +37,7 @@ namespace reactphysics3d { class Joint; class Island; class Profiler; +class RigidBodyComponents; class DynamicsComponents; // Structure ConstraintSolverData @@ -51,15 +52,15 @@ struct ConstraintSolverData { /// Current time step of the simulation decimal timeStep; - /// Reference to the dynamics components - DynamicsComponents& dynamicsComponents; + /// Reference to the rigid body components + RigidBodyComponents& rigidBodyComponents; /// True if warm starting of the solver is active bool isWarmStartingActive; /// Constructor - ConstraintSolverData(DynamicsComponents& dynamicsComponents) - :dynamicsComponents(dynamicsComponents) { + ConstraintSolverData(RigidBodyComponents& rigidBodyComponents) + :rigidBodyComponents(rigidBodyComponents) { } @@ -163,7 +164,7 @@ class ConstraintSolver { // -------------------- Methods -------------------- // /// Constructor - ConstraintSolver(Islands& islands, DynamicsComponents& dynamicsComponents); + ConstraintSolver(Islands& islands, RigidBodyComponents& rigidBodyComponents); /// Destructor ~ConstraintSolver() = default; diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index acb7a8b2..09a8b7c3 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -31,7 +31,6 @@ #include "utils/Profiler.h" #include "engine/Island.h" #include "components/CollisionBodyComponents.h" -#include "components/DynamicsComponents.h" #include "components/ProxyShapeComponents.h" #include "collision/ContactManifold.h" @@ -45,12 +44,13 @@ const decimal ContactSolver::SLOP = decimal(0.01); // Constructor ContactSolver::ContactSolver(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents, - RigidBodyComponents& rigidBodyComponents, DynamicsComponents& dynamicsComponents, - ProxyShapeComponents& proxyShapeComponents, const WorldSettings& worldSettings) + RigidBodyComponents& rigidBodyComponents, ProxyShapeComponents& proxyShapeComponents, + const WorldSettings& worldSettings) :mMemoryManager(memoryManager), mContactConstraints(nullptr), mContactPoints(nullptr), - mIslands(islands), mAllContactManifolds(nullptr), mAllContactPoints(nullptr), mBodyComponents(bodyComponents), - mRigidBodyComponents(rigidBodyComponents), mDynamicsComponents(dynamicsComponents), - mProxyShapeComponents(proxyShapeComponents), mIsSplitImpulseActive(true), mWorldSettings(worldSettings) { + mIslands(islands), mAllContactManifolds(nullptr), mAllContactPoints(nullptr), + mBodyComponents(bodyComponents), mRigidBodyComponents(rigidBodyComponents), + mProxyShapeComponents(proxyShapeComponents), mIsSplitImpulseActive(true), + mWorldSettings(worldSettings) { #ifdef IS_PROFILING_ACTIVE mProfiler = nullptr; @@ -132,18 +132,18 @@ void ContactSolver::initializeForIsland(uint islandIndex) { const ProxyShape* shape2 = mProxyShapeComponents.getProxyShape(externalManifold.proxyShapeEntity2); // Get the position of the two bodies - const Vector3& x1 = mDynamicsComponents.getCenterOfMassWorld(externalManifold.bodyEntity1); - const Vector3& x2 = mDynamicsComponents.getCenterOfMassWorld(externalManifold.bodyEntity2); + const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(externalManifold.bodyEntity1); + const Vector3& x2 = mRigidBodyComponents.getCenterOfMassWorld(externalManifold.bodyEntity2); // Initialize the internal contact manifold structure using the external // contact manifold new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver(); - mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody1 = mDynamicsComponents.getEntityIndex(body1->getEntity()); - mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody2 = mDynamicsComponents.getEntityIndex(body2->getEntity()); + mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1->getEntity()); + mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2->getEntity()); mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); - mContactConstraints[mNbContactManifolds].massInverseBody1 = mDynamicsComponents.getMassInverse(body1->getEntity()); - mContactConstraints[mNbContactManifolds].massInverseBody2 = mDynamicsComponents.getMassInverse(body2->getEntity()); + mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.getMassInverse(body1->getEntity()); + mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.getMassInverse(body2->getEntity()); mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.getNbContactPoints(); mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(body1, body2); @@ -357,22 +357,22 @@ void ContactSolver::warmStart() { Vector3 impulsePenetration(mContactPoints[contactPointIndex].normal.x * mContactPoints[contactPointIndex].penetrationImpulse, mContactPoints[contactPointIndex].normal.y * mContactPoints[contactPointIndex].penetrationImpulse, mContactPoints[contactPointIndex].normal.z * mContactPoints[contactPointIndex].penetrationImpulse); - mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x; - mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y; - mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z; - mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse; - mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse; - mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse; // Update the velocities of the body 2 by applying the impulse P - mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x; - mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y; - mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z; - mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse; - mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse; - mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse; } else { // If it is a new contact point @@ -412,12 +412,12 @@ void ContactSolver::warmStart() { mContactConstraints[c].r2CrossT1.z * mContactConstraints[c].friction1Impulse); // Update the velocities of the body 1 by applying the impulse P - mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; - mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 1 by applying the impulse P - mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; - mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Second friction constraint at the center of the contact manifold ----- // @@ -433,18 +433,18 @@ void ContactSolver::warmStart() { angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * mContactConstraints[c].friction2Impulse; // Update the velocities of the body 1 by applying the impulse P - mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; - mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; - mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; - mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; - mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; - mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; - mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Twist friction constraint at the center of the contact manifold ------ // @@ -458,10 +458,10 @@ void ContactSolver::warmStart() { angularImpulseBody2.z = mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse; // Update the velocities of the body 1 by applying the impulse P - mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Rolling resistance at the center of the contact manifold ------ // @@ -469,10 +469,10 @@ void ContactSolver::warmStart() { angularImpulseBody2 = mContactConstraints[c].rollingResistanceImpulse; // Update the velocities of the body 1 by applying the impulse P - mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2; // Update the velocities of the body 1 by applying the impulse P - mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; } else { // If it is a new contact manifold @@ -500,10 +500,10 @@ void ContactSolver::solve() { decimal sumPenetrationImpulse = 0.0; // Get the constrained velocities - const Vector3& v1 = mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1]; - const Vector3& w1 = mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1]; - const Vector3& v2 = mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2]; - const Vector3& w2 = mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2]; + const Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1]; + const Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1]; + const Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2]; + const Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2]; for (short int i=0; i e^(-x) ~ 1 - x // => v2 = v1 * (1 - c * dt) - for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { - const decimal linDampingFactor = mDynamicsComponents.mLinearDampings[i]; - const decimal angDampingFactor = mDynamicsComponents.mAngularDampings[i]; + const decimal linDampingFactor = mRigidBodyComponents.mLinearDampings[i]; + const decimal angDampingFactor = mRigidBodyComponents.mAngularDampings[i]; const decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep); const decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep); - mDynamicsComponents.mConstrainedLinearVelocities[i] = mDynamicsComponents.mConstrainedLinearVelocities[i] * linearDamping; - mDynamicsComponents.mConstrainedAngularVelocities[i] = mDynamicsComponents.mConstrainedAngularVelocities[i] * angularDamping; + mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] * linearDamping; + mRigidBodyComponents.mConstrainedAngularVelocities[i] = mRigidBodyComponents.mConstrainedAngularVelocities[i] * angularDamping; } } @@ -380,19 +380,24 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) { Entity entity = mEntityManager.createEntity(); mTransformComponents.addComponent(entity, false, TransformComponents::TransformComponent(transform)); - mDynamicsComponents.addComponent(entity, false, DynamicsComponents::DynamicsComponent(transform.getPosition())); // Create the rigid body RigidBody* rigidBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, - sizeof(RigidBody))) RigidBody(transform, *this, entity); + sizeof(RigidBody))) RigidBody(*this, entity); assert(rigidBody != nullptr); CollisionBodyComponents::CollisionBodyComponent bodyComponent(rigidBody); mCollisionBodyComponents.addComponent(entity, false, bodyComponent); - RigidBodyComponents::RigidBodyComponent rigidBodyComponent(rigidBody, BodyType::DYNAMIC); + RigidBodyComponents::RigidBodyComponent rigidBodyComponent(rigidBody, BodyType::DYNAMIC, transform.getPosition()); mRigidBodyComponents.addComponent(entity, false, rigidBodyComponent); + // Compute the inverse mass + mRigidBodyComponents.setMassInverse(entity, decimal(1.0) / mRigidBodyComponents.getInitMass(entity)); + + // Update the world inverse inertia tensor + rigidBody->updateInertiaTensorInverseWorld(); + // Add the rigid body to the physics world mBodies.add(rigidBody); mRigidBodies.add(rigidBody); @@ -641,9 +646,9 @@ void DynamicsWorld::createIslands() { RP3D_PROFILE("DynamicsWorld::createIslands()", mProfiler); // Reset all the isAlreadyInIsland variables of bodies and joints - for (uint b=0; b < mDynamicsComponents.getNbComponents(); b++) { + for (uint b=0; b < mRigidBodyComponents.getNbComponents(); b++) { - mDynamicsComponents.mIsAlreadyInIsland[b] = false; + mRigidBodyComponents.mIsAlreadyInIsland[b] = false; } for (List::Iterator it = mJoints.begin(); it != mJoints.end(); ++it) { (*it)->mIsAlreadyInIsland = false; @@ -657,21 +662,21 @@ void DynamicsWorld::createIslands() { // For each dynamic component // TODO : Here we iterate on dynamic component where we can have static, kinematic and dynamic bodies. Maybe we should // not use a dynamic component for a static body. - for (uint b=0; b < mDynamicsComponents.getNbEnabledComponents(); b++) { + for (uint b=0; b < mRigidBodyComponents.getNbEnabledComponents(); b++) { // If the body has already been added to an island, we go to the next body - if (mDynamicsComponents.mIsAlreadyInIsland[b]) continue; + if (mRigidBodyComponents.mIsAlreadyInIsland[b]) continue; // If the body is static, we go to the next body // TODO : Check if we still need this test if we loop over dynamicsComponents and static bodies are not part of them - if (mRigidBodyComponents.getBodyType(mDynamicsComponents.mBodies[b]) == BodyType::STATIC) continue; + if (mRigidBodyComponents.getBodyType(mRigidBodyComponents.mBodiesEntities[b]) == BodyType::STATIC) continue; // Reset the stack of bodies to visit bodyEntityIndicesToVisit.clear(); // Add the body into the stack of bodies to visit - mDynamicsComponents.mIsAlreadyInIsland[b] = true; - bodyEntityIndicesToVisit.push(mDynamicsComponents.mBodies[b]); + mRigidBodyComponents.mIsAlreadyInIsland[b] = true; + bodyEntityIndicesToVisit.push(mRigidBodyComponents.mBodiesEntities[b]); // Create the new island uint32 islandIndex = mIslands.addIsland(nbTotalManifolds); @@ -729,11 +734,11 @@ void DynamicsWorld::createIslands() { const Entity otherBodyEntity = pair.body1Entity == bodyToVisitEntity ? pair.body2Entity : pair.body1Entity; // Check if the other body has already been added to the island - if (mDynamicsComponents.getIsAlreadyInIsland(otherBodyEntity)) continue; + if (mRigidBodyComponents.getIsAlreadyInIsland(otherBodyEntity)) continue; // Insert the other body into the stack of bodies to visit bodyEntityIndicesToVisit.push(otherBodyEntity); - mDynamicsComponents.setIsAlreadyInIsland(otherBodyEntity, true); + mRigidBodyComponents.setIsAlreadyInIsland(otherBodyEntity, true); } else { @@ -763,20 +768,20 @@ void DynamicsWorld::createIslands() { const Entity otherBodyEntity = body1Entity == bodyToVisitEntity ? body2Entity : body1Entity; // Check if the other body has already been added to the island - if (mDynamicsComponents.getIsAlreadyInIsland(otherBodyEntity)) continue; + if (mRigidBodyComponents.getIsAlreadyInIsland(otherBodyEntity)) continue; // Insert the other body into the stack of bodies to visit bodyEntityIndicesToVisit.push(otherBodyEntity); - mDynamicsComponents.setIsAlreadyInIsland(otherBodyEntity, true); + mRigidBodyComponents.setIsAlreadyInIsland(otherBodyEntity, true); } } // Reset the isAlreadyIsland variable of the static bodies so that they // can also be included in the other islands - for (uint j=0; j < mDynamicsComponents.getNbEnabledComponents(); j++) { + for (uint j=0; j < mRigidBodyComponents.getNbEnabledComponents(); j++) { - if (mRigidBodyComponents.getBodyType(mDynamicsComponents.mBodies[j]) == BodyType::STATIC) { - mDynamicsComponents.mIsAlreadyInIsland[j] = false; + if (mRigidBodyComponents.getBodyType(mRigidBodyComponents.mBodiesEntities[j]) == BodyType::STATIC) { + mRigidBodyComponents.mIsAlreadyInIsland[j] = false; } } } diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index b6d95ff8..2a31b546 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -32,7 +32,6 @@ #include "configuration.h" #include "utils/Logger.h" #include "engine/ContactSolver.h" -#include "components/DynamicsComponents.h" #include "engine/Islands.h" /// Namespace ReactPhysics3D @@ -249,9 +248,9 @@ class DynamicsWorld : public CollisionWorld { inline void DynamicsWorld::resetBodiesForceAndTorque() { // For each body of the world - for (uint32 i=0; i < mDynamicsComponents.getNbComponents(); i++) { - mDynamicsComponents.mExternalForces[i].setToZero(); - mDynamicsComponents.mExternalTorques[i].setToZero(); + for (uint32 i=0; i < mRigidBodyComponents.getNbComponents(); i++) { + mRigidBodyComponents.mExternalForces[i].setToZero(); + mRigidBodyComponents.mExternalTorques[i].setToZero(); } }