mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Remove DynamicsComponents

Browse Source
This commit is contained in:
Daniel Chappuis 2019-07-21 22:36:30 +02:00
parent 6f9adc3a32
commit 3d09a28dd1
19 changed files with 975 additions and 1233 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -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"

135
src/body/RigidBody.cpp
View File

@ -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<Entity>& 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,

2
src/body/RigidBody.h
View File

@ -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;

289
src/components/DynamicsComponents.cpp
View File

@ -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 <cassert>
#include <random>
// 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<Entity*>(newBuffer);
Vector3* newConstrainedLinearVelocities = reinterpret_cast<Vector3*>(newBodies + nbComponentsToAllocate);
Vector3* newConstrainedAngularVelocities = reinterpret_cast<Vector3*>(newConstrainedLinearVelocities + nbComponentsToAllocate);
Vector3* newSplitLinearVelocities = reinterpret_cast<Vector3*>(newConstrainedAngularVelocities + nbComponentsToAllocate);
Vector3* newSplitAngularVelocities = reinterpret_cast<Vector3*>(newSplitLinearVelocities + nbComponentsToAllocate);
Vector3* newExternalForces = reinterpret_cast<Vector3*>(newSplitAngularVelocities + nbComponentsToAllocate);
Vector3* newExternalTorques = reinterpret_cast<Vector3*>(newExternalForces + nbComponentsToAllocate);
decimal* newLinearDampings = reinterpret_cast<decimal*>(newExternalTorques + nbComponentsToAllocate);
decimal* newAngularDampings = reinterpret_cast<decimal*>(newLinearDampings + nbComponentsToAllocate);
decimal* newInitMasses = reinterpret_cast<decimal*>(newAngularDampings + nbComponentsToAllocate);
decimal* newInverseMasses = reinterpret_cast<decimal*>(newInitMasses + nbComponentsToAllocate);
Matrix3x3* newInertiaTensorLocalInverses = reinterpret_cast<Matrix3x3*>(newInverseMasses + nbComponentsToAllocate);
Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast<Matrix3x3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate);
Vector3* newConstrainedPositions = reinterpret_cast<Vector3*>(newInertiaTensorWorldInverses + nbComponentsToAllocate);
Quaternion* newConstrainedOrientations = reinterpret_cast<Quaternion*>(newConstrainedPositions + nbComponentsToAllocate);
Vector3* newCentersOfMassLocal = reinterpret_cast<Vector3*>(newConstrainedOrientations + nbComponentsToAllocate);
Vector3* newCentersOfMassWorld = reinterpret_cast<Vector3*>(newCentersOfMassLocal + nbComponentsToAllocate);
bool* newIsGravityEnabled = reinterpret_cast<bool*>(newCentersOfMassWorld + nbComponentsToAllocate);
bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(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<Entity, uint32>(bodyEntity, index));
mNbComponents++;
assert(mDisabledStartIndex <= mNbComponents);
assert(mNbComponents == static_cast<uint32>(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, uint32>(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<Entity, uint32>(entity1, index2));
assert(mMapEntityToComponentIndex[mBodies[index1]] == index1);
assert(mMapEntityToComponentIndex[mBodies[index2]] == index2);
assert(mNbComponents == static_cast<uint32>(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();
}

560
src/components/DynamicsComponents.h
View File

@ -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

146
src/components/RigidBodyComponents.cpp
View File

@ -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<BodyType*>(newSleepTimes + nbComponentsToAllocate);
Vector3* newLinearVelocities = reinterpret_cast<Vector3*>(newBodyTypes + nbComponentsToAllocate);
Vector3* newAngularVelocities = reinterpret_cast<Vector3*>(newLinearVelocities + nbComponentsToAllocate);
Vector3* newExternalForces = reinterpret_cast<Vector3*>(newAngularVelocities + nbComponentsToAllocate);
Vector3* newExternalTorques = reinterpret_cast<Vector3*>(newExternalForces + nbComponentsToAllocate);
decimal* newLinearDampings = reinterpret_cast<decimal*>(newExternalTorques + nbComponentsToAllocate);
decimal* newAngularDampings = reinterpret_cast<decimal*>(newLinearDampings + nbComponentsToAllocate);
decimal* newInitMasses = reinterpret_cast<decimal*>(newAngularDampings + nbComponentsToAllocate);
decimal* newInverseMasses = reinterpret_cast<decimal*>(newInitMasses + nbComponentsToAllocate);
Matrix3x3* newInertiaTensorLocalInverses = reinterpret_cast<Matrix3x3*>(newInverseMasses + nbComponentsToAllocate);
Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast<Matrix3x3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate);
Vector3* newConstrainedLinearVelocities = reinterpret_cast<Vector3*>(newInertiaTensorWorldInverses + nbComponentsToAllocate);
Vector3* newConstrainedAngularVelocities = reinterpret_cast<Vector3*>(newConstrainedLinearVelocities + nbComponentsToAllocate);
Vector3* newSplitLinearVelocities = reinterpret_cast<Vector3*>(newConstrainedAngularVelocities + nbComponentsToAllocate);
Vector3* newSplitAngularVelocities = reinterpret_cast<Vector3*>(newSplitLinearVelocities + nbComponentsToAllocate);
Vector3* newConstrainedPositions = reinterpret_cast<Vector3*>(newSplitAngularVelocities + nbComponentsToAllocate);
Quaternion* newConstrainedOrientations = reinterpret_cast<Quaternion*>(newConstrainedPositions + nbComponentsToAllocate);
Vector3* newCentersOfMassLocal = reinterpret_cast<Vector3*>(newConstrainedOrientations + nbComponentsToAllocate);
Vector3* newCentersOfMassWorld = reinterpret_cast<Vector3*>(newCentersOfMassLocal + nbComponentsToAllocate);
bool* newIsGravityEnabled = reinterpret_cast<bool*>(newCentersOfMassWorld + nbComponentsToAllocate);
bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(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<Entity, uint32>(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<Entity, uint32>(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();
}

463
src/components/RigidBodyComponents.h
View File

@ -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;
}
}

61
src/constraint/BallAndSocketJoint.cpp
View File

@ -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);
}

