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

Clean code

Browse Source
This commit is contained in:
Daniel Chappuis 2019-05-21 20:40:11 +02:00
parent 1bc7e0710b
commit 669e74d528
13 changed files with 155 additions and 167 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/body/RigidBody.cpp
View File

@ -40,9 +40,8 @@ using namespace reactphysics3d;
* @param id The ID of the body
*/
RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, Entity entity, bodyindex id)
: CollisionBody(world, entity, id), mArrayIndex(0),
mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
mMaterial(world.mConfig), mJointsList(nullptr), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) {
: CollisionBody(world, entity, id), mMaterial(world.mConfig), mJointsList(nullptr),
mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) {
// Compute the inverse mass
mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
@ -173,7 +172,8 @@ inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
// Add the torque
const Vector3& externalTorque = mWorld.mDynamicsComponents.getExternalTorque(mEntity);
mWorld.mDynamicsComponents.setExternalTorque(mEntity, externalTorque + (point - mCenterOfMassWorld).cross(force));
const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
mWorld.mDynamicsComponents.setExternalTorque(mEntity, externalTorque + (point - centerOfMassWorld).cross(force));
}
// Set the local inertia tensor of the body (in local-space coordinates)
@ -278,16 +278,18 @@ void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) {
mIsCenterOfMassSetByUser = true;
const Vector3 oldCenterOfMass = mCenterOfMassWorld;
mCenterOfMassLocal = centerOfMassLocal;
const Vector3 oldCenterOfMass = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, centerOfMassLocal);
// Compute the center of mass in world-space coordinates
mCenterOfMassWorld = mWorld.mTransformComponents.getTransform(mEntity) * mCenterOfMassLocal;
const Vector3& updatedCenterOfMassLocal = mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity);
mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, mWorld.mTransformComponents.getTransform(mEntity) * updatedCenterOfMassLocal);
// Update the linear velocity of the center of mass
Vector3 linearVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
const Vector3& angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
linearVelocity += angularVelocity.cross(centerOfMassWorld - oldCenterOfMass);
mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity);
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
@ -498,7 +500,9 @@ void RigidBody::updateTransformWithCenterOfMass() {
// Translate the body according to the translation of the center of mass position
Transform& transform = mWorld.mTransformComponents.getTransform(mEntity);
transform.setPosition(mCenterOfMassWorld - transform.getOrientation() * mCenterOfMassLocal);
const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
const Vector3& centerOfMassLocal = mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity);
transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal);
}
// Set a new material for this rigid body
@ -563,15 +567,17 @@ void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
*/
void RigidBody::setTransform(const Transform& transform) {
const Vector3 oldCenterOfMass = mCenterOfMassWorld;
const Vector3 oldCenterOfMass = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
// Compute the new center of mass in world-space coordinates
mCenterOfMassWorld = transform * mCenterOfMassLocal;
const Vector3& centerOfMassLocal = mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity);
mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform * centerOfMassLocal);
// Update the linear velocity of the center of mass
Vector3 linearVelocity = mWorld.mDynamicsComponents.getLinearVelocity(mEntity);
const Vector3& angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
linearVelocity += angularVelocity.cross(centerOfMassWorld - oldCenterOfMass);
mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity);
CollisionBody::setTransform(transform);
@ -591,7 +597,7 @@ void RigidBody::recomputeMassInformation() {
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) mCenterOfMassLocal.setToZero();
if (!mIsCenterOfMassSetByUser) mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, Vector3::zero());
Matrix3x3 inertiaTensorLocal;
inertiaTensorLocal.setToZero();
@ -599,7 +605,7 @@ void RigidBody::recomputeMassInformation() {
// If it is a STATIC or a KINEMATIC body
if (mType == BodyType::STATIC || mType == BodyType::KINEMATIC) {
mCenterOfMassWorld = transform.getPosition();
mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform.getPosition());
return;
}
@ -612,7 +618,8 @@ void RigidBody::recomputeMassInformation() {
mWorld.mDynamicsComponents.setInitMass(mEntity, mWorld.mDynamicsComponents.getInitMass(mEntity) + proxyShape->getMass());
if (!mIsCenterOfMassSetByUser) {
mCenterOfMassLocal += proxyShape->getLocalToBodyTransform().getPosition() * proxyShape->getMass();
mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity) +
proxyShape->getLocalToBodyTransform().getPosition() * proxyShape->getMass());
}
}
@ -620,18 +627,18 @@ void RigidBody::recomputeMassInformation() {
mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
}
else {
mCenterOfMassWorld = transform.getPosition();
mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform.getPosition());
return;
}
// Compute the center of mass
const Vector3 oldCenterOfMass = mCenterOfMassWorld;
const Vector3 oldCenterOfMass = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
if (!mIsCenterOfMassSetByUser) {
mCenterOfMassLocal *= mWorld.mDynamicsComponents.getMassInverse(mEntity);
mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity) * mWorld.mDynamicsComponents.getMassInverse(mEntity));
}
mCenterOfMassWorld = transform * mCenterOfMassLocal;
mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform * mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity));
if (!mIsInertiaTensorSetByUser) {
@ -652,7 +659,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() - mCenterOfMassLocal;
Vector3 offset = shapeTransform.getPosition() - mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity);
decimal offsetSquare = offset.lengthSquare();
Matrix3x3 offsetMatrix;
offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0));
@ -676,7 +683,7 @@ void RigidBody::recomputeMassInformation() {
// Update the linear velocity of the center of mass
Vector3 linearVelocity = mWorld.mDynamicsComponents.getLinearVelocity(mEntity);
Vector3 angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
linearVelocity += angularVelocity.cross(mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity) - oldCenterOfMass);
mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity);
}

