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

Use Vector3 instead of Matrix3x3 for the local inertia tensor of a rigid body

Browse Source
This commit is contained in:
Daniel Chappuis 2020-03-10 07:23:51 +01:00
parent cda466f9da
commit 7dde393fa5
22 changed files with 142 additions and 177 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CHANGELOG.md
View File

@ -48,6 +48,7 @@
- The RigidBody::addCollider() method (previously addProxyShape() method) does not take a "mass" parameter anymore.
- The RigidBody::setCenterOfMassLocal() method has been renamed to RigidBody::setLocalCenterOfMass()
- The RigidBody::setInertiaTensorLocal() method has been renamed to RigidBody::setLocalInertiaTensor()
- Now, the local inertia tensor of a rigid body has to be set using a Vector3 instead of a Matrix3x3. You only need to provide the diagonal values of the matrix
- The RigidBody::recomputeMassInformation() method has been renamed to RigidBody::updateMassPropertiesFromColliders.
- Now, you need to manually call the RigidBody::recomputeMassInformation() method after adding colliders to a rigid body to recompute its inertia tensor, center of mass and mass
- The rendering in the testbed application has been improved

80
src/body/RigidBody.cpp
View File

@ -79,7 +79,7 @@ void RigidBody::setType(BodyType type) {
// Reset the inverse mass and inverse inertia tensor to zero
mWorld.mRigidBodyComponents.setMassInverse(mEntity, decimal(0));
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero());
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, Vector3::zero());
}
else { // If it is a dynamic body
@ -93,13 +93,11 @@ void RigidBody::setType(BodyType type) {
}
// Compute the inverse local inertia tensor
const Matrix3x3& inertiaTensorLocal = mWorld.mRigidBodyComponents.getLocalInertiaTensor(mEntity);
if (approxEqual(inertiaTensorLocal.getDeterminant(), decimal(0.0))) {
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero());
}
else {
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, inertiaTensorLocal.getInverse());
}
const Vector3& inertiaTensorLocal = mWorld.mRigidBodyComponents.getLocalInertiaTensor(mEntity);
Vector3 inverseInertiaTensorLocal(inertiaTensorLocal.x != decimal(0.0) ? decimal(1.0) / inertiaTensorLocal.x : 0,
inertiaTensorLocal.y != decimal(0.0) ? decimal(1.0) / inertiaTensorLocal.y : 0,
inertiaTensorLocal.z != decimal(0.0) ? decimal(1.0) / inertiaTensorLocal.z : 0);
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, inverseInertiaTensorLocal);
}
// Awake the body
@ -161,7 +159,7 @@ void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
}
// Return the local inertia tensor of the body (in body coordinates)
const Matrix3x3& RigidBody::getLocalInertiaTensor() const {
const Vector3& RigidBody::getLocalInertiaTensor() const {
return mWorld.mRigidBodyComponents.getLocalInertiaTensor(mEntity);
}
@ -172,17 +170,15 @@ const Matrix3x3& RigidBody::getLocalInertiaTensor() const {
* @param inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space
* coordinates
*/
void RigidBody::setLocalInertiaTensor(const Matrix3x3& inertiaTensorLocal) {
void RigidBody::setLocalInertiaTensor(const Vector3& inertiaTensorLocal) {
mWorld.mRigidBodyComponents.setLocalInertiaTensor(mEntity, inertiaTensorLocal);
// Compute the inverse local inertia tensor
if (approxEqual(inertiaTensorLocal.getDeterminant(), decimal(0.0))) {
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero());
}
else {
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, inertiaTensorLocal.getInverse());
}
Vector3 inverseInertiaTensorLocal(inertiaTensorLocal.x != decimal(0.0) ? decimal(1.0) / inertiaTensorLocal.x : 0,
inertiaTensorLocal.y != decimal(0.0) ? decimal(1.0) / inertiaTensorLocal.y : 0,
inertiaTensorLocal.z != decimal(0.0) ? decimal(1.0) / inertiaTensorLocal.z : 0);
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, inverseInertiaTensorLocal);
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mEntity.id) + ": Set inertiaTensorLocal=" + inertiaTensorLocal.to_string());
@ -313,11 +309,13 @@ Vector3 RigidBody::computeCenterOfMass() const {
}
// Compute the local-space inertia tensor and total mass of the body using its colliders
void RigidBody::computeMassAndInertiaTensorLocal(Matrix3x3& inertiaTensorLocal, decimal& totalMass) const {
void RigidBody::computeMassAndInertiaTensorLocal(Vector3& inertiaTensorLocal, decimal& totalMass) const {
inertiaTensorLocal.setToZero();
totalMass = decimal(0.0);
Matrix3x3 tempLocalInertiaTensor = Matrix3x3::zero();
const Vector3 centerOfMassLocal = mWorld.mRigidBodyComponents.getCenterOfMassLocal(mEntity);
// Compute the inertia tensor using all the colliders
@ -333,13 +331,16 @@ void RigidBody::computeMassAndInertiaTensorLocal(Matrix3x3& inertiaTensorLocal,
totalMass += colliderMass;
// Get the inertia tensor of the collider in its local-space
Matrix3x3 inertiaTensor;
collider->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, colliderMass);
Vector3 shapeLocalInertiaTensor = collider->getCollisionShape()->getLocalInertiaTensor(colliderMass);
// Convert the collider inertia tensor into the local-space of the body
const Transform& shapeTransform = collider->getLocalToBodyTransform();
Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix();
inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose();
Matrix3x3 rotationMatrixTranspose = rotationMatrix.getTranspose();
rotationMatrixTranspose[0] *= shapeLocalInertiaTensor.x;
rotationMatrixTranspose[1] *= shapeLocalInertiaTensor.y;
rotationMatrixTranspose[2] *= shapeLocalInertiaTensor.z;
Matrix3x3 inertiaTensor = rotationMatrix * rotationMatrixTranspose;
// Use the parallel axis theorem to convert the inertia tensor w.r.t the collider
// center into a inertia tensor w.r.t to the body origin.
@ -354,8 +355,11 @@ void RigidBody::computeMassAndInertiaTensorLocal(Matrix3x3& inertiaTensorLocal,
offsetMatrix[2] += offset * (-offset.z);
offsetMatrix *= colliderMass;
inertiaTensorLocal += inertiaTensor + offsetMatrix;
tempLocalInertiaTensor += inertiaTensor + offsetMatrix;
}
// Get the diagonal value of the computed local inertia tensor
inertiaTensorLocal.setAllValues(tempLocalInertiaTensor[0][0], tempLocalInertiaTensor[1][1], tempLocalInertiaTensor[2][2]);
}
// Compute and set the local-space inertia tensor of the body using its colliders
@ -367,19 +371,17 @@ void RigidBody::updateLocalInertiaTensorFromColliders() {
const Vector3 centerOfMassLocal = mWorld.mRigidBodyComponents.getCenterOfMassLocal(mEntity);
// Compute the local-space inertia tensor
Matrix3x3 inertiaTensorLocal;
Vector3 inertiaTensorLocal;
decimal totalMass;
computeMassAndInertiaTensorLocal(inertiaTensorLocal, totalMass);
mWorld.mRigidBodyComponents.setLocalInertiaTensor(mEntity, inertiaTensorLocal);
// Compute the inverse local inertia tensor
if (approxEqual(inertiaTensorLocal.getDeterminant(), decimal(0.0))) {
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero());
}
else {
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, inertiaTensorLocal.getInverse());
}
Vector3 inverseInertiaTensorLocal(inertiaTensorLocal.x != decimal(0.0) ? decimal(1.0) / inertiaTensorLocal.x : 0,
inertiaTensorLocal.y != decimal(0.0) ? decimal(1.0) / inertiaTensorLocal.y : 0,
inertiaTensorLocal.z != decimal(0.0) ? decimal(1.0) / inertiaTensorLocal.z : 0);
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, inverseInertiaTensorLocal);
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mEntity.id) + ": Set inertiaTensorLocal=" + inertiaTensorLocal.to_string());
@ -451,19 +453,17 @@ void RigidBody::updateMassPropertiesFromColliders() {
"Body " + std::to_string(mEntity.id) + ": Set centerOfMassLocal=" + centerOfMassLocal.to_string());
// Compute the mass and local-space inertia tensor
Matrix3x3 inertiaTensorLocal;
Vector3 inertiaTensorLocal;
decimal totalMass;
computeMassAndInertiaTensorLocal(inertiaTensorLocal, totalMass);
mWorld.mRigidBodyComponents.setLocalInertiaTensor(mEntity, inertiaTensorLocal);
// Compute the inverse local inertia tensor
if (approxEqual(inertiaTensorLocal.getDeterminant(), decimal(0.0))) {
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero());
}
else {
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, inertiaTensorLocal.getInverse());
}
Vector3 inverseInertiaTensorLocal(inertiaTensorLocal.x != decimal(0.0) ? decimal(1.0) / inertiaTensorLocal.x : 0,
inertiaTensorLocal.y != decimal(0.0) ? decimal(1.0) / inertiaTensorLocal.y : 0,
inertiaTensorLocal.z != decimal(0.0) ? decimal(1.0) / inertiaTensorLocal.z : 0);
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, inverseInertiaTensorLocal);
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mEntity.id) + ": Set inertiaTensorLocal=" + inertiaTensorLocal.to_string());
@ -800,11 +800,15 @@ void RigidBody::updateOverlappingPairs() {
}
/// Return the inverse of the inertia tensor in world coordinates.
const Matrix3x3 RigidBody::getInertiaTensorInverseWorld(PhysicsWorld& world, Entity bodyEntity) {
const Matrix3x3 RigidBody::getWorldInertiaTensorInverse(PhysicsWorld& world, Entity bodyEntity) {
Matrix3x3 orientation = world.mTransformComponents.getTransform(bodyEntity).getOrientation().getMatrix();
const Matrix3x3& inverseInertiaLocalTensor = world.mRigidBodyComponents.getInertiaTensorLocalInverse(bodyEntity);
return orientation * inverseInertiaLocalTensor * orientation.getTranspose();
const Vector3& inverseInertiaLocalTensor = world.mRigidBodyComponents.getInertiaTensorLocalInverse(bodyEntity);
Matrix3x3 orientationTranspose = orientation.getTranspose();
orientationTranspose[0] *= inverseInertiaLocalTensor.x;
orientationTranspose[1] *= inverseInertiaLocalTensor.y;
orientationTranspose[2] *= inverseInertiaLocalTensor.z;
return orientation * orientationTranspose;
}
// Set whether or not the body is allowed to go to sleep

8
src/body/RigidBody.h
View File

@ -63,10 +63,10 @@ class RigidBody : public CollisionBody {
Vector3 computeCenterOfMass() const;
/// Compute the local-space inertia tensor and total mass of the body using its colliders
void computeMassAndInertiaTensorLocal(Matrix3x3& inertiaTensorLocal, decimal& totalMass) const;
void computeMassAndInertiaTensorLocal(Vector3& inertiaTensorLocal, decimal& totalMass) const;
/// Return the inverse of the inertia tensor in world coordinates.
static const Matrix3x3 getInertiaTensorInverseWorld(PhysicsWorld& world, Entity bodyEntity);
static const Matrix3x3 getWorldInertiaTensorInverse(PhysicsWorld& world, Entity bodyEntity);
public :
@ -106,10 +106,10 @@ class RigidBody : public CollisionBody {
void setAngularVelocity(const Vector3& angularVelocity);
/// Return the local inertia tensor of the body (in body coordinates)
const Matrix3x3& getLocalInertiaTensor() const;
const Vector3& getLocalInertiaTensor() const;
/// Set the local inertia tensor of the body (in body coordinates)
void setLocalInertiaTensor(const Matrix3x3& inertiaTensorLocal);
void setLocalInertiaTensor(const Vector3& inertiaTensorLocal);
/// Return the center of mass of the body (in local-space coordinates)
const Vector3& getLocalCenterOfMass() const;

16
src/collision/shapes/BoxShape.cpp
View File

@ -81,18 +81,14 @@ BoxShape::BoxShape(const Vector3& halfExtents, MemoryAllocator& allocator)
// Return the local inertia tensor of the collision shape
/**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal factor = (decimal(1.0) / decimal(3.0)) * mass;
decimal xSquare = mHalfExtents.x * mHalfExtents.x;
decimal ySquare = mHalfExtents.y * mHalfExtents.y;
decimal zSquare = mHalfExtents.z * mHalfExtents.z;
tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
0.0, factor * (xSquare + zSquare), 0.0,
0.0, 0.0, factor * (xSquare + ySquare));
Vector3 BoxShape::getLocalInertiaTensor(decimal mass) const {
const decimal factor = (decimal(1.0) / decimal(3.0)) * mass;
const decimal xSquare = mHalfExtents.x * mHalfExtents.x;
const decimal ySquare = mHalfExtents.y * mHalfExtents.y;
const decimal zSquare = mHalfExtents.z * mHalfExtents.z;
return Vector3(factor * (ySquare + zSquare), factor * (xSquare + zSquare), factor * (xSquare + ySquare));
}
// Raycast method with feedback information

2
src/collision/shapes/BoxShape.h
View File

@ -96,7 +96,7 @@ class BoxShape : public ConvexPolyhedronShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
virtual Vector3 getLocalInertiaTensor(decimal mass) const override;
/// Compute and return the volume of the collision shape
virtual decimal getVolume() const override;

30
src/collision/shapes/CapsuleShape.cpp
View File

@ -46,28 +46,24 @@ CapsuleShape::CapsuleShape(decimal radius, decimal height, MemoryAllocator& allo
// Return the local inertia tensor of the capsule
/**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
Vector3 CapsuleShape::getLocalInertiaTensor(decimal mass) const {
// The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1
decimal height = mHalfHeight + mHalfHeight;
decimal radiusSquare = mMargin * mMargin;
decimal heightSquare = height * height;
decimal radiusSquareDouble = radiusSquare + radiusSquare;
decimal factor1 = decimal(2.0) * mMargin / (decimal(4.0) * mMargin + decimal(3.0) * height);
decimal factor2 = decimal(3.0) * height / (decimal(4.0) * mMargin + decimal(3.0) * height);
decimal sum1 = decimal(0.4) * radiusSquareDouble;
decimal sum2 = decimal(0.75) * height * mMargin + decimal(0.5) * heightSquare;
decimal sum3 = decimal(0.25) * radiusSquare + decimal(1.0 / 12.0) * heightSquare;
decimal IxxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3;
decimal Iyy = factor1 * mass * sum1 + factor2 * mass * decimal(0.25) * radiusSquareDouble;
tensor.setAllValues(IxxAndzz, 0.0, 0.0,
0.0, Iyy, 0.0,
0.0, 0.0, IxxAndzz);
const decimal height = mHalfHeight + mHalfHeight;
const decimal radiusSquare = mMargin * mMargin;
const decimal heightSquare = height * height;
const decimal radiusSquareDouble = radiusSquare + radiusSquare;
const decimal factor1 = decimal(2.0) * mMargin / (decimal(4.0) * mMargin + decimal(3.0) * height);
const decimal factor2 = decimal(3.0) * height / (decimal(4.0) * mMargin + decimal(3.0) * height);
const decimal sum1 = decimal(0.4) * radiusSquareDouble;
const decimal sum2 = decimal(0.75) * height * mMargin + decimal(0.5) * heightSquare;
const decimal sum3 = decimal(0.25) * radiusSquare + decimal(1.0 / 12.0) * heightSquare;
const decimal IxxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3;
const decimal Iyy = factor1 * mass * sum1 + factor2 * mass * decimal(0.25) * radiusSquareDouble;
return Vector3(IxxAndzz, Iyy, IxxAndzz);
}
// Return true if a point is inside the collision shape

2
src/collision/shapes/CapsuleShape.h
View File

@ -112,7 +112,7 @@ class CapsuleShape : public ConvexShape {
virtual bool isPolyhedron() const override;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
virtual Vector3 getLocalInertiaTensor(decimal mass) const override;
/// Return the string representation of the shape
virtual std::string to_string() const override;

2
src/collision/shapes/CollisionShape.h
View File

@ -141,7 +141,7 @@ class CollisionShape {
uint32 getId() const;
/// Return the local inertia tensor of the collision shapes
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0;
virtual Vector3 getLocalInertiaTensor(decimal mass) const=0;
/// Compute and return the volume of the collision shape
virtual decimal getVolume() const=0;

20
src/collision/shapes/ConcaveMeshShape.h
View File

@ -193,9 +193,6 @@ class ConcaveMeshShape : public ConcaveShape {
/// Return the local bounds of the shape in x, y and z directions.
virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
/// Return the string representation of the shape
virtual std::string to_string() const override;
@ -233,23 +230,6 @@ inline void ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
max = treeAABB.getMax();
}
// Return the local inertia tensor of the shape
/**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
// Default inertia tensor
// Note that this is not very realistic for a concave triangle mesh.
// However, in most cases, it will only be used static bodies and therefore,
// the inertia tensor is not used.
tensor.setAllValues(mass, 0, 0,
0, mass, 0,
0, 0, mass);
}
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId) {

16
src/collision/shapes/ConcaveShape.h
View File

@ -101,6 +101,9 @@ class ConcaveShape : public CollisionShape {
/// Set the scale of the shape
void setScale(const Vector3& scale);
/// Return the local inertia tensor of the collision shape
virtual Vector3 getLocalInertiaTensor(decimal mass) const override;
/// Return true if the collision shape is convex, false if it is concave
virtual bool isConvex() const override;
@ -158,6 +161,19 @@ inline void ConcaveShape::setScale(const Vector3& scale) {
notifyColliderAboutChangedSize();
}
// Return the local inertia tensor of the shape
/**
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline Vector3 ConcaveShape::getLocalInertiaTensor(decimal mass) const {
// Default inertia tensor
// Note that this is not very realistic for a concave triangle mesh.
// However, in most cases, it will only be used static bodies and therefore,
// the inertia tensor is not used.
return Vector3(mass, mass, mass);
}
}

20
src/collision/shapes/ConvexMeshShape.h
View File

@ -106,7 +106,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
/// Return the local inertia tensor of the collision shape.
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
virtual Vector3 getLocalInertiaTensor(decimal mass) const override;
/// Return the number of faces of the polyhedron
virtual uint getNbFaces() const override;
@ -179,20 +179,16 @@ inline void ConvexMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
/// The local inertia tensor of the convex mesh is approximated using the inertia tensor
/// of its bounding box.
/**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal factor = (decimal(1.0) / decimal(3.0)) * mass;
Vector3 realExtent = decimal(0.5) * (mMaxBounds - mMinBounds);
inline Vector3 ConvexMeshShape::getLocalInertiaTensor(decimal mass) const {
const decimal factor = (decimal(1.0) / decimal(3.0)) * mass;
const Vector3 realExtent = decimal(0.5) * (mMaxBounds - mMinBounds);
assert(realExtent.x > 0 && realExtent.y > 0 && realExtent.z > 0);
decimal xSquare = realExtent.x * realExtent.x;
decimal ySquare = realExtent.y * realExtent.y;
decimal zSquare = realExtent.z * realExtent.z;
tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
0.0, factor * (xSquare + zSquare), 0.0,
0.0, 0.0, factor * (xSquare + ySquare));
const decimal xSquare = realExtent.x * realExtent.x;
const decimal ySquare = realExtent.y * realExtent.y;
const decimal zSquare = realExtent.z * realExtent.z;
return Vector3(factor * (ySquare + zSquare), factor * (xSquare + zSquare), factor * (xSquare + ySquare));
}
// Return the number of faces of the polyhedron

20
src/collision/shapes/HeightFieldShape.h
View File

@ -151,9 +151,6 @@ class HeightFieldShape : public ConcaveShape {
/// Return the local bounds of the shape in x, y and z directions.
virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
/// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void computeOverlappingTriangles(const AABB& localAABB, List<Vector3>& triangleVertices,
List<Vector3>& triangleVerticesNormals, List<uint>& shapeIds,
@ -205,23 +202,6 @@ inline int HeightFieldShape::computeIntegerGridValue(decimal value) const {
return (value < decimal(0.0)) ? value - decimal(0.5) : value + decimal(0.5);
}
// Return the local inertia tensor
/**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void HeightFieldShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
// Default inertia tensor
// Note that this is not very realistic for a concave triangle mesh.
// However, in most cases, it will only be used static bodies and therefore,
// the inertia tensor is not used.
tensor.setAllValues(mass, 0, 0,
0, mass, 0,
0, 0, mass);
}
// Compute the shape Id for a given triangle
inline uint HeightFieldShape::computeTriangleShapeId(uint iIndex, uint jIndex, uint secondTriangleIncrement) const {

10
src/collision/shapes/SphereShape.h
View File

@ -91,7 +91,7 @@ class SphereShape : public ConvexShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
virtual Vector3 getLocalInertiaTensor(decimal mass) const override;
/// Compute and return the volume of the collision shape
virtual decimal getVolume() const override;
@ -172,15 +172,11 @@ inline void SphereShape::getLocalBounds(Vector3& min, Vector3& max) const {
// Return the local inertia tensor of the sphere
/**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
inline Vector3 SphereShape::getLocalInertiaTensor(decimal mass) const {
decimal diag = decimal(0.4) * mass * mMargin * mMargin;
tensor.setAllValues(diag, 0.0, 0.0,
0.0, diag, 0.0,
0.0, 0.0, diag);
return Vector3(diag, diag, diag);
}
// Compute and return the volume of the collision shape

6
src/collision/shapes/TriangleShape.h
View File

@ -128,7 +128,7 @@ class TriangleShape : public ConvexPolyhedronShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
virtual Vector3 getLocalInertiaTensor(decimal mass) const override;
/// Update the AABB of a body using its collision shape
virtual void computeAABB(AABB& aabb, const Transform& transform) const override;
@ -222,8 +222,8 @@ inline void TriangleShape::getLocalBounds(Vector3& min, Vector3& max) const {
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void TriangleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
tensor.setToZero();
inline Vector3 TriangleShape::getLocalInertiaTensor(decimal mass) const {
return Vector3(0, 0, 0);
}
// Return true if a point is inside the collision shape

28
src/components/RigidBodyComponents.cpp
View File

@ -39,8 +39,8 @@ RigidBodyComponents::RigidBodyComponents(MemoryAllocator& allocator)
sizeof(bool) + sizeof(bool) + sizeof(decimal) + sizeof(BodyType) +
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(decimal) + sizeof(decimal) + sizeof(Vector3) +
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) +
sizeof(bool) + sizeof(bool) + sizeof(List<Entity>)) {
@ -76,8 +76,8 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) {
decimal* newAngularDampings = reinterpret_cast<decimal*>(newLinearDampings + nbComponentsToAllocate);
decimal* newMasses = reinterpret_cast<decimal*>(newAngularDampings + nbComponentsToAllocate);
decimal* newInverseMasses = reinterpret_cast<decimal*>(newMasses + nbComponentsToAllocate);
Matrix3x3* newInertiaTensorLocal = reinterpret_cast<Matrix3x3*>(newInverseMasses + nbComponentsToAllocate);
Matrix3x3* newInertiaTensorLocalInverses = reinterpret_cast<Matrix3x3*>(newInertiaTensorLocal + nbComponentsToAllocate);
Vector3* newInertiaTensorLocal = reinterpret_cast<Vector3*>(newInverseMasses + nbComponentsToAllocate);
Vector3* newInertiaTensorLocalInverses = reinterpret_cast<Vector3*>(newInertiaTensorLocal + nbComponentsToAllocate);
Vector3* newConstrainedLinearVelocities = reinterpret_cast<Vector3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate);
Vector3* newConstrainedAngularVelocities = reinterpret_cast<Vector3*>(newConstrainedLinearVelocities + nbComponentsToAllocate);
Vector3* newSplitLinearVelocities = reinterpret_cast<Vector3*>(newConstrainedAngularVelocities + nbComponentsToAllocate);
@ -108,8 +108,8 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) {
memcpy(newAngularDampings, mAngularDampings, mNbComponents * sizeof(decimal));
memcpy(newMasses, mMasses, mNbComponents * sizeof(decimal));
memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal));
memcpy(newInertiaTensorLocal, mLocalInertiaTensors, mNbComponents * sizeof(Matrix3x3));
memcpy(newInertiaTensorLocalInverses, mInverseInertiaTensorsLocal, mNbComponents * sizeof(Matrix3x3));
memcpy(newInertiaTensorLocal, mLocalInertiaTensors, mNbComponents * sizeof(Vector3));
memcpy(newInertiaTensorLocalInverses, mInverseInertiaTensorsLocal, mNbComponents * sizeof(Vector3));
memcpy(newConstrainedLinearVelocities, mConstrainedLinearVelocities, mNbComponents * sizeof(Vector3));
memcpy(newConstrainedAngularVelocities, mConstrainedAngularVelocities, mNbComponents * sizeof(Vector3));
memcpy(newSplitLinearVelocities, mSplitLinearVelocities, mNbComponents * sizeof(Vector3));
@ -178,8 +178,8 @@ void RigidBodyComponents::addComponent(Entity bodyEntity, bool isSleeping, const
mAngularDampings[index] = decimal(0.0);
mMasses[index] = decimal(1.0);
mInverseMasses[index] = decimal(1.0);
new (mLocalInertiaTensors + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 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 (mLocalInertiaTensors + index) Vector3(1.0, 1.0, 1.0);
new (mInverseInertiaTensorsLocal + index) Vector3(1.0, 1.0, 1.0);
new (mConstrainedLinearVelocities + index) Vector3(0, 0, 0);
new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0);
new (mSplitLinearVelocities + index) Vector3(0, 0, 0);
@ -222,8 +222,8 @@ void RigidBodyComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex
mAngularDampings[destIndex] = mAngularDampings[srcIndex];
mMasses[destIndex] = mMasses[srcIndex];
mInverseMasses[destIndex] = mInverseMasses[srcIndex];
new (mLocalInertiaTensors + destIndex) Matrix3x3(mLocalInertiaTensors[srcIndex]);
new (mInverseInertiaTensorsLocal + destIndex) Matrix3x3(mInverseInertiaTensorsLocal[srcIndex]);
new (mLocalInertiaTensors + destIndex) Vector3(mLocalInertiaTensors[srcIndex]);
new (mInverseInertiaTensorsLocal + destIndex) Vector3(mInverseInertiaTensorsLocal[srcIndex]);
new (mConstrainedLinearVelocities + destIndex) Vector3(mConstrainedLinearVelocities[srcIndex]);
new (mConstrainedAngularVelocities + destIndex) Vector3(mConstrainedAngularVelocities[srcIndex]);
new (mSplitLinearVelocities + destIndex) Vector3(mSplitLinearVelocities[srcIndex]);
@ -265,8 +265,8 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) {
decimal angularDamping1 = mAngularDampings[index1];
decimal mass1 = mMasses[index1];
decimal inverseMass1 = mInverseMasses[index1];
Matrix3x3 inertiaTensorLocal1 = mLocalInertiaTensors[index1];
Matrix3x3 inertiaTensorLocalInverse1 = mInverseInertiaTensorsLocal[index1];
Vector3 inertiaTensorLocal1 = mLocalInertiaTensors[index1];
Vector3 inertiaTensorLocalInverse1 = mInverseInertiaTensorsLocal[index1];
Vector3 constrainedLinearVelocity1(mConstrainedLinearVelocities[index1]);
Vector3 constrainedAngularVelocity1(mConstrainedAngularVelocities[index1]);
Vector3 splitLinearVelocity1(mSplitLinearVelocities[index1]);
@ -336,8 +336,8 @@ void RigidBodyComponents::destroyComponent(uint32 index) {
mAngularVelocities[index].~Vector3();
mExternalForces[index].~Vector3();
mExternalTorques[index].~Vector3();
mLocalInertiaTensors[index].~Matrix3x3();
mInverseInertiaTensorsLocal[index].~Matrix3x3();
mLocalInertiaTensors[index].~Vector3();
mInverseInertiaTensorsLocal[index].~Vector3();
mConstrainedLinearVelocities[index].~Vector3();
mConstrainedAngularVelocities[index].~Vector3();
mSplitLinearVelocities[index].~Vector3();

20
src/components/RigidBodyComponents.h
View File

@ -107,11 +107,11 @@ class RigidBodyComponents : public Components {
decimal* mInverseMasses;
/// Array with the inertia tensor of each component
Matrix3x3* mLocalInertiaTensors;
Vector3* mLocalInertiaTensors;
/// Array with the inverse of the inertia tensor of each component
// TODO : We should use a Vector3 here for the diagonal instead of a Matrix3x3
Matrix3x3* mInverseInertiaTensorsLocal;
Vector3* mInverseInertiaTensorsLocal;
/// Array with the constrained linear velocity of each component
Vector3* mConstrainedLinearVelocities;
@ -251,13 +251,13 @@ class RigidBodyComponents : public Components {
void setMassInverse(Entity bodyEntity, decimal inverseMass);
/// Return the local inertia tensor of an entity
const Matrix3x3& getLocalInertiaTensor(Entity bodyEntity);
const Vector3& getLocalInertiaTensor(Entity bodyEntity);
/// Set the local inertia tensor of an entity
void setLocalInertiaTensor(Entity bodyEntity, const Matrix3x3& inertiaTensorLocal);
void setLocalInertiaTensor(Entity bodyEntity, const Vector3& inertiaTensorLocal);
/// Return the inverse local inertia tensor of an entity
const Matrix3x3& getInertiaTensorLocalInverse(Entity bodyEntity);
const Vector3& getInertiaTensorLocalInverse(Entity bodyEntity);
/// Set the external force of an entity
void setExternalForce(Entity bodyEntity, const Vector3& externalForce);
@ -272,7 +272,7 @@ class RigidBodyComponents : public Components {
void setAngularDamping(Entity bodyEntity, decimal angularDamping);
/// Set the inverse local inertia tensor of an entity
void setInverseInertiaTensorLocal(Entity bodyEntity, const Matrix3x3& inertiaTensorLocalInverse);
void setInverseInertiaTensorLocal(Entity bodyEntity, const Vector3& inertiaTensorLocalInverse);
/// Return the constrained linear velocity of an entity
const Vector3& getConstrainedLinearVelocity(Entity bodyEntity) const;
@ -511,7 +511,7 @@ inline decimal RigidBodyComponents::getMassInverse(Entity bodyEntity) const {
}
// Return the inverse local inertia tensor of an entity
inline const Matrix3x3& RigidBodyComponents::getInertiaTensorLocalInverse(Entity bodyEntity) {
inline const Vector3& RigidBodyComponents::getInertiaTensorLocalInverse(Entity bodyEntity) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
@ -567,7 +567,7 @@ inline void RigidBodyComponents::setMassInverse(Entity bodyEntity, decimal inver
}
// Return the local inertia tensor of an entity
inline const Matrix3x3& RigidBodyComponents::getLocalInertiaTensor(Entity bodyEntity) {
inline const Vector3& RigidBodyComponents::getLocalInertiaTensor(Entity bodyEntity) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
@ -575,7 +575,7 @@ inline const Matrix3x3& RigidBodyComponents::getLocalInertiaTensor(Entity bodyEn
}
// Set the local inertia tensor of an entity
inline void RigidBodyComponents::setLocalInertiaTensor(Entity bodyEntity, const Matrix3x3& inertiaTensorLocal) {
inline void RigidBodyComponents::setLocalInertiaTensor(Entity bodyEntity, const Vector3& inertiaTensorLocal) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
@ -583,7 +583,7 @@ inline void RigidBodyComponents::setLocalInertiaTensor(Entity bodyEntity, const
}
// Set the inverse local inertia tensor of an entity
inline void RigidBodyComponents::setInverseInertiaTensorLocal(Entity bodyEntity, const Matrix3x3& inertiaTensorLocalInverse) {
inline void RigidBodyComponents::setInverseInertiaTensorLocal(Entity bodyEntity, const Vector3& inertiaTensorLocalInverse) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));

4
src/systems/ContactSolverSystem.cpp
View File

@ -146,8 +146,8 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) {
new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver();
mContactConstraints[mNbContactManifolds].rigidBodyComponentIndexBody1 = rigidBodyIndex1;
mContactConstraints[mNbContactManifolds].rigidBodyComponentIndexBody2 = rigidBodyIndex2;
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = RigidBody::getInertiaTensorInverseWorld(mWorld, externalManifold.bodyEntity1);
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = RigidBody::getInertiaTensorInverseWorld(mWorld, externalManifold.bodyEntity2);
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = RigidBody::getWorldInertiaTensorInverse(mWorld, externalManifold.bodyEntity1);
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = RigidBody::getWorldInertiaTensorInverse(mWorld, externalManifold.bodyEntity2);
mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex1];
mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex2];
mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.nbContactPoints;

2
src/systems/DynamicsSystem.cpp
View File

@ -131,7 +131,7 @@ void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) {
mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + timeStep *
mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mExternalForces[i];
mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep *
RigidBody::getInertiaTensorInverseWorld(mWorld, mRigidBodyComponents.mBodiesEntities[i]) * mRigidBodyComponents.mExternalTorques[i];
RigidBody::getWorldInertiaTensorInverse(mWorld, mRigidBodyComponents.mBodiesEntities[i]) * mRigidBodyComponents.mExternalTorques[i];
}
// Apply gravity force

8
src/systems/SolveBallAndSocketJointSystem.cpp
View File

@ -60,8 +60,8 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
// Get the inertia tensor of bodies
mBallAndSocketJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
mBallAndSocketJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
mBallAndSocketJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
mBallAndSocketJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
}
// For each joint
@ -261,8 +261,8 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// Recompute the inverse inertia tensors
mBallAndSocketJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
mBallAndSocketJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
mBallAndSocketJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
mBallAndSocketJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
}
// For each joint component

8
src/systems/SolveFixedJointSystem.cpp
View File

@ -60,8 +60,8 @@ void SolveFixedJointSystem::initBeforeSolve() {
assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
// Get the inertia tensor of bodies
mFixedJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
mFixedJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
mFixedJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
mFixedJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
}
// For each joint
@ -347,8 +347,8 @@ void SolveFixedJointSystem::solvePositionConstraint() {
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// Recompute the inverse inertia tensors
mFixedJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
mFixedJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
mFixedJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
mFixedJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
}
// For each joint

8
src/systems/SolveHingeJointSystem.cpp
View File

@ -60,8 +60,8 @@ void SolveHingeJointSystem::initBeforeSolve() {
assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
// Get the inertia tensor of bodies
mHingeJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
mHingeJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
mHingeJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
mHingeJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
}
// For each joint
@ -540,8 +540,8 @@ void SolveHingeJointSystem::solvePositionConstraint() {
Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// Recompute the inverse inertia tensors
mHingeJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
mHingeJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
mHingeJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
mHingeJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
}
// For each joint component

8
src/systems/SolveSliderJointSystem.cpp
View File

@ -60,8 +60,8 @@ void SolveSliderJointSystem::initBeforeSolve() {
assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
// Get the inertia tensor of bodies
mSliderJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
mSliderJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
mSliderJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
mSliderJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
}
// For each joint
@ -620,8 +620,8 @@ void SolveSliderJointSystem::solvePositionConstraint() {
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// Recompute the inverse inertia tensors
mSliderJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
mSliderJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
mSliderJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
mSliderJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
}
// For each joint component