61
src/constraint/FixedJoint.cpp
View File

@ -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);
}

65
src/constraint/HingeJoint.cpp
View File

@ -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);
}

69
src/constraint/SliderJoint.cpp
View File

@ -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

7
src/engine/CollisionWorld.cpp
View File

@ -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

4
src/engine/CollisionWorld.h
View File

@ -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;

4
src/engine/ConstraintSolver.cpp
View File

@ -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

11
src/engine/ConstraintSolver.h
View File

@ -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;

184
src/engine/ContactSolver.cpp
View File

@ -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<mContactConstraints[c].nbContacts; i++) {
@ -546,22 +546,22 @@ void ContactSolver::solve() {
mContactPoints[contactPointIndex].normal.z * deltaLambda);
// Update the velocities of the body 1 by applying the impulse P
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda;
// Update the velocities of the body 2 by applying the impulse P
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda;
sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse;
@ -569,10 +569,10 @@ void ContactSolver::solve() {
if (mIsSplitImpulseActive) {
// Split impulse (position correction)
const Vector3& v1Split = mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1];
const Vector3& w1Split = mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1];
const Vector3& v2Split = mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2];
const Vector3& w2Split = mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2];
const Vector3& v1Split = mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1];
const Vector3& w1Split = mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1];
const Vector3& v2Split = mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2];
const Vector3& w2Split = mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2];
//Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1);
Vector3 deltaVSplit(v2Split.x + w2Split.y * mContactPoints[contactPointIndex].r2.z - w2Split.z * mContactPoints[contactPointIndex].r2.y - v1Split.x -
@ -597,22 +597,22 @@ void ContactSolver::solve() {
mContactPoints[contactPointIndex].normal.z * deltaLambdaSplit);
// Update the velocities of the body 1 by applying the impulse P
mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit;
mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit;
mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit;
mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit;
mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit;
mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit;
// Update the velocities of the body 1 by applying the impulse P
mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit;
mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit;
mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit;
mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit;
mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit;
mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit;
}
contactPointIndex++;
@ -653,18 +653,18 @@ void ContactSolver::solve() {
mContactConstraints[c].r2CrossT1.z * deltaLambda);
// 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;
// ------ Second friction constraint at the center of the contact manifol ----- //
@ -702,16 +702,16 @@ void ContactSolver::solve() {
angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * deltaLambda;
// 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;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
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;
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;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
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;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Twist friction constraint at the center of the contact manifol ------ //
@ -734,10 +734,10 @@ void ContactSolver::solve() {
angularImpulseBody2.z = mContactConstraints[c].normal.z * deltaLambda;
// 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;
// --------- Rolling resistance constraint at the center of the contact manifold --------- //
@ -755,10 +755,10 @@ void ContactSolver::solve() {
deltaLambdaRolling = mContactConstraints[c].rollingResistanceImpulse - lambdaTempRolling;
// Update the velocities of the body 1 by applying the impulse P
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling;
// Update the velocities of the body 2 by applying the impulse P
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
}
}
}

7
src/engine/ContactSolver.h
View File

