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

Working on cone limit

Browse Source
This commit is contained in:
Daniel Chappuis 2021-05-04 15:50:16 +02:00
parent 716aa1940d
commit 4a802a759c
6 changed files with 76 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/reactphysics3d/components/BallAndSocketJointComponents.h
View File

@ -97,6 +97,9 @@ class BallAndSocketJointComponents : public Components {
/// Inverse of mass matrix K=JM^-1J^t for the cone limit
decimal* mInverseMassMatrixConeLimit;
/// True if the cone limit is violated
bool* mIsConeLimitViolated;
// -------------------- Methods -------------------- //
/// Allocate memory for a given number of components

3
include/reactphysics3d/constraint/BallAndSocketJoint.h
View File

@ -132,6 +132,9 @@ class BallAndSocketJoint : public Joint {
/// Return the cone limit half angle
decimal getConeLimitHalfAngle() const;
/// Return the current cone angle in radians (in [0,
decimal getConeAngle() const;
/// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space
virtual Vector3 getReactionForce(decimal timeStep) const override;

3
include/reactphysics3d/systems/SolveBallAndSocketJointSystem.h
View File

@ -111,6 +111,9 @@ class SolveBallAndSocketJointSystem {
/// Set to true to enable warm starting
void setIsWarmStartingActive(bool isWarmStartingActive);
/// Return the current cone angle (for the cone limit)
decimal computeCurrentConeAngle(uint32 jointIndex) const;
#ifdef IS_RP3D_PROFILING_ENABLED
/// Set the profiler

10
src/components/BallAndSocketJointComponents.cpp
View File

@ -37,7 +37,8 @@ BallAndSocketJointComponents::BallAndSocketJointComponents(MemoryAllocator& allo
:Components(allocator, sizeof(Entity) + sizeof(BallAndSocketJoint*) + sizeof(Vector3) +
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector3) +
sizeof(Matrix3x3) + sizeof(Vector3)) {
sizeof(Matrix3x3) + sizeof(Vector3) + sizeof(bool) + sizeof(decimal) +
sizeof(decimal) + sizeof(decimal) + sizeof(bool)) {
// Allocate memory for the components data
allocate(INIT_NB_ALLOCATED_COMPONENTS);
@ -71,6 +72,7 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) {
decimal* newConeLimitImpulse = reinterpret_cast<decimal*>(newIsConeLimitEnabled + nbComponentsToAllocate);
decimal* newConeLimitHalfAngle = reinterpret_cast<decimal*>(newConeLimitImpulse + nbComponentsToAllocate);
decimal* newInverseMassMatrixConeLimit = reinterpret_cast<decimal*>(newConeLimitHalfAngle + nbComponentsToAllocate);
bool* newIsConeLimitViolated = reinterpret_cast<bool*>(newInverseMassMatrixConeLimit + nbComponentsToAllocate);
// If there was already components before
if (mNbComponents > 0) {
@ -91,6 +93,7 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) {
memcpy(newConeLimitImpulse, mConeLimitImpulse, mNbComponents * sizeof(decimal));
memcpy(newConeLimitHalfAngle, mConeLimitHalfAngle, mNbComponents * sizeof(decimal));
memcpy(newInverseMassMatrixConeLimit, mInverseMassMatrixConeLimit, mNbComponents * sizeof(decimal));
memcpy(newIsConeLimitViolated, mIsConeLimitViolated, mNbComponents * sizeof(bool));
// Deallocate previous memory
mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize);
@ -113,6 +116,7 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) {
mConeLimitImpulse = newConeLimitImpulse;
mConeLimitHalfAngle = newConeLimitHalfAngle;
mInverseMassMatrixConeLimit = newInverseMassMatrixConeLimit;
mIsConeLimitViolated = newIsConeLimitViolated;
}
// Add a component
@ -137,6 +141,7 @@ void BallAndSocketJointComponents::addComponent(Entity jointEntity, bool isSleep
mConeLimitImpulse[index] = decimal(0.0);
mConeLimitHalfAngle[index] = PI_RP3D;
mInverseMassMatrixConeLimit[index] = decimal(0.0);
mIsConeLimitViolated[index] = false;
// Map the entity with the new component lookup index
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(jointEntity, index));
@ -169,6 +174,7 @@ void BallAndSocketJointComponents::moveComponentToIndex(uint32 srcIndex, uint32
mConeLimitImpulse[destIndex] = mConeLimitImpulse[srcIndex];
mConeLimitHalfAngle[destIndex] = mConeLimitHalfAngle[srcIndex];
mInverseMassMatrixConeLimit[destIndex] = mInverseMassMatrixConeLimit[srcIndex];
mIsConeLimitViolated[destIndex] = mIsConeLimitViolated[srcIndex];
// Destroy the source component
destroyComponent(srcIndex);
@ -200,6 +206,7 @@ void BallAndSocketJointComponents::swapComponents(uint32 index1, uint32 index2)
decimal coneLimitImpulse1 = mConeLimitImpulse[index1];
decimal coneLimitHalfAngle1 = mConeLimitHalfAngle[index1];
decimal inverseMassMatrixConeLimit1 = mInverseMassMatrixConeLimit[index1];
bool isConeLimitViolated = mIsConeLimitViolated[index1];
// Destroy component 1
destroyComponent(index1);
@ -222,6 +229,7 @@ void BallAndSocketJointComponents::swapComponents(uint32 index1, uint32 index2)
mConeLimitImpulse[index2] = coneLimitImpulse1;
mConeLimitHalfAngle[index2] = coneLimitHalfAngle1;
mInverseMassMatrixConeLimit[index2] = inverseMassMatrixConeLimit1;
mIsConeLimitViolated[index2] = isConeLimitViolated;
// Update the entity to component index mapping
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(jointEntity1, index2));

5
src/constraint/BallAndSocketJoint.cpp
View File

@ -84,6 +84,11 @@ decimal BallAndSocketJoint::getConeLimitHalfAngle() const {
return mWorld.mBallAndSocketJointsComponents.getConeLimitHalfAngle(mEntity);
}
// Return the current cone angle in radians (in [0, pi])
decimal BallAndSocketJoint::getConeAngle() const {
...
}
// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space
Vector3 BallAndSocketJoint::getReactionForce(decimal timeStep) const {
assert(timeStep > MACHINE_EPSILON);

55
src/systems/SolveBallAndSocketJointSystem.cpp
View File

@ -27,6 +27,7 @@
#include <reactphysics3d/systems/SolveBallAndSocketJointSystem.h>
#include <reactphysics3d/engine/PhysicsWorld.h>
#include <reactphysics3d/body/RigidBody.h>
#include <cmath>
using namespace reactphysics3d;
@ -70,8 +71,10 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
mBallAndSocketJointComponents.mI1[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody1];
mBallAndSocketJointComponents.mI2[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody2];
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
const Transform& transformBody1 = mTransformComponents.getTransform(body1Entity);
const Transform& transformBody2 = mTransformComponents.getTransform(body2Entity);
const Quaternion& orientationBody1 = transformBody1.getOrientation();
const Quaternion& orientationBody2 = transformBody2.getOrientation();
// Compute the vector from body center to the anchor point in world-space
mBallAndSocketJointComponents.mR1World[i] = orientationBody1 * mBallAndSocketJointComponents.mLocalAnchorPointBody1[i];
@ -114,6 +117,45 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
mBallAndSocketJointComponents.mBiasVector[i] = biasFactor * (x2 + r2World - x1 - r1World);
}
// Compute the current angle around the hinge axis
decimal coneAngle = computeCurrentConeAngle(jointIndex);
// Check if the cone limit constraints is violated or not
decimal coneLimitError = coneAngle - mBallAndSocketJointComponents.mConeLimitHalfAngle[i];
bool oldIsConeLimitViolated = mBallAndSocketJointComponents.mIsConeLimitViolated[i];
bool isConeLimitViolated = coneAngle > mBallAndSocketJointComponents.mConeLimitHalfAngle[i];
mBallAndSocketJointComponents.mIsConeLimitViolated[i] = isConeLimitViolated;
if (!isConeLimitViolated) {
mBallAndSocketJointComponents.mConeLimitImpulse[i] = decimal(0.0);
}
// If the cone limit is enabled
if (mBallAndSocketJointComponents.mIsConeLimitEnabled[i]) {
Vector3& a1 = mHingeJointComponents.mA1[i];
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix)
decimal inverseMassMatrixLimitMotor = a1.dot(mHingeJointComponents.mI1[i] * a1) + a1.dot(mHingeJointComponents.mI2[i] * a1);
inverseMassMatrixLimitMotor = (inverseMassMatrixLimitMotor > decimal(0.0)) ?
decimal(1.0) / inverseMassMatrixLimitMotor : decimal(0.0);
mHingeJointComponents.mInverseMassMatrixLimitMotor[i] = inverseMassMatrixLimitMotor;
if (mHingeJointComponents.mIsLimitEnabled[i]) {
// Compute the bias "b" of the lower limit constraint
mHingeJointComponents.mBLowerLimit[i] = decimal(0.0);
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mHingeJointComponents.mBLowerLimit[i] = biasFactor * lowerLimitError;
}
// Compute the bias "b" of the upper limit constraint
mHingeJointComponents.mBUpperLimit[i] = decimal(0.0);
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mHingeJointComponents.mBUpperLimit[i] = biasFactor * upperLimitError;
}
}
}
// If warm-starting is not enabled
if (!mIsWarmStartingActive) {
@ -318,3 +360,12 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
}
}
}
// Return the current cone angle (for the cone limit)
/**
* @return The positive cone angle in radian in range [0, PI]
*/
decimal SolveBallAndSocketJointSystem::computeCurrentConeAngle(uint32 jointIndex) const {
return std::acos(-mBallAndSocketJointComponents.mR1World[jointIndex], mBallAndSocketJointComponents.mR2World[jointIndex]);
}