diff --git a/examples/joints/Scene.cpp b/examples/joints/Scene.cpp index a651334d..c5c9312e 100644 --- a/examples/joints/Scene.cpp +++ b/examples/joints/Scene.cpp @@ -127,7 +127,7 @@ void Scene::simulate() { if (mIsRunning) { // Update the motor speed of the Slider Joint (to move up and down) - long double motorSpeed = 3 * cos(mDynamicsWorld->getPhysicsTime() * 1.5); + long double motorSpeed = 2 * cos(mDynamicsWorld->getPhysicsTime() * 1.5); mSliderJoint->setMotorSpeed(rp3d::decimal(motorSpeed)); // Take a simulation step diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 9e6a132c..9e84b2bc 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -194,9 +194,10 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat mImpulseMotor = 0.0; } - if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) { + // If the motor or limits are enabled + if (mIsMotorEnabled || (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated))) { - // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) + // Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix) mInverseMassMatrixLimitMotor = 0.0; if (mBody1->isMotionEnabled()) { mInverseMassMatrixLimitMotor += mA1.dot(mI1 * mA1); @@ -207,16 +208,19 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0); - // Compute the bias "b" of the lower limit constraint - mBLowerLimit = 0.0; - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBLowerLimit = biasFactor * lowerLimitError; - } + if (mIsLimitEnabled) { - // Compute the bias "b" of the upper limit constraint - mBUpperLimit = 0.0; - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBUpperLimit = biasFactor * upperLimitError; + // Compute the bias "b" of the lower limit constraint + mBLowerLimit = 0.0; + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBLowerLimit = biasFactor * lowerLimitError; + } + + // Compute the bias "b" of the upper limit constraint + mBUpperLimit = 0.0; + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBUpperLimit = biasFactor * upperLimitError; + } } } } @@ -423,6 +427,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS // --------------- Motor --------------- // + // If the motor is enabled if (mIsMotorEnabled) { // Compute J*v for the motor diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 9a3cdf97..4a9764fc 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -184,6 +184,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mBRotation = biasFactor * decimal(2.0) * qError.getVectorV(); } + // If the limits are enabled if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) { // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) @@ -212,16 +213,20 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa } } - // Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix) - mInverseMassMatrixMotor = 0.0; - if (mBody1->isMotionEnabled()) { - mInverseMassMatrixMotor += mBody1->getMassInverse(); + // If the motor is enabled + if (mIsMotorEnabled) { + + // Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix) + mInverseMassMatrixMotor = 0.0; + if (mBody1->isMotionEnabled()) { + mInverseMassMatrixMotor += mBody1->getMassInverse(); + } + if (mBody2->isMotionEnabled()) { + mInverseMassMatrixMotor += mBody2->getMassInverse(); + } + mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ? + decimal(1.0) / mInverseMassMatrixMotor : decimal(0.0); } - if (mBody2->isMotionEnabled()) { - mInverseMassMatrixMotor += mBody2->getMassInverse(); - } - mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ? - decimal(1.0) / mInverseMassMatrixMotor : decimal(0.0); // If warm-starting is not enabled if (!constraintSolverData.isWarmStartingActive) { diff --git a/src/engine/Profiler.cpp b/src/engine/Profiler.cpp index 2759976a..ad86b557 100644 --- a/src/engine/Profiler.cpp +++ b/src/engine/Profiler.cpp @@ -232,7 +232,7 @@ void Profiler::printRecursiveNodeReport(ProfileNodeIterator* iterator, (currentTotalTime / parentTime) * 100.0 : 0.0; for (int j=0; jgetCurrentName() << " : " << - fraction << " % | " << (currentTotalTime / long double(nbFrames)) << + fraction << " % | " << (currentTotalTime / (long double) (nbFrames)) << " ms/frame (" << iterator->getCurrentNbTotalCalls() << " calls)" << std::endl; totalTime += currentTotalTime;