@ -316,9 +316,6 @@ class ContactSolver {
/// Reference to the dynamics components
RigidBodyComponents& mRigidBodyComponents;
/// Reference to the dynamics components
DynamicsComponents& mDynamicsComponents;
/// Reference to the proxy-shapes components
// TODO : Do we really need to use this ?
ProxyShapeComponents& mProxyShapeComponents;
@ -364,8 +361,8 @@ class ContactSolver {
/// Constructor
ContactSolver(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents,
RigidBodyComponents& rigidBodyComponents, DynamicsComponents &dynamicsComponents,
ProxyShapeComponents& proxyShapeComponents, const WorldSettings& worldSettings);
RigidBodyComponents& rigidBodyComponents, ProxyShapeComponents& proxyShapeComponents,
const WorldSettings& worldSettings);
/// Destructor
~ContactSolver() = default;

131
src/engine/DynamicsWorld.cpp
View File

@ -50,9 +50,9 @@ using namespace std;
DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldSettings, Logger* logger, Profiler* profiler)
: CollisionWorld(worldSettings, logger, profiler),
mIslands(mMemoryManager.getSingleFrameAllocator()),
mContactSolver(mMemoryManager, mIslands, mCollisionBodyComponents, mRigidBodyComponents, mDynamicsComponents,
mContactSolver(mMemoryManager, mIslands, mCollisionBodyComponents, mRigidBodyComponents,
mProxyShapesComponents, mConfig),
mConstraintSolver(mIslands, mDynamicsComponents),
mConstraintSolver(mIslands, mRigidBodyComponents),
mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations),
mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations),
mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()),
@ -169,24 +169,24 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
const decimal isSplitImpulseActive = mContactSolver.isSplitImpulseActive() ? decimal(1.0) : decimal(0.0);
for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
// Get the constrained velocity
Vector3 newLinVelocity = mDynamicsComponents.mConstrainedLinearVelocities[i];
Vector3 newAngVelocity = mDynamicsComponents.mConstrainedAngularVelocities[i];
Vector3 newLinVelocity = mRigidBodyComponents.mConstrainedLinearVelocities[i];
Vector3 newAngVelocity = mRigidBodyComponents.mConstrainedAngularVelocities[i];
// Add the split impulse velocity from Contact Solver (only used
// to update the position)
newLinVelocity += isSplitImpulseActive * mDynamicsComponents.mSplitLinearVelocities[i];
newAngVelocity += isSplitImpulseActive * mDynamicsComponents.mSplitAngularVelocities[i];
newLinVelocity += isSplitImpulseActive * mRigidBodyComponents.mSplitLinearVelocities[i];
newAngVelocity += isSplitImpulseActive * mRigidBodyComponents.mSplitAngularVelocities[i];
// Get current position and orientation of the body
const Vector3& currentPosition = mDynamicsComponents.mCentersOfMassWorld[i];
const Quaternion& currentOrientation = mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]).getOrientation();
const Vector3& currentPosition = mRigidBodyComponents.mCentersOfMassWorld[i];
const Quaternion& currentOrientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation();
// Update the new constrained position and orientation of the body
mDynamicsComponents.mConstrainedPositions[i] = currentPosition + newLinVelocity * mTimeStep;
mDynamicsComponents.mConstrainedOrientations[i] = currentOrientation + Quaternion(0, newAngVelocity) *
mRigidBodyComponents.mConstrainedPositions[i] = currentPosition + newLinVelocity * mTimeStep;
mRigidBodyComponents.mConstrainedOrientations[i] = currentOrientation + Quaternion(0, newAngVelocity) *
currentOrientation * decimal(0.5) * mTimeStep;
}
}
@ -198,35 +198,35 @@ void DynamicsWorld::updateBodiesState() {
// TODO : Make sure we compute this in a system
for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
// Update the linear and angular velocity of the body
mRigidBodyComponents.setLinearVelocity(mDynamicsComponents.mBodies[i], mDynamicsComponents.mConstrainedLinearVelocities[i]);
mRigidBodyComponents.setAngularVelocity(mDynamicsComponents.mBodies[i], mDynamicsComponents.mConstrainedAngularVelocities[i]);
mRigidBodyComponents.setLinearVelocity(mRigidBodyComponents.mBodiesEntities[i], mRigidBodyComponents.mConstrainedLinearVelocities[i]);
mRigidBodyComponents.setAngularVelocity(mRigidBodyComponents.mBodiesEntities[i], mRigidBodyComponents.mConstrainedAngularVelocities[i]);
// Update the position of the center of mass of the body
mDynamicsComponents.mCentersOfMassWorld[i] = mDynamicsComponents.mConstrainedPositions[i];
mRigidBodyComponents.mCentersOfMassWorld[i] = mRigidBodyComponents.mConstrainedPositions[i];
// Update the orientation of the body
const Quaternion& constrainedOrientation = mDynamicsComponents.mConstrainedOrientations[i];
mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]).setOrientation(constrainedOrientation.getUnit());
const Quaternion& constrainedOrientation = mRigidBodyComponents.mConstrainedOrientations[i];
mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).setOrientation(constrainedOrientation.getUnit());
}
// Update the transform of the body (using the new center of mass and new orientation)
for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
Transform& transform = mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]);
const Vector3& centerOfMassWorld = mDynamicsComponents.mCentersOfMassWorld[i];
const Vector3& centerOfMassLocal = mDynamicsComponents.mCentersOfMassLocal[i];
Transform& transform = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]);
const Vector3& centerOfMassWorld = mRigidBodyComponents.mCentersOfMassWorld[i];
const Vector3& centerOfMassLocal = mRigidBodyComponents.mCentersOfMassLocal[i];
transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal);
}
// Update the world inverse inertia tensor of the body
for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
Matrix3x3 orientation = mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]).getOrientation().getMatrix();
const Matrix3x3& inverseInertiaLocalTensor = mDynamicsComponents.mInverseInertiaTensorsLocal[i];
mDynamicsComponents.mInverseInertiaTensorsWorld[i] = orientation * inverseInertiaLocalTensor * orientation.getTranspose();
Matrix3x3 orientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation().getMatrix();
const Matrix3x3& inverseInertiaLocalTensor = mRigidBodyComponents.getInertiaTensorLocalInverse(mRigidBodyComponents.mBodiesEntities[i]);
mRigidBodyComponents.setInverseInertiaTensorWorld(mRigidBodyComponents.mBodiesEntities[i], orientation * inverseInertiaLocalTensor * orientation.getTranspose());
}
// Update the proxy-shapes components
@ -236,9 +236,9 @@ void DynamicsWorld::updateBodiesState() {
// Reset the split velocities of the bodies
void DynamicsWorld::resetSplitVelocities() {
for(uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
mDynamicsComponents.mSplitLinearVelocities[i].setToZero();
mDynamicsComponents.mSplitAngularVelocities[i].setToZero();
for(uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
mRigidBodyComponents.mSplitLinearVelocities[i].setToZero();
mRigidBodyComponents.mSplitAngularVelocities[i].setToZero();
}
}
@ -255,29 +255,29 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
resetSplitVelocities();
// Integration component velocities using force/torque
for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
assert(mDynamicsComponents.mSplitLinearVelocities[i] == Vector3(0, 0, 0));
assert(mDynamicsComponents.mSplitAngularVelocities[i] == Vector3(0, 0, 0));
assert(mRigidBodyComponents.mSplitLinearVelocities[i] == Vector3(0, 0, 0));
assert(mRigidBodyComponents.mSplitAngularVelocities[i] == Vector3(0, 0, 0));
const Vector3& linearVelocity = mRigidBodyComponents.getLinearVelocity(mDynamicsComponents.mBodies[i]);
const Vector3& angularVelocity = mRigidBodyComponents.getAngularVelocity(mDynamicsComponents.mBodies[i]);
const Vector3& linearVelocity = mRigidBodyComponents.getLinearVelocity(mRigidBodyComponents.mBodiesEntities[i]);
const Vector3& angularVelocity = mRigidBodyComponents.getAngularVelocity(mRigidBodyComponents.mBodiesEntities[i]);
// Integrate the external force to get the new velocity of the body
mDynamicsComponents.mConstrainedLinearVelocities[i] = linearVelocity + mTimeStep *
mDynamicsComponents.mInverseMasses[i] * mDynamicsComponents.mExternalForces[i];
mDynamicsComponents.mConstrainedAngularVelocities[i] = angularVelocity + mTimeStep *
mDynamicsComponents.mInverseInertiaTensorsWorld[i] * mDynamicsComponents.mExternalTorques[i];
mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + mTimeStep *
mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mExternalForces[i];
mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + mTimeStep *
mRigidBodyComponents.mInverseInertiaTensorsWorld[i] * mRigidBodyComponents.mExternalTorques[i];
}
// Apply gravity force
for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
// If the gravity has to be applied to this rigid body
if (mDynamicsComponents.mIsGravityEnabled[i] && mIsGravityEnabled) {
if (mRigidBodyComponents.mIsGravityEnabled[i] && mIsGravityEnabled) {
// Integrate the gravity force
mDynamicsComponents.mConstrainedLinearVelocities[i] = mDynamicsComponents.mConstrainedLinearVelocities[i] + mTimeStep *
mDynamicsComponents.mInverseMasses[i] * mDynamicsComponents.mInitMasses[i] * mGravity;
mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] + mTimeStep *
mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mInitMasses[i] * mGravity;
}
}
@ -294,14 +294,14 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
// Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ...
// => 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<Joint*>::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;
}
}
}

7
src/engine/DynamicsWorld.h
View File

@ -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();
}
}