| /* |
| Bullet Continuous Collision Detection and Physics Library |
| Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ |
| |
| This software is provided 'as-is', without any express or implied warranty. |
| In no event will the authors be held liable for any damages arising from the use of this software. |
| Permission is granted to anyone to use this software for any purpose, |
| including commercial applications, and to alter it and redistribute it freely, |
| subject to the following restrictions: |
| |
| 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. |
| 3. This notice may not be removed or altered from any source distribution. |
| */ |
| /* |
| 2007-09-09 |
| Refactored by Francisco Le?n |
| email: projectileman@yahoo.com |
| http://gimpact.sf.net |
| */ |
| |
| #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" |
| #include "BulletDynamics/Dynamics/btRigidBody.h" |
| #include "LinearMath/btTransformUtil.h" |
| #include "LinearMath/btTransformUtil.h" |
| #include <new> |
| |
| |
| |
| #define D6_USE_OBSOLETE_METHOD false |
| |
| |
| btGeneric6DofConstraint::btGeneric6DofConstraint() |
| :btTypedConstraint(D6_CONSTRAINT_TYPE), |
| m_useLinearReferenceFrameA(true), |
| m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) |
| { |
| } |
| |
| |
| |
| btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) |
| : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB) |
| , m_frameInA(frameInA) |
| , m_frameInB(frameInB), |
| m_useLinearReferenceFrameA(useLinearReferenceFrameA), |
| m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) |
| { |
| |
| } |
| |
| |
| |
| #define GENERIC_D6_DISABLE_WARMSTARTING 1 |
| |
| |
| |
| btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); |
| btScalar btGetMatrixElem(const btMatrix3x3& mat, int index) |
| { |
| int i = index%3; |
| int j = index/3; |
| return mat[i][j]; |
| } |
| |
| |
| |
| ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html |
| bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz); |
| bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) |
| { |
| // // rot = cy*cz -cy*sz sy |
| // // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx |
| // // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy |
| // |
| |
| btScalar fi = btGetMatrixElem(mat,2); |
| if (fi < btScalar(1.0f)) |
| { |
| if (fi > btScalar(-1.0f)) |
| { |
| xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8)); |
| xyz[1] = btAsin(btGetMatrixElem(mat,2)); |
| xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); |
| return true; |
| } |
| else |
| { |
| // WARNING. Not unique. XA - ZA = -atan2(r10,r11) |
| xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); |
| xyz[1] = -SIMD_HALF_PI; |
| xyz[2] = btScalar(0.0); |
| return false; |
| } |
| } |
| else |
| { |
| // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) |
| xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); |
| xyz[1] = SIMD_HALF_PI; |
| xyz[2] = 0.0; |
| } |
| return false; |
| } |
| |
| //////////////////////////// btRotationalLimitMotor //////////////////////////////////// |
| |
| int btRotationalLimitMotor::testLimitValue(btScalar test_value) |
| { |
| if(m_loLimit>m_hiLimit) |
| { |
| m_currentLimit = 0;//Free from violation |
| return 0; |
| } |
| if (test_value < m_loLimit) |
| { |
| m_currentLimit = 1;//low limit violation |
| m_currentLimitError = test_value - m_loLimit; |
| return 1; |
| } |
| else if (test_value> m_hiLimit) |
| { |
| m_currentLimit = 2;//High limit violation |
| m_currentLimitError = test_value - m_hiLimit; |
| return 2; |
| }; |
| |
| m_currentLimit = 0;//Free from violation |
| return 0; |
| |
| } |
| |
| |
| |
| btScalar btRotationalLimitMotor::solveAngularLimits( |
| btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, |
| btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB) |
| { |
| if (needApplyTorques()==false) return 0.0f; |
| |
| btScalar target_velocity = m_targetVelocity; |
| btScalar maxMotorForce = m_maxMotorForce; |
| |
| //current error correction |
| if (m_currentLimit!=0) |
| { |
| target_velocity = -m_ERP*m_currentLimitError/(timeStep); |
| maxMotorForce = m_maxLimitForce; |
| } |
| |
| maxMotorForce *= timeStep; |
| |
| // current velocity difference |
| |
| btVector3 angVelA; |
| bodyA.getAngularVelocity(angVelA); |
| btVector3 angVelB; |
| bodyB.getAngularVelocity(angVelB); |
| |
| btVector3 vel_diff; |
| vel_diff = angVelA-angVelB; |
| |
| |
| |
| btScalar rel_vel = axis.dot(vel_diff); |
| |
| // correction velocity |
| btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); |
| |
| |
| if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON ) |
| { |
| return 0.0f;//no need for applying force |
| } |
| |
| |
| // correction impulse |
| btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; |
| |
| // clip correction impulse |
| btScalar clippedMotorImpulse; |
| |
| ///@todo: should clip against accumulated impulse |
| if (unclippedMotorImpulse>0.0f) |
| { |
| clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; |
| } |
| else |
| { |
| clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; |
| } |
| |
| |
| // sort with accumulated impulses |
| btScalar lo = btScalar(-BT_LARGE_FLOAT); |
| btScalar hi = btScalar(BT_LARGE_FLOAT); |
| |
| btScalar oldaccumImpulse = m_accumulatedImpulse; |
| btScalar sum = oldaccumImpulse + clippedMotorImpulse; |
| m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; |
| |
| clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; |
| |
| btVector3 motorImp = clippedMotorImpulse * axis; |
| |
| //body0->applyTorqueImpulse(motorImp); |
| //body1->applyTorqueImpulse(-motorImp); |
| |
| bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse); |
| bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse); |
| |
| |
| return clippedMotorImpulse; |
| |
| |
| } |
| |
| //////////////////////////// End btRotationalLimitMotor //////////////////////////////////// |
| |
| |
| |
| |
| //////////////////////////// btTranslationalLimitMotor //////////////////////////////////// |
| |
| |
| int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value) |
| { |
| btScalar loLimit = m_lowerLimit[limitIndex]; |
| btScalar hiLimit = m_upperLimit[limitIndex]; |
| if(loLimit > hiLimit) |
| { |
| m_currentLimit[limitIndex] = 0;//Free from violation |
| m_currentLimitError[limitIndex] = btScalar(0.f); |
| return 0; |
| } |
| |
| if (test_value < loLimit) |
| { |
| m_currentLimit[limitIndex] = 2;//low limit violation |
| m_currentLimitError[limitIndex] = test_value - loLimit; |
| return 2; |
| } |
| else if (test_value> hiLimit) |
| { |
| m_currentLimit[limitIndex] = 1;//High limit violation |
| m_currentLimitError[limitIndex] = test_value - hiLimit; |
| return 1; |
| }; |
| |
| m_currentLimit[limitIndex] = 0;//Free from violation |
| m_currentLimitError[limitIndex] = btScalar(0.f); |
| return 0; |
| } |
| |
| |
| |
| btScalar btTranslationalLimitMotor::solveLinearAxis( |
| btScalar timeStep, |
| btScalar jacDiagABInv, |
| btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA, |
| btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB, |
| int limit_index, |
| const btVector3 & axis_normal_on_a, |
| const btVector3 & anchorPos) |
| { |
| |
| ///find relative velocity |
| // btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); |
| // btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); |
| btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition(); |
| btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); |
| |
| btVector3 vel1; |
| bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); |
| btVector3 vel2; |
| bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); |
| btVector3 vel = vel1 - vel2; |
| |
| btScalar rel_vel = axis_normal_on_a.dot(vel); |
| |
| |
| |
| /// apply displacement correction |
| |
| //positional error (zeroth order error) |
| btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); |
| btScalar lo = btScalar(-BT_LARGE_FLOAT); |
| btScalar hi = btScalar(BT_LARGE_FLOAT); |
| |
| btScalar minLimit = m_lowerLimit[limit_index]; |
| btScalar maxLimit = m_upperLimit[limit_index]; |
| |
| //handle the limits |
| if (minLimit < maxLimit) |
| { |
| { |
| if (depth > maxLimit) |
| { |
| depth -= maxLimit; |
| lo = btScalar(0.); |
| |
| } |
| else |
| { |
| if (depth < minLimit) |
| { |
| depth -= minLimit; |
| hi = btScalar(0.); |
| } |
| else |
| { |
| return 0.0f; |
| } |
| } |
| } |
| } |
| |
| btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; |
| |
| |
| |
| |
| btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index]; |
| btScalar sum = oldNormalImpulse + normalImpulse; |
| m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; |
| normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; |
| |
| btVector3 impulse_vector = axis_normal_on_a * normalImpulse; |
| //body1.applyImpulse( impulse_vector, rel_pos1); |
| //body2.applyImpulse(-impulse_vector, rel_pos2); |
| |
| btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a); |
| btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a); |
| bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); |
| bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); |
| |
| |
| |
| |
| return normalImpulse; |
| } |
| |
| //////////////////////////// btTranslationalLimitMotor //////////////////////////////////// |
| |
| void btGeneric6DofConstraint::calculateAngleInfo() |
| { |
| btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis(); |
| matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); |
| // in euler angle mode we do not actually constrain the angular velocity |
| // along the axes axis[0] and axis[2] (although we do use axis[1]) : |
| // |
| // to get constrain w2-w1 along ...not |
| // ------ --------------------- ------ |
| // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] |
| // d(angle[1])/dt = 0 ax[1] |
| // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] |
| // |
| // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. |
| // to prove the result for angle[0], write the expression for angle[0] from |
| // GetInfo1 then take the derivative. to prove this for angle[2] it is |
| // easier to take the euler rate expression for d(angle[2])/dt with respect |
| // to the components of w and set that to 0. |
| btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0); |
| btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2); |
| |
| m_calculatedAxis[1] = axis2.cross(axis0); |
| m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); |
| m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); |
| |
| m_calculatedAxis[0].normalize(); |
| m_calculatedAxis[1].normalize(); |
| m_calculatedAxis[2].normalize(); |
| |
| } |
| |
| void btGeneric6DofConstraint::calculateTransforms() |
| { |
| calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); |
| } |
| |
| void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB) |
| { |
| m_calculatedTransformA = transA * m_frameInA; |
| m_calculatedTransformB = transB * m_frameInB; |
| calculateLinearInfo(); |
| calculateAngleInfo(); |
| } |
| |
| |
| |
| void btGeneric6DofConstraint::buildLinearJacobian( |
| btJacobianEntry & jacLinear,const btVector3 & normalWorld, |
| const btVector3 & pivotAInW,const btVector3 & pivotBInW) |
| { |
| new (&jacLinear) btJacobianEntry( |
| m_rbA.getCenterOfMassTransform().getBasis().transpose(), |
| m_rbB.getCenterOfMassTransform().getBasis().transpose(), |
| pivotAInW - m_rbA.getCenterOfMassPosition(), |
| pivotBInW - m_rbB.getCenterOfMassPosition(), |
| normalWorld, |
| m_rbA.getInvInertiaDiagLocal(), |
| m_rbA.getInvMass(), |
| m_rbB.getInvInertiaDiagLocal(), |
| m_rbB.getInvMass()); |
| } |
| |
| |
| |
| void btGeneric6DofConstraint::buildAngularJacobian( |
| btJacobianEntry & jacAngular,const btVector3 & jointAxisW) |
| { |
| new (&jacAngular) btJacobianEntry(jointAxisW, |
| m_rbA.getCenterOfMassTransform().getBasis().transpose(), |
| m_rbB.getCenterOfMassTransform().getBasis().transpose(), |
| m_rbA.getInvInertiaDiagLocal(), |
| m_rbB.getInvInertiaDiagLocal()); |
| |
| } |
| |
| |
| |
| bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) |
| { |
| btScalar angle = m_calculatedAxisAngleDiff[axis_index]; |
| angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit); |
| m_angularLimits[axis_index].m_currentPosition = angle; |
| //test limits |
| m_angularLimits[axis_index].testLimitValue(angle); |
| return m_angularLimits[axis_index].needApplyTorques(); |
| } |
| |
| |
| |
| void btGeneric6DofConstraint::buildJacobian() |
| { |
| #ifndef __SPU__ |
| if (m_useSolveConstraintObsolete) |
| { |
| |
| // Clear accumulated impulses for the next simulation step |
| m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); |
| int i; |
| for(i = 0; i < 3; i++) |
| { |
| m_angularLimits[i].m_accumulatedImpulse = btScalar(0.); |
| } |
| //calculates transform |
| calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); |
| |
| // const btVector3& pivotAInW = m_calculatedTransformA.getOrigin(); |
| // const btVector3& pivotBInW = m_calculatedTransformB.getOrigin(); |
| calcAnchorPos(); |
| btVector3 pivotAInW = m_AnchorPos; |
| btVector3 pivotBInW = m_AnchorPos; |
| |
| // not used here |
| // btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); |
| // btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); |
| |
| btVector3 normalWorld; |
| //linear part |
| for (i=0;i<3;i++) |
| { |
| if (m_linearLimits.isLimited(i)) |
| { |
| if (m_useLinearReferenceFrameA) |
| normalWorld = m_calculatedTransformA.getBasis().getColumn(i); |
| else |
| normalWorld = m_calculatedTransformB.getBasis().getColumn(i); |
| |
| buildLinearJacobian( |
| m_jacLinear[i],normalWorld , |
| pivotAInW,pivotBInW); |
| |
| } |
| } |
| |
| // angular part |
| for (i=0;i<3;i++) |
| { |
| //calculates error angle |
| if (testAngularLimitMotor(i)) |
| { |
| normalWorld = this->getAxis(i); |
| // Create angular atom |
| buildAngularJacobian(m_jacAng[i],normalWorld); |
| } |
| } |
| |
| } |
| #endif //__SPU__ |
| |
| } |
| |
| |
| void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) |
| { |
| if (m_useSolveConstraintObsolete) |
| { |
| info->m_numConstraintRows = 0; |
| info->nub = 0; |
| } else |
| { |
| //prepare constraint |
| calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); |
| info->m_numConstraintRows = 0; |
| info->nub = 6; |
| int i; |
| //test linear limits |
| for(i = 0; i < 3; i++) |
| { |
| if(m_linearLimits.needApplyForce(i)) |
| { |
| info->m_numConstraintRows++; |
| info->nub--; |
| } |
| } |
| //test angular limits |
| for (i=0;i<3 ;i++ ) |
| { |
| if(testAngularLimitMotor(i)) |
| { |
| info->m_numConstraintRows++; |
| info->nub--; |
| } |
| } |
| } |
| } |
| |
| void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info) |
| { |
| if (m_useSolveConstraintObsolete) |
| { |
| info->m_numConstraintRows = 0; |
| info->nub = 0; |
| } else |
| { |
| //pre-allocate all 6 |
| info->m_numConstraintRows = 6; |
| info->nub = 0; |
| } |
| } |
| |
| |
| void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info) |
| { |
| getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(),m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity()); |
| } |
| |
| void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) |
| { |
| btAssert(!m_useSolveConstraintObsolete); |
| |
| //prepare constraint |
| calculateTransforms(transA,transB); |
| |
| int i; |
| //test linear limits |
| for(i = 0; i < 3; i++) |
| { |
| if(m_linearLimits.needApplyForce(i)) |
| { |
| |
| } |
| } |
| //test angular limits |
| for (i=0;i<3 ;i++ ) |
| { |
| if(testAngularLimitMotor(i)) |
| { |
| |
| } |
| } |
| |
| int row = setLinearLimits(info,transA,transB,linVelA,linVelB,angVelA,angVelB); |
| setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB); |
| } |
| |
| |
| |
| int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) |
| { |
| int row = 0; |
| //solve linear limits |
| btRotationalLimitMotor limot; |
| for (int i=0;i<3 ;i++ ) |
| { |
| if(m_linearLimits.needApplyForce(i)) |
| { // re-use rotational motor code |
| limot.m_bounce = btScalar(0.f); |
| limot.m_currentLimit = m_linearLimits.m_currentLimit[i]; |
| limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i]; |
| limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i]; |
| limot.m_damping = m_linearLimits.m_damping; |
| limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; |
| limot.m_ERP = m_linearLimits.m_restitution; |
| limot.m_hiLimit = m_linearLimits.m_upperLimit[i]; |
| limot.m_limitSoftness = m_linearLimits.m_limitSoftness; |
| limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; |
| limot.m_maxLimitForce = btScalar(0.f); |
| limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; |
| limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; |
| btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i); |
| row += get_limit_motor_info2(&limot, |
| transA,transB,linVelA,linVelB,angVelA,angVelB |
| , info, row, axis, 0); |
| } |
| } |
| return row; |
| } |
| |
| |
| |
| int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) |
| { |
| btGeneric6DofConstraint * d6constraint = this; |
| int row = row_offset; |
| //solve angular limits |
| for (int i=0;i<3 ;i++ ) |
| { |
| if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) |
| { |
| btVector3 axis = d6constraint->getAxis(i); |
| row += get_limit_motor_info2( |
| d6constraint->getRotationalLimitMotor(i), |
| transA,transB,linVelA,linVelB,angVelA,angVelB, |
| info,row,axis,1); |
| } |
| } |
| |
| return row; |
| } |
| |
| |
| |
| void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) |
| { |
| if (m_useSolveConstraintObsolete) |
| { |
| |
| |
| m_timeStep = timeStep; |
| |
| //calculateTransforms(); |
| |
| int i; |
| |
| // linear |
| |
| btVector3 pointInA = m_calculatedTransformA.getOrigin(); |
| btVector3 pointInB = m_calculatedTransformB.getOrigin(); |
| |
| btScalar jacDiagABInv; |
| btVector3 linear_axis; |
| for (i=0;i<3;i++) |
| { |
| if (m_linearLimits.isLimited(i)) |
| { |
| jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal(); |
| |
| if (m_useLinearReferenceFrameA) |
| linear_axis = m_calculatedTransformA.getBasis().getColumn(i); |
| else |
| linear_axis = m_calculatedTransformB.getBasis().getColumn(i); |
| |
| m_linearLimits.solveLinearAxis( |
| m_timeStep, |
| jacDiagABInv, |
| m_rbA,bodyA,pointInA, |
| m_rbB,bodyB,pointInB, |
| i,linear_axis, m_AnchorPos); |
| |
| } |
| } |
| |
| // angular |
| btVector3 angular_axis; |
| btScalar angularJacDiagABInv; |
| for (i=0;i<3;i++) |
| { |
| if (m_angularLimits[i].needApplyTorques()) |
| { |
| |
| // get axis |
| angular_axis = getAxis(i); |
| |
| angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal(); |
| |
| m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB); |
| } |
| } |
| } |
| } |
| |
| |
| |
| void btGeneric6DofConstraint::updateRHS(btScalar timeStep) |
| { |
| (void)timeStep; |
| |
| } |
| |
| |
| |
| btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const |
| { |
| return m_calculatedAxis[axis_index]; |
| } |
| |
| |
| btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const |
| { |
| return m_calculatedLinearDiff[axisIndex]; |
| } |
| |
| |
| btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const |
| { |
| return m_calculatedAxisAngleDiff[axisIndex]; |
| } |
| |
| |
| |
| void btGeneric6DofConstraint::calcAnchorPos(void) |
| { |
| btScalar imA = m_rbA.getInvMass(); |
| btScalar imB = m_rbB.getInvMass(); |
| btScalar weight; |
| if(imB == btScalar(0.0)) |
| { |
| weight = btScalar(1.0); |
| } |
| else |
| { |
| weight = imA / (imA + imB); |
| } |
| const btVector3& pA = m_calculatedTransformA.getOrigin(); |
| const btVector3& pB = m_calculatedTransformB.getOrigin(); |
| m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight); |
| return; |
| } |
| |
| |
| |
| void btGeneric6DofConstraint::calculateLinearInfo() |
| { |
| m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin(); |
| m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff; |
| for(int i = 0; i < 3; i++) |
| { |
| m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i]; |
| m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]); |
| } |
| } |
| |
| |
| |
| int btGeneric6DofConstraint::get_limit_motor_info2( |
| btRotationalLimitMotor * limot, |
| const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, |
| btConstraintInfo2 *info, int row, btVector3& ax1, int rotational) |
| { |
| int srow = row * info->rowskip; |
| int powered = limot->m_enableMotor; |
| int limit = limot->m_currentLimit; |
| if (powered || limit) |
| { // if the joint is powered, or has joint limits, add in the extra row |
| btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; |
| btScalar *J2 = rotational ? info->m_J2angularAxis : 0; |
| J1[srow+0] = ax1[0]; |
| J1[srow+1] = ax1[1]; |
| J1[srow+2] = ax1[2]; |
| if(rotational) |
| { |
| J2[srow+0] = -ax1[0]; |
| J2[srow+1] = -ax1[1]; |
| J2[srow+2] = -ax1[2]; |
| } |
| if((!rotational)) |
| { |
| btVector3 ltd; // Linear Torque Decoupling vector |
| btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin(); |
| ltd = c.cross(ax1); |
| info->m_J1angularAxis[srow+0] = ltd[0]; |
| info->m_J1angularAxis[srow+1] = ltd[1]; |
| info->m_J1angularAxis[srow+2] = ltd[2]; |
| |
| c = m_calculatedTransformB.getOrigin() - transB.getOrigin(); |
| ltd = -c.cross(ax1); |
| info->m_J2angularAxis[srow+0] = ltd[0]; |
| info->m_J2angularAxis[srow+1] = ltd[1]; |
| info->m_J2angularAxis[srow+2] = ltd[2]; |
| } |
| // if we're limited low and high simultaneously, the joint motor is |
| // ineffective |
| if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0; |
| info->m_constraintError[srow] = btScalar(0.f); |
| if (powered) |
| { |
| info->cfm[srow] = 0.0f; |
| if(!limit) |
| { |
| btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity; |
| |
| btScalar mot_fact = getMotorFactor( limot->m_currentPosition, |
| limot->m_loLimit, |
| limot->m_hiLimit, |
| tag_vel, |
| info->fps * info->erp); |
| info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity; |
| info->m_lowerLimit[srow] = -limot->m_maxMotorForce; |
| info->m_upperLimit[srow] = limot->m_maxMotorForce; |
| } |
| } |
| if(limit) |
| { |
| btScalar k = info->fps * limot->m_ERP; |
| if(!rotational) |
| { |
| info->m_constraintError[srow] += k * limot->m_currentLimitError; |
| } |
| else |
| { |
| info->m_constraintError[srow] += -k * limot->m_currentLimitError; |
| } |
| info->cfm[srow] = 0.0f; |
| if (limot->m_loLimit == limot->m_hiLimit) |
| { // limited low and high simultaneously |
| info->m_lowerLimit[srow] = -SIMD_INFINITY; |
| info->m_upperLimit[srow] = SIMD_INFINITY; |
| } |
| else |
| { |
| if (limit == 1) |
| { |
| info->m_lowerLimit[srow] = 0; |
| info->m_upperLimit[srow] = SIMD_INFINITY; |
| } |
| else |
| { |
| info->m_lowerLimit[srow] = -SIMD_INFINITY; |
| info->m_upperLimit[srow] = 0; |
| } |
| // deal with bounce |
| if (limot->m_bounce > 0) |
| { |
| // calculate joint velocity |
| btScalar vel; |
| if (rotational) |
| { |
| vel = angVelA.dot(ax1); |
| //make sure that if no body -> angVelB == zero vec |
| // if (body1) |
| vel -= angVelB.dot(ax1); |
| } |
| else |
| { |
| vel = linVelA.dot(ax1); |
| //make sure that if no body -> angVelB == zero vec |
| // if (body1) |
| vel -= linVelB.dot(ax1); |
| } |
| // only apply bounce if the velocity is incoming, and if the |
| // resulting c[] exceeds what we already have. |
| if (limit == 1) |
| { |
| if (vel < 0) |
| { |
| btScalar newc = -limot->m_bounce* vel; |
| if (newc > info->m_constraintError[srow]) |
| info->m_constraintError[srow] = newc; |
| } |
| } |
| else |
| { |
| if (vel > 0) |
| { |
| btScalar newc = -limot->m_bounce * vel; |
| if (newc < info->m_constraintError[srow]) |
| info->m_constraintError[srow] = newc; |
| } |
| } |
| } |
| } |
| } |
| return 1; |
| } |
| else return 0; |
| } |
| |
| |
| |
| |
