Move local-to-world transform of ProxyShape into the ProxyShapeComponents
This commit is contained in:
Daniel Chappuis
parent
5f05fa372d
commit
2c2b75def7
|
|
@ -86,9 +86,10 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape, con
|
|||
Vector3 localBoundsMax;
|
||||
// TODO : Maybe this method can directly returns an AABB
|
||||
collisionShape->getLocalBounds(localBoundsMin, localBoundsMax);
|
||||
const Transform localToWorldTransform = mWorld.mTransformComponents.getTransform(mEntity) * transform;
|
||||
ProxyShapeComponents::ProxyShapeComponent proxyShapeComponent(mEntity, proxyShape,
|
||||
AABB(localBoundsMin, localBoundsMax),
|
||||
transform, collisionShape, decimal(1), 0x0001, 0xFFFF);
|
||||
transform, collisionShape, decimal(1), 0x0001, 0xFFFF, localToWorldTransform);
|
||||
bool isActive = mWorld.mCollisionBodyComponents.getIsActive(mEntity);
|
||||
mWorld.mProxyShapesComponents.addComponent(proxyShapeEntity, !isActive, proxyShapeComponent);
|
||||
|
||||
|
|
@ -218,6 +219,11 @@ void CollisionBody::updateBroadPhaseState(decimal timeStep) const {
|
|||
const List<Entity>& proxyShapesEntities = mWorld.mCollisionBodyComponents.getProxyShapes(mEntity);
|
||||
for (uint i=0; i < proxyShapesEntities.size(); i++) {
|
||||
|
||||
// Update the local-to-world transform of the proxy-shape
|
||||
mWorld.mProxyShapesComponents.setLocalToWorldTransform(proxyShapesEntities[i],
|
||||
mWorld.mTransformComponents.getTransform(mEntity) *
|
||||
mWorld.mProxyShapesComponents.getLocalToBodyTransform(proxyShapesEntities[i]));
|
||||
|
||||
// Update the proxy
|
||||
mWorld.mCollisionDetection.updateProxyShape(proxyShapesEntities[i], timeStep);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -336,10 +336,10 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
|
|||
Vector3 localBoundsMax;
|
||||
// TODO : Maybe this method can directly returns an AABB
|
||||
collisionShape->getLocalBounds(localBoundsMin, localBoundsMax);
|
||||
|
||||
const Transform localToWorldTransform = mWorld.mTransformComponents.getTransform(mEntity) * transform;
|
||||
ProxyShapeComponents::ProxyShapeComponent proxyShapeComponent(mEntity, proxyShape,
|
||||
AABB(localBoundsMin, localBoundsMax),
|
||||
transform, collisionShape, mass, 0x0001, 0xFFFF);
|
||||
transform, collisionShape, mass, 0x0001, 0xFFFF, localToWorldTransform);
|
||||
bool isSleeping = mWorld.mRigidBodyComponents.getIsSleeping(mEntity);
|
||||
mWorld.mProxyShapesComponents.addComponent(proxyShapeEntity, isSleeping, proxyShapeComponent);
|
||||
|
||||
|
|
@ -510,9 +510,6 @@ void RigidBody::setTransform(const Transform& transform) {
|
|||
|
||||
CollisionBody::setTransform(transform);
|
||||
|
||||
// Update the transform of the body
|
||||
mWorld.mTransformComponents.setTransform(mEntity, transform);
|
||||
|
||||
// Awake the body if it is sleeping
|
||||
setIsSleeping(false);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -115,6 +115,10 @@ void ProxyShape::setLocalToBodyTransform(const Transform& transform) {
|
|||
|
||||
mBody->mWorld.mProxyShapesComponents.setLocalToBodyTransform(mEntity, transform);
|
||||
|
||||
// Update the local-to-world transform
|
||||
const Transform& bodyTransform = mBody->mWorld.mTransformComponents.getTransform(mBody->getEntity());
|
||||
mBody->mWorld.mProxyShapesComponents.setLocalToWorldTransform(mEntity, bodyTransform * transform);
|
||||
|
||||
RigidBody* rigidBody = static_cast<RigidBody*>(mBody);
|
||||
if (rigidBody != nullptr) {
|
||||
mBody->mWorld.mRigidBodyComponents.setIsSleeping(mBody->getEntity(), false);
|
||||
|
|
@ -122,8 +126,6 @@ void ProxyShape::setLocalToBodyTransform(const Transform& transform) {
|
|||
|
||||
mBody->mWorld.mCollisionDetection.updateProxyShape(mEntity, 0);
|
||||
|
||||
int broadPhaseId = mBody->mWorld.mProxyShapesComponents.getBroadPhaseId(mEntity);
|
||||
|
||||
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::ProxyShape,
|
||||
"ProxyShape " + std::to_string(broadPhaseId) + ": Set localToBodyTransform=" +
|
||||
transform.to_string());
|
||||
|
|
@ -183,7 +185,7 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
|
|||
if (!mBody->isActive()) return false;
|
||||
|
||||
// Convert the ray into the local-space of the collision shape
|
||||
const Transform localToWorldTransform = getLocalToWorldTransform();
|
||||
const Transform localToWorldTransform = mBody->mWorld.mProxyShapesComponents.getLocalToWorldTransform(mEntity);
|
||||
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
|
||||
Ray rayLocal(worldToLocalTransform * ray.point1,
|
||||
worldToLocalTransform * ray.point2,
|
||||
|
|
@ -222,8 +224,7 @@ unsigned short ProxyShape::getCollideWithMaskBits() const {
|
|||
* shape to the world-space
|
||||
*/
|
||||
const Transform ProxyShape::getLocalToWorldTransform() const {
|
||||
return mBody->mWorld.mTransformComponents.getTransform(mBody->getEntity()) *
|
||||
mBody->mWorld.mProxyShapesComponents.getLocalToBodyTransform(mEntity);
|
||||
return mBody->mWorld.mProxyShapesComponents.getLocalToWorldTransform(mEntity);
|
||||
}
|
||||
|
||||
#ifdef IS_PROFILING_ACTIVE
|
||||
|
|
|
|||
|
|
@ -37,7 +37,7 @@ using namespace reactphysics3d;
|
|||
ProxyShapeComponents::ProxyShapeComponents(MemoryAllocator& allocator)
|
||||
:Components(allocator, sizeof(Entity) + sizeof(Entity) + sizeof(ProxyShape*) + sizeof(int) +
|
||||
sizeof(Transform) + sizeof(CollisionShape*) + sizeof(decimal) + sizeof(unsigned short) +
|
||||
sizeof(unsigned short)) {
|
||||
sizeof(unsigned short) + sizeof(Transform)) {
|
||||
|
||||
// Allocate memory for the components data
|
||||
allocate(INIT_NB_ALLOCATED_COMPONENTS);
|
||||
|
|
@ -65,6 +65,7 @@ void ProxyShapeComponents::allocate(uint32 nbComponentsToAllocate) {
|
|||
decimal* newMasses = reinterpret_cast<decimal*>(newCollisionShapes + nbComponentsToAllocate);
|
||||
unsigned short* newCollisionCategoryBits = reinterpret_cast<unsigned short*>(newMasses + nbComponentsToAllocate);
|
||||
unsigned short* newCollideWithMaskBits = reinterpret_cast<unsigned short*>(newCollisionCategoryBits + nbComponentsToAllocate);
|
||||
Transform* newLocalToWorldTransforms = reinterpret_cast<Transform*>(newCollideWithMaskBits + nbComponentsToAllocate);
|
||||
|
||||
// If there was already components before
|
||||
if (mNbComponents > 0) {
|
||||
|
|
@ -79,6 +80,7 @@ void ProxyShapeComponents::allocate(uint32 nbComponentsToAllocate) {
|
|||
memcpy(newMasses, mMasses, mNbComponents * sizeof(decimal));
|
||||
memcpy(newCollisionCategoryBits, mCollisionCategoryBits, mNbComponents * sizeof(unsigned short));
|
||||
memcpy(newCollideWithMaskBits, mCollideWithMaskBits, mNbComponents * sizeof(unsigned short));
|
||||
memcpy(newLocalToWorldTransforms, mLocalToWorldTransforms, mNbComponents * sizeof(Transform));
|
||||
|
||||
// Deallocate previous memory
|
||||
mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize);
|
||||
|
|
@ -95,6 +97,7 @@ void ProxyShapeComponents::allocate(uint32 nbComponentsToAllocate) {
|
|||
mMasses = newMasses;
|
||||
mCollisionCategoryBits = newCollisionCategoryBits;
|
||||
mCollideWithMaskBits = newCollideWithMaskBits;
|
||||
mLocalToWorldTransforms = newLocalToWorldTransforms;
|
||||
|
||||
mNbAllocatedComponents = nbComponentsToAllocate;
|
||||
}
|
||||
|
|
@ -115,6 +118,7 @@ void ProxyShapeComponents::addComponent(Entity proxyShapeEntity, bool isSleeping
|
|||
new (mMasses + index) decimal(component.mass);
|
||||
new (mCollisionCategoryBits + index) unsigned short(component.collisionCategoryBits);
|
||||
new (mCollideWithMaskBits + index) unsigned short(component.collideWithMaskBits);
|
||||
new (mLocalToWorldTransforms + index) Transform(component.localToWorldTransform);
|
||||
|
||||
// Map the entity with the new component lookup index
|
||||
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(proxyShapeEntity, index));
|
||||
|
|
@ -140,6 +144,7 @@ void ProxyShapeComponents::moveComponentToIndex(uint32 srcIndex, uint32 destInde
|
|||
new (mMasses + destIndex) decimal(mMasses[srcIndex]);
|
||||
new (mCollisionCategoryBits + destIndex) unsigned short(mCollisionCategoryBits[srcIndex]);
|
||||
new (mCollideWithMaskBits + destIndex) unsigned short(mCollideWithMaskBits[srcIndex]);
|
||||
new (mLocalToWorldTransforms + destIndex) Transform(mLocalToWorldTransforms[srcIndex]);
|
||||
|
||||
// Destroy the source component
|
||||
destroyComponent(srcIndex);
|
||||
|
|
@ -165,6 +170,7 @@ void ProxyShapeComponents::swapComponents(uint32 index1, uint32 index2) {
|
|||
decimal mass1 = mMasses[index1];
|
||||
unsigned short collisionCategoryBits1 = mCollisionCategoryBits[index1];
|
||||
unsigned short collideWithMaskBits1 = mCollideWithMaskBits[index1];
|
||||
Transform localToWorldTransform1 = mLocalToWorldTransforms[index1];
|
||||
|
||||
// Destroy component 1
|
||||
destroyComponent(index1);
|
||||
|
|
@ -181,6 +187,7 @@ void ProxyShapeComponents::swapComponents(uint32 index1, uint32 index2) {
|
|||
new (mMasses + index2) decimal(mass1);
|
||||
new (mCollisionCategoryBits + index2) unsigned short(collisionCategoryBits1);
|
||||
new (mCollideWithMaskBits + index2) unsigned short(collideWithMaskBits1);
|
||||
new (mLocalToWorldTransforms + index2) Transform(localToWorldTransform1);
|
||||
|
||||
// Update the entity to component index mapping
|
||||
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(proxyShapeEntity1, index2));
|
||||
|
|
@ -204,4 +211,5 @@ void ProxyShapeComponents::destroyComponent(uint32 index) {
|
|||
mProxyShapes[index] = nullptr;
|
||||
mLocalToBodyTransforms[index].~Transform();
|
||||
mCollisionShapes[index] = nullptr;
|
||||
mLocalToWorldTransforms[index].~Transform();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -65,7 +65,6 @@ class ProxyShapeComponents : public Components {
|
|||
ProxyShape** mProxyShapes;
|
||||
|
||||
/// Ids of the proxy-shapes for the broad-phase algorithm
|
||||
// TODO : Try to change type to uint32
|
||||
int32* mBroadPhaseIds;
|
||||
|
||||
/// Transform from local-space of the proxy-shape to the body-space of its body
|
||||
|
|
@ -90,6 +89,9 @@ class ProxyShapeComponents : public Components {
|
|||
/// proxy shape will collide with every collision categories by default.
|
||||
unsigned short* mCollideWithMaskBits;
|
||||
|
||||
/// Array with the local-to-world transforms of the proxy-shapes
|
||||
Transform* mLocalToWorldTransforms;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Allocate memory for a given number of components
|
||||
|
|
@ -112,18 +114,20 @@ class ProxyShapeComponents : public Components {
|
|||
Entity bodyEntity;
|
||||
ProxyShape* proxyShape;
|
||||
AABB localBounds;
|
||||
Transform localToBodyTransform;
|
||||
const Transform& localToBodyTransform;
|
||||
CollisionShape* collisionShape;
|
||||
decimal mass;
|
||||
unsigned short collisionCategoryBits;
|
||||
unsigned short collideWithMaskBits;
|
||||
const Transform& localToWorldTransform;
|
||||
|
||||
/// Constructor
|
||||
ProxyShapeComponent(Entity bodyEntity, ProxyShape* proxyShape, AABB localBounds, Transform localToBodyTransform,
|
||||
ProxyShapeComponent(Entity bodyEntity, ProxyShape* proxyShape, AABB localBounds, const Transform& localToBodyTransform,
|
||||
CollisionShape* collisionShape, decimal mass, unsigned short collisionCategoryBits,
|
||||
unsigned short collideWithMaskBits)
|
||||
unsigned short collideWithMaskBits, const Transform& localToWorldTransform)
|
||||
:bodyEntity(bodyEntity), proxyShape(proxyShape), localBounds(localBounds), localToBodyTransform(localToBodyTransform),
|
||||
collisionShape(collisionShape), mass(mass), collisionCategoryBits(collisionCategoryBits), collideWithMaskBits(collideWithMaskBits) {
|
||||
collisionShape(collisionShape), mass(mass), collisionCategoryBits(collisionCategoryBits), collideWithMaskBits(collideWithMaskBits),
|
||||
localToWorldTransform(localToWorldTransform) {
|
||||
|
||||
}
|
||||
};
|
||||
|
|
@ -175,6 +179,12 @@ class ProxyShapeComponents : public Components {
|
|||
/// Set the "collide with" mask bits of a given proxy-shape
|
||||
void setCollideWithMaskBits(Entity proxyShapeEntity, unsigned short collideWithMaskBits);
|
||||
|
||||
/// Return the local-to-world transform of a proxy-shape
|
||||
const Transform& getLocalToWorldTransform(Entity proxyShapeEntity) const;
|
||||
|
||||
/// Set the local-to-world transform of a proxy-shape
|
||||
void setLocalToWorldTransform(Entity proxyShapeEntity, const Transform& transform);
|
||||
|
||||
// -------------------- Friendship -------------------- //
|
||||
|
||||
friend class BroadPhaseSystem;
|
||||
|
|
@ -276,6 +286,22 @@ inline void ProxyShapeComponents::setCollideWithMaskBits(Entity proxyShapeEntity
|
|||
mCollideWithMaskBits[mMapEntityToComponentIndex[proxyShapeEntity]] = collideWithMaskBits;
|
||||
}
|
||||
|
||||
// Return the local-to-world transform of a proxy-shape
|
||||
inline const Transform& ProxyShapeComponents::getLocalToWorldTransform(Entity proxyShapeEntity) const {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(proxyShapeEntity));
|
||||
|
||||
return mLocalToWorldTransforms[mMapEntityToComponentIndex[proxyShapeEntity]];
|
||||
}
|
||||
|
||||
// Set the local-to-world transform of a proxy-shape
|
||||
inline void ProxyShapeComponents::setLocalToWorldTransform(Entity proxyShapeEntity, const Transform& transform) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(proxyShapeEntity));
|
||||
|
||||
mLocalToWorldTransforms[mMapEntityToComponentIndex[proxyShapeEntity]] = transform;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -39,8 +39,8 @@ BallAndSocketJoint::BallAndSocketJoint(Entity entity, DynamicsWorld& world, cons
|
|||
: Joint(entity, world) {
|
||||
|
||||
// Get the transforms of the two bodies
|
||||
Transform& body1Transform = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity());
|
||||
Transform& body2Transform = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity());
|
||||
const Transform& body1Transform = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity());
|
||||
const Transform& body2Transform = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity());
|
||||
|
||||
// Compute the local-space anchor point for each body
|
||||
mWorld.mBallAndSocketJointsComponents.setLocalAnchorPointBody1(entity, body1Transform.getInverse() * jointInfo.anchorPointWorldSpace);
|
||||
|
|
|
|||
|
|
@ -40,8 +40,8 @@ HingeJoint::HingeJoint(Entity entity, DynamicsWorld &world, const HingeJointInfo
|
|||
assert(upperLimit >= decimal(0) && upperLimit <= decimal(2.0) * PI);
|
||||
|
||||
// Compute the local-space anchor point for each body
|
||||
Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity());
|
||||
Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity());
|
||||
const Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity());
|
||||
const Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity());
|
||||
mWorld.mHingeJointsComponents.setLocalAnchorPointBody1(mEntity, transform1.getInverse() * jointInfo.anchorPointWorldSpace);
|
||||
mWorld.mHingeJointsComponents.setLocalAnchorPointBody2(mEntity, transform2.getInverse() * jointInfo.anchorPointWorldSpace);
|
||||
|
||||
|
|
|
|||
|
|
@ -55,7 +55,7 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS
|
|||
mConstraintSolverSystem(*this, mIslands, mRigidBodyComponents, mTransformComponents, mJointsComponents,
|
||||
mBallAndSocketJointsComponents, mFixedJointsComponents, mHingeJointsComponents,
|
||||
mSliderJointsComponents),
|
||||
mDynamicsSystem(*this, mRigidBodyComponents, mTransformComponents, mIsGravityEnabled, mGravity),
|
||||
mDynamicsSystem(*this, mCollisionBodyComponents, mRigidBodyComponents, mTransformComponents, mProxyShapesComponents, mIsGravityEnabled, mGravity),
|
||||
mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations),
|
||||
mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations),
|
||||
mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()),
|
||||
|
|
|
|||
|
|
@ -202,7 +202,7 @@ void CollisionDetectionSystem::updateOverlappingPairs(const List<Pair<int, int>>
|
|||
// Compute the middle-phase collision detection
|
||||
void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairMap& overlappingPairs, NarrowPhaseInput& narrowPhaseInput) {
|
||||
|
||||
RP3D_PROFILE("CollisionDetection::computeMiddlePhase()", mProfiler);
|
||||
RP3D_PROFILE("CollisionDetectionSystem::computeMiddlePhase()", mProfiler);
|
||||
|
||||
// Reserve memory for the narrow-phase input using cached capacity from previous frame
|
||||
narrowPhaseInput.reserveMemory();
|
||||
|
|
@ -262,8 +262,9 @@ void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairMap& overlappin
|
|||
// No middle-phase is necessary, simply create a narrow phase info
|
||||
// for the narrow-phase collision detection
|
||||
narrowPhaseInput.addNarrowPhaseTest(pair, shape1->getCollisionShape(), shape2->getCollisionShape(),
|
||||
shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(),
|
||||
algorithmType, mMemoryManager.getSingleFrameAllocator());
|
||||
mProxyShapesComponents.getLocalToWorldTransform(proxyShape1Entity),
|
||||
mProxyShapesComponents.getLocalToWorldTransform(proxyShape2Entity),
|
||||
algorithmType, mMemoryManager.getSingleFrameAllocator());
|
||||
|
||||
}
|
||||
// Concave vs Convex algorithm
|
||||
|
|
@ -318,8 +319,8 @@ void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(OverlappingPair
|
|||
assert(algorithmType != NarrowPhaseAlgorithmType::None);
|
||||
|
||||
// Compute the convex shape AABB in the local-space of the convex shape
|
||||
const Transform convexToConcaveTransform = concaveProxyShape->getLocalToWorldTransform().getInverse() *
|
||||
convexProxyShape->getLocalToWorldTransform();
|
||||
const Transform convexToConcaveTransform = mProxyShapesComponents.getLocalToWorldTransform(concaveProxyShape->getEntity()).getInverse() *
|
||||
mProxyShapesComponents.getLocalToWorldTransform(convexProxyShape->getEntity());
|
||||
AABB aabb;
|
||||
convexShape->computeAABB(aabb, convexToConcaveTransform);
|
||||
|
||||
|
|
@ -356,7 +357,8 @@ void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(OverlappingPair
|
|||
// Create a narrow phase info for the narrow-phase collision detection
|
||||
narrowPhaseInput.addNarrowPhaseTest(pair, isShape1Convex ? convexShape : triangleShape,
|
||||
isShape1Convex ? triangleShape : convexShape,
|
||||
shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(),
|
||||
mProxyShapesComponents.getLocalToWorldTransform(shape1->getEntity()),
|
||||
mProxyShapesComponents.getLocalToWorldTransform(shape2->getEntity()),
|
||||
algorithmType, allocator);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -124,11 +124,6 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) {
|
|||
assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity1));
|
||||
assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity2));
|
||||
|
||||
// Get the two contact shapes
|
||||
// TODO : Do we really need to get the proxy-shape here
|
||||
const ProxyShape* shape1 = mProxyShapeComponents.getProxyShape(externalManifold.proxyShapeEntity1);
|
||||
const ProxyShape* shape2 = mProxyShapeComponents.getProxyShape(externalManifold.proxyShapeEntity2);
|
||||
|
||||
// Get the position of the two bodies
|
||||
const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(externalManifold.bodyEntity1);
|
||||
const Vector3& x2 = mRigidBodyComponents.getCenterOfMassWorld(externalManifold.bodyEntity2);
|
||||
|
|
@ -165,8 +160,8 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) {
|
|||
ContactPoint& externalContact = (*mAllContactPoints)[c];
|
||||
|
||||
// Get the contact point on the two bodies
|
||||
Vector3 p1 = shape1->getLocalToWorldTransform() * externalContact.getLocalPointOnShape1();
|
||||
Vector3 p2 = shape2->getLocalToWorldTransform() * externalContact.getLocalPointOnShape2();
|
||||
Vector3 p1 = mProxyShapeComponents.getLocalToWorldTransform(externalManifold.proxyShapeEntity1) * externalContact.getLocalPointOnShape1();
|
||||
Vector3 p2 = mProxyShapeComponents.getLocalToWorldTransform(externalManifold.proxyShapeEntity2) * externalContact.getLocalPointOnShape2();
|
||||
|
||||
new (mContactPoints + mNbContactPoints) ContactPointSolver();
|
||||
mContactPoints[mNbContactPoints].externalContact = &externalContact;
|
||||
|
|
|
|||
|
|
@ -31,9 +31,10 @@
|
|||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor
|
||||
DynamicsSystem::DynamicsSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, bool& isGravityEnabled, Vector3& gravity)
|
||||
:mWorld(world), mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), mIsGravityEnabled(isGravityEnabled),
|
||||
mGravity(gravity) {
|
||||
DynamicsSystem::DynamicsSystem(DynamicsWorld& world, CollisionBodyComponents& collisionBodyComponents, RigidBodyComponents& rigidBodyComponents,
|
||||
TransformComponents& transformComponents, ProxyShapeComponents& proxyShapeComponents, bool& isGravityEnabled, Vector3& gravity)
|
||||
:mWorld(world), mCollisionBodyComponents(collisionBodyComponents), mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), mProxyShapeComponents(proxyShapeComponents),
|
||||
mIsGravityEnabled(isGravityEnabled), mGravity(gravity) {
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -64,7 +65,7 @@ void DynamicsSystem::integrateRigidBodiesPositions(decimal timeStep, bool isSpli
|
|||
// Update the new constrained position and orientation of the body
|
||||
mRigidBodyComponents.mConstrainedPositions[i] = currentPosition + newLinVelocity * timeStep;
|
||||
mRigidBodyComponents.mConstrainedOrientations[i] = currentOrientation + Quaternion(0, newAngVelocity) *
|
||||
currentOrientation * decimal(0.5) * timeStep;
|
||||
currentOrientation * decimal(0.5) * timeStep;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -95,6 +96,20 @@ void DynamicsSystem::updateBodiesState() {
|
|||
const Vector3& centerOfMassLocal = mRigidBodyComponents.mCentersOfMassLocal[i];
|
||||
transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal);
|
||||
}
|
||||
|
||||
// Update the local-to-world transform of the proxy-shapes
|
||||
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
// For all the proxy collision shapes of the body
|
||||
const List<Entity>& proxyShapesEntities = mCollisionBodyComponents.getProxyShapes(mRigidBodyComponents.mBodiesEntities[i]);
|
||||
for (uint j=0; j < proxyShapesEntities.size(); j++) {
|
||||
|
||||
// Update the local-to-world transform of the proxy-shape
|
||||
mProxyShapeComponents.setLocalToWorldTransform(proxyShapesEntities[j],
|
||||
mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]) *
|
||||
mProxyShapeComponents.getLocalToBodyTransform(proxyShapesEntities[j]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Integrate the velocities of rigid bodies.
|
||||
|
|
|
|||
|
|
@ -28,8 +28,10 @@
|
|||
|
||||
// Libraries
|
||||
#include "utils/Profiler.h"
|
||||
#include "components/CollisionBodyComponents.h"
|
||||
#include "components/RigidBodyComponents.h"
|
||||
#include "components/TransformComponents.h"
|
||||
#include "components/ProxyShapeComponents.h"
|
||||
|
||||
namespace reactphysics3d {
|
||||
|
||||
|
|
@ -49,12 +51,18 @@ class DynamicsSystem {
|
|||
/// Physics world
|
||||
DynamicsWorld& mWorld;
|
||||
|
||||
/// Reference to the collision body components
|
||||
CollisionBodyComponents& mCollisionBodyComponents;
|
||||
|
||||
/// Reference to the rigid body components
|
||||
RigidBodyComponents& mRigidBodyComponents;
|
||||
|
||||
/// Reference to the transform components
|
||||
TransformComponents& mTransformComponents;
|
||||
|
||||
/// Reference to the proxy-shapes components
|
||||
ProxyShapeComponents& mProxyShapeComponents;
|
||||
|
||||
/// Reference to the variable to know if gravity is enabled in the world
|
||||
bool& mIsGravityEnabled;
|
||||
|
||||
|
|
@ -72,8 +80,9 @@ class DynamicsSystem {
|
|||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
DynamicsSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents,
|
||||
bool& isGravityEnabled, Vector3& gravity);
|
||||
DynamicsSystem(DynamicsWorld& world, CollisionBodyComponents& collisionBodyComponents,
|
||||
RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents,
|
||||
ProxyShapeComponents& proxyShapeComponents, bool& isGravityEnabled, Vector3& gravity);
|
||||
|
||||
/// Destructor
|
||||
~DynamicsSystem() = default;
|
||||
|
|
|
|||
|
|
@ -619,7 +619,7 @@ void SolveSliderJointSystem::solvePositionConstraint() {
|
|||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
// Recompute the inertia tensor of bodies
|
||||
// Recompute the inverse inertia tensors
|
||||
mSliderJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
|
||||
mSliderJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
|
||||
}
|
||||
|
|
@ -665,11 +665,11 @@ void SolveSliderJointSystem::solvePositionConstraint() {
|
|||
const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
|
||||
const Vector3& r1 = mSliderJointComponents.getR1(jointEntity);
|
||||
const Vector3& r2 = mSliderJointComponents.getR2(jointEntity);
|
||||
const Vector3& r1 = mSliderJointComponents.mR1[i];
|
||||
const Vector3& r2 = mSliderJointComponents.mR2[i];
|
||||
|
||||
const Vector3& n1 = mSliderJointComponents.getN1(jointEntity);
|
||||
const Vector3& n2 = mSliderJointComponents.getN2(jointEntity);
|
||||
const Vector3& n1 = mSliderJointComponents.mN1[i];
|
||||
const Vector3& n2 = mSliderJointComponents.mN2[i];
|
||||
|
||||
Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
|
||||
Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2];
|
||||
|
|
@ -683,29 +683,29 @@ void SolveSliderJointSystem::solvePositionConstraint() {
|
|||
// Compute the two orthogonal vectors to the slider axis in world-space
|
||||
mSliderJointComponents.mSliderAxisWorld[i] = q1 * mSliderJointComponents.mSliderAxisBody1[i];
|
||||
mSliderJointComponents.mSliderAxisWorld[i].normalize();
|
||||
mSliderJointComponents.setN1(jointEntity, mSliderJointComponents.mSliderAxisWorld[i].getOneUnitOrthogonalVector());
|
||||
mSliderJointComponents.setN2(jointEntity, mSliderJointComponents.mSliderAxisWorld[i].cross(n1));
|
||||
mSliderJointComponents.mN1[i] = mSliderJointComponents.mSliderAxisWorld[i].getOneUnitOrthogonalVector();
|
||||
mSliderJointComponents.mN2[i] = mSliderJointComponents.mSliderAxisWorld[i].cross(n1);
|
||||
|
||||
// Check if the limit constraints are violated or not
|
||||
decimal uDotSliderAxis = u.dot(mSliderJointComponents.mSliderAxisWorld[i]);
|
||||
decimal lowerLimitError = uDotSliderAxis - mSliderJointComponents.getLowerLimit(jointEntity);
|
||||
decimal upperLimitError = mSliderJointComponents.getUpperLimit(jointEntity) - uDotSliderAxis;
|
||||
mSliderJointComponents.setIsLowerLimitViolated(jointEntity, lowerLimitError <= 0);
|
||||
mSliderJointComponents.setIsUpperLimitViolated(jointEntity, upperLimitError <= 0);
|
||||
mSliderJointComponents.mIsLowerLimitViolated[i] = lowerLimitError <= 0;
|
||||
mSliderJointComponents.mIsUpperLimitViolated[i] = upperLimitError <= 0;
|
||||
|
||||
// Compute the cross products used in the Jacobians
|
||||
mSliderJointComponents.setR2CrossN1(jointEntity, r2.cross(n1));
|
||||
mSliderJointComponents.setR2CrossN2(jointEntity, r2.cross(n2));
|
||||
mSliderJointComponents.setR2CrossSliderAxis(jointEntity, r2.cross(mSliderJointComponents.mSliderAxisWorld[i]));
|
||||
mSliderJointComponents.mR2CrossN1[i] = r2.cross(n1);
|
||||
mSliderJointComponents.mR2CrossN2[i] = r2.cross(n2);
|
||||
mSliderJointComponents.mR2CrossSliderAxis[i] = r2.cross(mSliderJointComponents.mSliderAxisWorld[i]);
|
||||
const Vector3 r1PlusU = r1 + u;
|
||||
mSliderJointComponents.setR1PlusUCrossN1(jointEntity, r1PlusU.cross(n1));
|
||||
mSliderJointComponents.setR1PlusUCrossN2(jointEntity, r1PlusU.cross(n2));
|
||||
mSliderJointComponents.setR1PlusUCrossSliderAxis(jointEntity, r1PlusU.cross(mSliderJointComponents.mSliderAxisWorld[i]));
|
||||
mSliderJointComponents.mR1PlusUCrossN1[i] = r1PlusU.cross(n1);
|
||||
mSliderJointComponents.mR1PlusUCrossN2[i] = r1PlusU.cross(n2);
|
||||
mSliderJointComponents.mR1PlusUCrossSliderAxis[i] = r1PlusU.cross(mSliderJointComponents.mSliderAxisWorld[i]);
|
||||
|
||||
const Vector3& r2CrossN1 = mSliderJointComponents.getR2CrossN1(jointEntity);
|
||||
const Vector3& r2CrossN2 = mSliderJointComponents.getR2CrossN2(jointEntity);
|
||||
const Vector3& r1PlusUCrossN1 = mSliderJointComponents.getR1PlusUCrossN1(jointEntity);
|
||||
const Vector3& r1PlusUCrossN2 = mSliderJointComponents.getR1PlusUCrossN2(jointEntity);
|
||||
const Vector3& r2CrossN1 = mSliderJointComponents.mR2CrossN1[i];
|
||||
const Vector3& r2CrossN2 = mSliderJointComponents.mR2CrossN2[i];
|
||||
const Vector3& r1PlusUCrossN1 = mSliderJointComponents.mR1PlusUCrossN1[i];
|
||||
const Vector3& r1PlusUCrossN2 = mSliderJointComponents.mR1PlusUCrossN2[i];
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
|
|
@ -730,18 +730,17 @@ void SolveSliderJointSystem::solvePositionConstraint() {
|
|||
const decimal el22 = sumInverseMass + r1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
|
||||
r2CrossN2.dot(I2R2CrossN2);
|
||||
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
|
||||
Matrix2x2& inverseMassMatrixTranslation = mSliderJointComponents.getInverseMassMatrixTranslation(jointEntity);
|
||||
inverseMassMatrixTranslation.setToZero();
|
||||
mSliderJointComponents.mInverseMassMatrixTranslation[i].setToZero();
|
||||
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
|
||||
mSliderJointComponents.setInverseMassMatrixTranslation(jointEntity, matrixKTranslation.getInverse());
|
||||
mSliderJointComponents.mInverseMassMatrixTranslation[i] = matrixKTranslation.getInverse();
|
||||
}
|
||||
|
||||
// Compute the position error for the 2 translation constraints
|
||||
const Vector2 translationError(u.dot(n1), u.dot(n2));
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 2 translation constraints
|
||||
Vector2 lambdaTranslation = inverseMassMatrixTranslation * (-translationError);
|
||||
Vector2 lambdaTranslation = mSliderJointComponents.mInverseMassMatrixTranslation[i] * (-translationError);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
|
||||
const Vector3 linearImpulseBody1 = -n1 * lambdaTranslation.x - n2 * lambdaTranslation.y;
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user