13
src/body/RigidBody.h
View File

@ -50,23 +50,10 @@ class MemoryManager;
*/
class RigidBody : public CollisionBody {
private :
/// Index of the body in arrays for contact/constraint solver
// TODO : REMOVE THIS
uint mArrayIndex;
protected :
// -------------------- Attributes -------------------- //
/// Center of mass of the body in local-space coordinates.
/// The center of mass can therefore be different from the body origin
Vector3 mCenterOfMassLocal;
/// Center of mass of the body in world-space coordinates
Vector3 mCenterOfMassWorld;
/// Inverse Local inertia tensor of the body (in local-space) set
/// by the user with respect to the center of mass of the body
Matrix3x3 mUserInertiaTensorLocalInverse;

33
src/components/DynamicsComponents.cpp
View File

@ -38,7 +38,8 @@ DynamicsComponents::DynamicsComponents(MemoryAllocator& allocator)
:Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof(Vector3) + 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(bool) + sizeof(bool)) {
sizeof(Vector3) + sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) + sizeof(bool) +
sizeof(bool)) {
// Allocate memory for the components data
allocate(INIT_NB_ALLOCATED_COMPONENTS);
@ -74,7 +75,9 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast<Matrix3x3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate);
Vector3* newConstrainedPositions = reinterpret_cast<Vector3*>(newInertiaTensorWorldInverses + nbComponentsToAllocate);
Quaternion* newConstrainedOrientations = reinterpret_cast<Quaternion*>(newConstrainedPositions + nbComponentsToAllocate);
bool* newIsGravityEnabled = reinterpret_cast<bool*>(newConstrainedOrientations + 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
@ -98,6 +101,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
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));
@ -123,6 +128,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
mInverseInertiaTensorsWorld = newInertiaTensorWorldInverses;
mConstrainedPositions = newConstrainedPositions;
mConstrainedOrientations = newConstrainedOrientations;
mCentersOfMassLocal = newCentersOfMassLocal;
mCentersOfMassWorld = newCentersOfMassWorld;
mIsGravityEnabled = newIsGravityEnabled;
mIsAlreadyInIsland = newIsAlreadyInIsland;
mNbAllocatedComponents = nbComponentsToAllocate;
@ -136,8 +143,8 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const
// Insert the new component data
new (mBodies + index) Entity(bodyEntity);
new (mLinearVelocities + index) Vector3(component.linearVelocity);
new (mAngularVelocities + index) Vector3(component.angularVelocity);
new (mLinearVelocities + index) Vector3(0, 0, 0);
new (mAngularVelocities + index) Vector3(0, 0, 0);
new (mConstrainedLinearVelocities + index) Vector3(0, 0, 0);
new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0);
new (mSplitLinearVelocities + index) Vector3(0, 0, 0);
@ -152,6 +159,8 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const
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;
@ -184,10 +193,12 @@ void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex)
mAngularDampings[destIndex] = mAngularDampings[srcIndex];
mInitMasses[destIndex] = mInitMasses[srcIndex];
mInverseMasses[destIndex] = mInverseMasses[srcIndex];
mInverseInertiaTensorsLocal[destIndex] = mInverseInertiaTensorsLocal[srcIndex];
mInverseInertiaTensorsWorld[destIndex] = mInverseInertiaTensorsWorld[srcIndex];
mConstrainedPositions[destIndex] = mConstrainedPositions[srcIndex];
mConstrainedOrientations[destIndex] = mConstrainedOrientations[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];
@ -226,6 +237,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
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];
@ -252,6 +265,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
mInverseInertiaTensorsWorld[index2] = inertiaTensorWorldInverse1;
mConstrainedPositions[index2] = constrainedPosition1;
mConstrainedOrientations[index2] = constrainedOrientation1;
mCentersOfMassLocal[index2] = centerOfMassLocal1;
mCentersOfMassWorld[index2] = centerOfMassWorld1;
mIsGravityEnabled[index2] = isGravityEnabled1;
mIsAlreadyInIsland[index2] = isAlreadyInIsland1;
@ -285,4 +300,6 @@ void DynamicsComponents::destroyComponent(uint32 index) {
mInverseInertiaTensorsWorld[index].~Matrix3x3();
mConstrainedPositions[index].~Vector3();
mConstrainedOrientations[index].~Quaternion();
mCentersOfMassLocal[index].~Vector3();
mCentersOfMassWorld[index].~Vector3();
}

57
src/components/DynamicsComponents.h
View File

@ -102,6 +102,12 @@ class DynamicsComponents : public Components {
/// 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;
@ -127,12 +133,11 @@ class DynamicsComponents : public Components {
/// Structure for the data of a transform component
struct DynamicsComponent {
const Vector3& linearVelocity;
const Vector3& angularVelocity;
const Vector3& worldPosition;
/// Constructor
DynamicsComponent(const Vector3& linearVelocity, const Vector3& angularVelocity)
: linearVelocity(linearVelocity), angularVelocity(angularVelocity) {
DynamicsComponent(const Vector3& worldPosition)
: worldPosition(worldPosition) {
}
};
@ -196,6 +201,12 @@ class DynamicsComponents : public Components {
/// 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;
@ -250,6 +261,12 @@ class DynamicsComponents : public Components {
/// 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
bool setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled);
@ -412,6 +429,22 @@ inline const Quaternion& DynamicsComponents::getConstrainedOrientation(Entity bo
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) {
@ -524,6 +557,22 @@ inline void DynamicsComponents::setConstrainedOrientation(Entity bodyEntity, con
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 {

8
src/constraint/BallAndSocketJoint.cpp
View File

@ -45,13 +45,9 @@ BallAndSocketJoint::BallAndSocketJoint(uint id, const BallAndSocketJointInfo& jo
// Initialize before solving the constraint
void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the velocity array
mIndexBody1 = mBody1->mArrayIndex;
mIndexBody2 = mBody2->mArrayIndex;
// Get the bodies center of mass and orientations
const Vector3& x1 = mBody1->mCenterOfMassWorld;
const Vector3& x2 = mBody2->mCenterOfMassWorld;
const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity);
const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity);
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();

8
src/constraint/FixedJoint.cpp
View File

@ -60,13 +60,9 @@ FixedJoint::FixedJoint(uint id, const FixedJointInfo& jointInfo)
// Initialize before solving the constraint
void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the velocity array
mIndexBody1 = mBody1->mArrayIndex;
mIndexBody2 = mBody2->mArrayIndex;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->mCenterOfMassWorld;
const Vector3& x2 = mBody2->mCenterOfMassWorld;
const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity);
const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity);
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();

8
src/constraint/HingeJoint.cpp
View File

@ -67,13 +67,9 @@ HingeJoint::HingeJoint(uint id, const HingeJointInfo& jointInfo)
// Initialize before solving the constraint
void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the velocity array
mIndexBody1 = mBody1->mArrayIndex;
mIndexBody2 = mBody2->mArrayIndex;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->mCenterOfMassWorld;
const Vector3& x2 = mBody2->mCenterOfMassWorld;
const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity);
const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity);
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();

6
src/constraint/Joint.h
View File

@ -140,12 +140,6 @@ class Joint {
/// Type of the joint
const JointType mType;
/// Body 1 index in the velocity array to solve the constraint
uint mIndexBody1;
/// Body 2 index in the velocity array to solve the constraint
uint mIndexBody2;
/// Position correction technique used for the constraint (used for joints)
JointsPositionCorrectionTechnique mPositionCorrectionTechnique;

8
src/constraint/SliderJoint.cpp
View File

@ -75,13 +75,9 @@ SliderJoint::SliderJoint(uint id, const SliderJointInfo& jointInfo)
// Initialize before solving the constraint
void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the veloc ity array
mIndexBody1 = mBody1->mArrayIndex;
mIndexBody2 = mBody2->mArrayIndex;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->mCenterOfMassWorld;
const Vector3& x2 = mBody2->mCenterOfMassWorld;
const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity);
const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity);
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();

6
src/engine/ContactSolver.cpp
View File

@ -131,14 +131,12 @@ void ContactSolver::initializeForIsland(uint islandIndex) {
const ProxyShape* shape2 = mProxyShapeComponents.getProxyShape(externalManifold.proxyShapeEntity2);
// Get the position of the two bodies
const Vector3& x1 = body1->mCenterOfMassWorld;
const Vector3& x2 = body2->mCenterOfMassWorld;
const Vector3& x1 = mDynamicsComponents.getCenterOfMassWorld(externalManifold.bodyEntity1);
const Vector3& x2 = mDynamicsComponents.getCenterOfMassWorld(externalManifold.bodyEntity2);
// Initialize the internal contact manifold structure using the external
// contact manifold
new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver();
mContactConstraints[mNbContactManifolds].indexBody1 = body1->mArrayIndex;
mContactConstraints[mNbContactManifolds].indexBody2 = body2->mArrayIndex;
mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody1 = mDynamicsComponents.getEntityIndex(body1->getEntity());
mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody2 = mDynamicsComponents.getEntityIndex(body2->getEntity());
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();

8
src/engine/ContactSolver.h
View File

@ -174,14 +174,6 @@ class ContactSolver {
/// Pointer to the external contact manifold
ContactManifold* externalContactManifold;
/// Index of body 1 in the constraint solver
// TODO : Remove this
int32 indexBody1;
/// Index of body 2 in the constraint solver
// TODO : Remove this
int32 indexBody2;
/// Index of body 1 in the dynamics components arrays
uint32 dynamicsComponentIndexBody1;

115
src/engine/DynamicsWorld.cpp
View File

@ -163,42 +163,27 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesPositions()", mProfiler);
// TODO : We should loop over non-sleeping dynamic components here and not over islands
const decimal isSplitImpulseActive = mContactSolver.isSplitImpulseActive() ? decimal(1.0) : decimal(0.0);
// For each island of the world
for (uint i=0; i < mIslands.getNbIslands(); i++) {
for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
// For each body of the island
for (uint b=0; b < mIslands.bodyEntities[i].size(); b++) {
// Get the constrained velocity
Vector3 newLinVelocity = mDynamicsComponents.mConstrainedLinearVelocities[i];
Vector3 newAngVelocity = mDynamicsComponents.mConstrainedAngularVelocities[i];
const Entity bodyEntity = mIslands.bodyEntities[i][b];
RigidBody* body = static_cast<RigidBody*>(mBodyComponents.getBody(bodyEntity));
// Add the split impulse velocity from Contact Solver (only used
// to update the position)
newLinVelocity += isSplitImpulseActive * mDynamicsComponents.mSplitLinearVelocities[i];
newAngVelocity += isSplitImpulseActive * mDynamicsComponents.mSplitAngularVelocities[i];
// Get the constrained velocity
Vector3 newLinVelocity = mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity);
Vector3 newAngVelocity = mDynamicsComponents.getConstrainedAngularVelocity(bodyEntity);
// Get current position and orientation of the body
const Vector3& currentPosition = mDynamicsComponents.mCentersOfMassWorld[i];
const Quaternion& currentOrientation = mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]).getOrientation();
// TODO : Remove this
Vector3 testLinVel = newLinVelocity;
// Add the split impulse velocity from Contact Solver (only used
// to update the position)
if (mContactSolver.isSplitImpulseActive()) {
newLinVelocity += mDynamicsComponents.getSplitLinearVelocity(bodyEntity);
newAngVelocity += mDynamicsComponents.getSplitAngularVelocity(bodyEntity);
}
// Get current position and orientation of the body
const Vector3& currentPosition = body->mCenterOfMassWorld;
const Quaternion& currentOrientation = mTransformComponents.getTransform(body->getEntity()).getOrientation();
// Update the new constrained position and orientation of the body
mDynamicsComponents.setConstrainedPosition(bodyEntity, currentPosition + newLinVelocity * mTimeStep);
mDynamicsComponents.setConstrainedOrientation(bodyEntity, currentOrientation +
Quaternion(0, newAngVelocity) *
currentOrientation * decimal(0.5) * mTimeStep);
}
// Update the new constrained position and orientation of the body
mDynamicsComponents.mConstrainedPositions[i] = currentPosition + newLinVelocity * mTimeStep;
mDynamicsComponents.mConstrainedOrientations[i] = currentOrientation + Quaternion(0, newAngVelocity) *
currentOrientation * decimal(0.5) * mTimeStep;
}
}
@ -209,60 +194,41 @@ void DynamicsWorld::updateBodiesState() {
// TODO : Make sure we compute this in a system
// TODO : We should loop over non-sleeping dynamic components here and not over islands
for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
// For each island of the world
for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) {
// Update the linear and angular velocity of the body
mDynamicsComponents.mLinearVelocities[i] = mDynamicsComponents.mConstrainedLinearVelocities[i];
mDynamicsComponents.mAngularVelocities[i] = mDynamicsComponents.mConstrainedAngularVelocities[i];
// For each body of the island
for (uint b=0; b < mIslands.bodyEntities[islandIndex].size(); b++) {
// Update the position of the center of mass of the body
mDynamicsComponents.mCentersOfMassWorld[i] = mDynamicsComponents.mConstrainedPositions[i];
Entity bodyEntity = mIslands.bodyEntities[islandIndex][b];
RigidBody* body = static_cast<RigidBody*>(mBodyComponents.getBody(bodyEntity));
// Update the orientation of the body
const Quaternion& constrainedOrientation = mDynamicsComponents.mConstrainedOrientations[i];
mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]).setOrientation(constrainedOrientation.getUnit());
}
uint index = body->mArrayIndex;
// Update the transform of the body (using the new center of mass and new orientation)
for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
// Update the linear and angular velocity of the body
mDynamicsComponents.setLinearVelocity(bodyEntity, mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity));
mDynamicsComponents.setAngularVelocity(bodyEntity, mDynamicsComponents.getConstrainedAngularVelocity(bodyEntity));
Transform& transform = mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]);
const Vector3& centerOfMassWorld = mDynamicsComponents.mCentersOfMassWorld[i];
const Vector3& centerOfMassLocal = mDynamicsComponents.mCentersOfMassLocal[i];
transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal);
}
// Update the position of the center of mass of the body
body->mCenterOfMassWorld = mDynamicsComponents.getConstrainedPosition(bodyEntity);
// Update the world inverse inertia tensor of the body
for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
// Update the orientation of the body
const Quaternion& constrainedOrientation = mDynamicsComponents.getConstrainedOrientation(bodyEntity);
mTransformComponents.getTransform(bodyEntity).setOrientation(constrainedOrientation.getUnit());
// Update the transform of the body (using the new center of mass and new orientation)
body->updateTransformWithCenterOfMass();
// Update the world inverse inertia tensor of the body
body->updateInertiaTensorInverseWorld();
}
Matrix3x3 orientation = mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]).getOrientation().getMatrix();
const Matrix3x3& inverseInertiaLocalTensor = mDynamicsComponents.mInverseInertiaTensorsLocal[i];
mDynamicsComponents.mInverseInertiaTensorsWorld[i] = orientation * inverseInertiaLocalTensor * orientation.getTranspose();
}
// Update the proxy-shapes components
mCollisionDetection.updateProxyShapes();
}
// Initialize the bodies velocities arrays for the next simulation step.
void DynamicsWorld::initVelocityArrays() {
RP3D_PROFILE("DynamicsWorld::initVelocityArrays()", mProfiler);
// Allocate memory for the bodies velocity arrays
uint nbBodies = mRigidBodies.size();
assert(mDynamicsComponents.getNbComponents() == nbBodies);
// Initialize the map of body indexes in the velocity arrays
uint i = 0;
for (List<RigidBody*>::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
(*it)->mArrayIndex = i++;
}
}
// Reset the split velocities of the bodies
void DynamicsWorld::resetSplitVelocities() {
@ -281,9 +247,6 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()", mProfiler);
// Initialize the bodies velocity arrays
initVelocityArrays();
// Reset the split velocities of the bodies
resetSplitVelocities();
@ -416,7 +379,7 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::max());
mTransformComponents.addComponent(entity, false, TransformComponents::TransformComponent(transform));
mDynamicsComponents.addComponent(entity, false, DynamicsComponents::DynamicsComponent(Vector3::zero(), Vector3::zero()));
mDynamicsComponents.addComponent(entity, false, DynamicsComponents::DynamicsComponent(transform.getPosition()));
// Create the rigid body
RigidBody* rigidBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,

3
src/engine/DynamicsWorld.h
View File

@ -112,9 +112,6 @@ class DynamicsWorld : public CollisionWorld {
/// Reset the external force and torque applied to the bodies
void resetBodiesForceAndTorque();
/// Initialize the bodies velocities arrays for the next simulation step.
void initVelocityArrays();
/// Reset the split velocities of the bodies
void resetSplitVelocities();