diff options
author | Erwin Coumans <blender@erwincoumans.com> | 2009-03-03 06:41:24 +0300 |
---|---|---|
committer | Erwin Coumans <blender@erwincoumans.com> | 2009-03-03 06:41:24 +0300 |
commit | 982a5cc60dd93b21ce110b3fff831d05720047bf (patch) | |
tree | 5d6245b0006f249c48e900b748aedd4484c10da3 /extern/bullet2/src/BulletDynamics | |
parent | f8ef887880ac217cc831b2ab9677e1f9e825537b (diff) |
Upgraded to Bullet 2.74. The upgrade introduced a few bugs, which need to be fixed before Blender 2.49.
In particular, the Bullet vehicle seems broken, and some soft-body demos don't work.
No new features or benefits are added yet, but a few improvements are planned before Blender 2.49 release.
Please update the build systems, and add those 3 files:
extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp
I'll watch the Blender mailing list, in case this commit causes some issues.
Diffstat (limited to 'extern/bullet2/src/BulletDynamics')
30 files changed, 4153 insertions, 2055 deletions
diff --git a/extern/bullet2/src/BulletDynamics/CMakeLists.txt b/extern/bullet2/src/BulletDynamics/CMakeLists.txt index 58b023e9775..ecfcbfef929 100644 --- a/extern/bullet2/src/BulletDynamics/CMakeLists.txt +++ b/extern/bullet2/src/BulletDynamics/CMakeLists.txt @@ -1,36 +1,93 @@ -INCLUDE_DIRECTORIES( -${BULLET_PHYSICS_SOURCE_DIR}/src } -) - -ADD_LIBRARY(LibBulletDynamics +INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src } ) +SET(BulletDynamics_SRCS ConstraintSolver/btContactConstraint.cpp - ConstraintSolver/btContactConstraint.h ConstraintSolver/btConeTwistConstraint.cpp - ConstraintSolver/btConeTwistConstraint.h ConstraintSolver/btGeneric6DofConstraint.cpp - ConstraintSolver/btGeneric6DofConstraint.h ConstraintSolver/btHingeConstraint.cpp - ConstraintSolver/btHingeConstraint.h ConstraintSolver/btPoint2PointConstraint.cpp - ConstraintSolver/btPoint2PointConstraint.h ConstraintSolver/btSequentialImpulseConstraintSolver.cpp - ConstraintSolver/btSequentialImpulseConstraintSolver.h ConstraintSolver/btSliderConstraint.cpp - ConstraintSolver/btSliderConstraint.h ConstraintSolver/btSolve2LinearConstraint.cpp - ConstraintSolver/btSolve2LinearConstraint.h ConstraintSolver/btTypedConstraint.cpp - ConstraintSolver/btTypedConstraint.h Dynamics/Bullet-C-API.cpp Dynamics/btDiscreteDynamicsWorld.cpp - Dynamics/btDiscreteDynamicsWorld.h Dynamics/btSimpleDynamicsWorld.cpp - Dynamics/btSimpleDynamicsWorld.h Dynamics/btRigidBody.cpp - Dynamics/btRigidBody.h Vehicle/btRaycastVehicle.cpp - Vehicle/btRaycastVehicle.h Vehicle/btWheelInfo.cpp + Character/btKinematicCharacterController.cpp +) + +SET(Root_HDRS + ../btBulletDynamicsCommon.h + ../btBulletCollisionCommon.h +) +SET(ConstraintSolver_HDRS + ConstraintSolver/btConstraintSolver.h + ConstraintSolver/btContactConstraint.h + ConstraintSolver/btContactSolverInfo.h + ConstraintSolver/btConeTwistConstraint.h + ConstraintSolver/btGeneric6DofConstraint.h + ConstraintSolver/btHingeConstraint.h + ConstraintSolver/btJacobianEntry.h + ConstraintSolver/btPoint2PointConstraint.h + ConstraintSolver/btSequentialImpulseConstraintSolver.h + ConstraintSolver/btSliderConstraint.h + ConstraintSolver/btSolve2LinearConstraint.h + ConstraintSolver/btSolverBody.h + ConstraintSolver/btSolverConstraint.h + ConstraintSolver/btTypedConstraint.h +) +SET(Dynamics_HDRS + Dynamics/btContinuousDynamicsWorld.h + Dynamics/btDiscreteDynamicsWorld.h + Dynamics/btDynamicsWorld.h + Dynamics/btSimpleDynamicsWorld.h + Dynamics/btRigidBody.h +) +SET(Vehicle_HDRS + Vehicle/btRaycastVehicle.h + Vehicle/btVehicleRaycaster.h Vehicle/btWheelInfo.h ) + +SET(Character_HDRS + Character/btCharacterControllerInterface.h + Character/btKinematicCharacterController.h +) + + + +SET(BulletDynamics_HDRS + ${Root_HDRS} + ${ConstraintSolver_HDRS} + ${Dynamics_HDRS} + ${Vehicle_HDRS} + ${Character_HDRS} +) + + +ADD_LIBRARY(BulletDynamics ${BulletDynamics_SRCS} ${BulletDynamics_HDRS}) +SET_TARGET_PROPERTIES(BulletDynamics PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(BulletDynamics PROPERTIES SOVERSION ${BULLET_VERSION}) +IF (BUILD_SHARED_LIBS) + TARGET_LINK_LIBRARIES(BulletDynamics BulletCollision LinearMath) +ENDIF (BUILD_SHARED_LIBS) + +IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + INSTALL(TARGETS BulletDynamics DESTINATION lib) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} DESTINATION include FILES_MATCHING PATTERN "*.h") +ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + +IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(BulletDynamics PROPERTIES FRAMEWORK true) + + SET_TARGET_PROPERTIES(BulletDynamics PROPERTIES PUBLIC_HEADER "${Root_HDRS}") + # Have to list out sub-directories manually: + SET_PROPERTY(SOURCE ${ConstraintSolver_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/ConstraintSolver) + SET_PROPERTY(SOURCE ${Dynamics_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Dynamics) + SET_PROPERTY(SOURCE ${Vehicle_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Vehicle) + SET_PROPERTY(SOURCE ${Character_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Character) + +ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 61dad522a5b..29c8496c36f 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -22,8 +22,16 @@ Written by: Marcus Hennix #include "LinearMath/btMinMax.h" #include <new> +//----------------------------------------------------------------------------- + +#define CONETWIST_USE_OBSOLETE_SOLVER false +#define CONETWIST_DEF_FIX_THRESH btScalar(.05f) + +//----------------------------------------------------------------------------- + btConeTwistConstraint::btConeTwistConstraint() -:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE) +:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE), +m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) { } @@ -31,69 +39,228 @@ btConeTwistConstraint::btConeTwistConstraint() btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame,const btTransform& rbBFrame) :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), - m_angularOnly(false) + m_angularOnly(false), + m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) { - m_swingSpan1 = btScalar(1e30); - m_swingSpan2 = btScalar(1e30); - m_twistSpan = btScalar(1e30); - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - - m_solveTwistLimit = false; - m_solveSwingLimit = false; - + init(); } btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame) :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame), - m_angularOnly(false) + m_angularOnly(false), + m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) { m_rbBFrame = m_rbAFrame; - - m_swingSpan1 = btScalar(1e30); - m_swingSpan2 = btScalar(1e30); - m_twistSpan = btScalar(1e30); - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; + init(); +} - m_solveTwistLimit = false; - m_solveSwingLimit = false; - -} -void btConeTwistConstraint::buildJacobian() +void btConeTwistConstraint::init() { - m_appliedImpulse = btScalar(0.); - - //set bias, sign, clear accumulator - m_swingCorrection = btScalar(0.); - m_twistLimitSign = btScalar(0.); + m_angularOnly = false; m_solveTwistLimit = false; m_solveSwingLimit = false; - m_accTwistLimitImpulse = btScalar(0.); - m_accSwingLimitImpulse = btScalar(0.); + m_bMotorEnabled = false; + m_maxMotorImpulse = btScalar(-1); + + setLimit(btScalar(1e30), btScalar(1e30), btScalar(1e30)); + m_damping = btScalar(0.01); + m_fixThresh = CONETWIST_DEF_FIX_THRESH; +} - if (!m_angularOnly) + +//----------------------------------------------------------------------------- + +void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) +{ + if (m_useSolveConstraintObsolete) { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - btVector3 relPos = pivotBInW - pivotAInW; + info->m_numConstraintRows = 0; + info->nub = 0; + } + else + { + info->m_numConstraintRows = 3; + info->nub = 3; + calcAngleInfo2(); + if(m_solveSwingLimit) + { + info->m_numConstraintRows++; + info->nub--; + if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) + { + info->m_numConstraintRows++; + info->nub--; + } + } + if(m_solveTwistLimit) + { + info->m_numConstraintRows++; + info->nub--; + } + } +} // btConeTwistConstraint::getInfo1() + +//----------------------------------------------------------------------------- - btVector3 normal[3]; - if (relPos.length2() > SIMD_EPSILON) +void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) +{ + btAssert(!m_useSolveConstraintObsolete); + //retrieve matrices + btTransform body0_trans; + body0_trans = m_rbA.getCenterOfMassTransform(); + btTransform body1_trans; + body1_trans = m_rbB.getCenterOfMassTransform(); + // set jacobian + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[info->rowskip+1] = 1; + info->m_J1linearAxis[2*info->rowskip+2] = 1; + btVector3 a1 = body0_trans.getBasis() * m_rbAFrame.getOrigin(); + { + btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); + btVector3 a1neg = -a1; + a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + btVector3 a2 = body1_trans.getBasis() * m_rbBFrame.getOrigin(); + { + btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); + a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + // set right hand side + btScalar k = info->fps * info->erp; + int j; + for (j=0; j<3; j++) + { + info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); + info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY; + info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY; + } + int row = 3; + int srow = row * info->rowskip; + btVector3 ax1; + // angular limits + if(m_solveSwingLimit) + { + btScalar *J1 = info->m_J1angularAxis; + btScalar *J2 = info->m_J2angularAxis; + if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) + { + btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame; + btVector3 p = trA.getBasis().getColumn(1); + btVector3 q = trA.getBasis().getColumn(2); + int srow1 = srow + info->rowskip; + J1[srow+0] = p[0]; + J1[srow+1] = p[1]; + J1[srow+2] = p[2]; + J1[srow1+0] = q[0]; + J1[srow1+1] = q[1]; + J1[srow1+2] = q[2]; + J2[srow+0] = -p[0]; + J2[srow+1] = -p[1]; + J2[srow+2] = -p[2]; + J2[srow1+0] = -q[0]; + J2[srow1+1] = -q[1]; + J2[srow1+2] = -q[2]; + btScalar fact = info->fps * m_relaxationFactor; + info->m_constraintError[srow] = fact * m_swingAxis.dot(p); + info->m_constraintError[srow1] = fact * m_swingAxis.dot(q); + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + info->m_lowerLimit[srow1] = -SIMD_INFINITY; + info->m_upperLimit[srow1] = SIMD_INFINITY; + srow = srow1 + info->rowskip; + } + else + { + ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor; + J1[srow+0] = ax1[0]; + J1[srow+1] = ax1[1]; + J1[srow+2] = ax1[2]; + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; + btScalar k = info->fps * m_biasFactor; + + info->m_constraintError[srow] = k * m_swingCorrection; + info->cfm[srow] = 0.0f; + // m_swingCorrection is always positive or 0 + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = SIMD_INFINITY; + srow += info->rowskip; + } + } + if(m_solveTwistLimit) + { + ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor; + btScalar *J1 = info->m_J1angularAxis; + btScalar *J2 = info->m_J2angularAxis; + J1[srow+0] = ax1[0]; + J1[srow+1] = ax1[1]; + J1[srow+2] = ax1[2]; + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; + btScalar k = info->fps * m_biasFactor; + info->m_constraintError[srow] = k * m_twistCorrection; + info->cfm[srow] = 0.0f; + if(m_twistSpan > 0.0f) { - normal[0] = relPos.normalized(); + + if(m_twistCorrection > 0.0f) + { + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else + { + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = 0; + } } else { - normal[0].setValue(btScalar(1.0),0,0); + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; } + srow += info->rowskip; + } +} + +//----------------------------------------------------------------------------- - btPlaneSpace1(normal[0], normal[1], normal[2]); +void btConeTwistConstraint::buildJacobian() +{ + if (m_useSolveConstraintObsolete) + { + m_appliedImpulse = btScalar(0.); + m_accTwistLimitImpulse = btScalar(0.); + m_accSwingLimitImpulse = btScalar(0.); - for (int i=0;i<3;i++) + if (!m_angularOnly) { - new (&m_jac[i]) btJacobianEntry( + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + btVector3 relPos = pivotBInW - pivotAInW; + + btVector3 normal[3]; + if (relPos.length2() > SIMD_EPSILON) + { + normal[0] = relPos.normalized(); + } + else + { + normal[0].setValue(btScalar(1.0),0,0); + } + + btPlaneSpace1(normal[0], normal[1], normal[2]); + + for (int i=0;i<3;i++) + { + new (&m_jac[i]) btJacobianEntry( m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), pivotAInW - m_rbA.getCenterOfMassPosition(), @@ -103,9 +270,243 @@ void btConeTwistConstraint::buildJacobian() m_rbA.getInvMass(), m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvMass()); + } + } + + calcAngleInfo2(); + } +} + +//----------------------------------------------------------------------------- + +void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) +{ + if (m_useSolveConstraintObsolete) + { + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + + btScalar tau = btScalar(0.3); + + //linear part + if (!m_angularOnly) + { + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + btVector3 vel1; + bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); + btVector3 vel2; + bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); + btVector3 vel = vel1 - vel2; + + for (int i=0;i<3;i++) + { + const btVector3& normal = m_jac[i].m_linearJointAxis; + btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); + + btScalar rel_vel; + rel_vel = normal.dot(vel); + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; + m_appliedImpulse += impulse; + + btVector3 ftorqueAxis1 = rel_pos1.cross(normal); + btVector3 ftorqueAxis2 = rel_pos2.cross(normal); + bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); + bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); + + } + } + + // apply motor + if (m_bMotorEnabled) + { + // compute current and predicted transforms + btTransform trACur = m_rbA.getCenterOfMassTransform(); + btTransform trBCur = m_rbB.getCenterOfMassTransform(); + btVector3 omegaA; bodyA.getAngularVelocity(omegaA); + btVector3 omegaB; bodyB.getAngularVelocity(omegaB); + btTransform trAPred; trAPred.setIdentity(); + btVector3 zerovec(0,0,0); + btTransformUtil::integrateTransform( + trACur, zerovec, omegaA, timeStep, trAPred); + btTransform trBPred; trBPred.setIdentity(); + btTransformUtil::integrateTransform( + trBCur, zerovec, omegaB, timeStep, trBPred); + + // compute desired transforms in world + btTransform trPose(m_qTarget); + btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse(); + btTransform trADes = trBPred * trABDes; + btTransform trBDes = trAPred * trABDes.inverse(); + + // compute desired omegas in world + btVector3 omegaADes, omegaBDes; + + btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes); + btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes); + + // compute delta omegas + btVector3 dOmegaA = omegaADes - omegaA; + btVector3 dOmegaB = omegaBDes - omegaB; + + // compute weighted avg axis of dOmega (weighting based on inertias) + btVector3 axisA, axisB; + btScalar kAxisAInv = 0, kAxisBInv = 0; + + if (dOmegaA.length2() > SIMD_EPSILON) + { + axisA = dOmegaA.normalized(); + kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA); + } + + if (dOmegaB.length2() > SIMD_EPSILON) + { + axisB = dOmegaB.normalized(); + kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB); + } + + btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB; + + static bool bDoTorque = true; + if (bDoTorque && avgAxis.length2() > SIMD_EPSILON) + { + avgAxis.normalize(); + kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis); + kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis); + btScalar kInvCombined = kAxisAInv + kAxisBInv; + + btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) / + (kInvCombined * kInvCombined); + + if (m_maxMotorImpulse >= 0) + { + btScalar fMaxImpulse = m_maxMotorImpulse; + if (m_bNormalizedMotorStrength) + fMaxImpulse = fMaxImpulse/kAxisAInv; + + btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse; + btScalar newUnclampedMag = newUnclampedAccImpulse.length(); + if (newUnclampedMag > fMaxImpulse) + { + newUnclampedAccImpulse.normalize(); + newUnclampedAccImpulse *= fMaxImpulse; + impulse = newUnclampedAccImpulse - m_accMotorImpulse; + } + m_accMotorImpulse += impulse; + } + + btScalar impulseMag = impulse.length(); + btVector3 impulseAxis = impulse / impulseMag; + + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); + + } + } + else // no motor: do a little damping + { + const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); + const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); + btVector3 relVel = angVelB - angVelA; + if (relVel.length2() > SIMD_EPSILON) + { + btVector3 relVelAxis = relVel.normalized(); + btScalar m_kDamping = btScalar(1.) / + (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) + + getRigidBodyB().computeAngularImpulseDenominator(relVelAxis)); + btVector3 impulse = m_damping * m_kDamping * relVel; + + btScalar impulseMag = impulse.length(); + btVector3 impulseAxis = impulse / impulseMag; + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); + } + } + + // joint limits + { + ///solve angular part + btVector3 angVelA; + bodyA.getAngularVelocity(angVelA); + btVector3 angVelB; + bodyB.getAngularVelocity(angVelB); + + // solve swing limit + if (m_solveSwingLimit) + { + btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep; + btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis); + if (relSwingVel > 0) + amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor; + btScalar impulseMag = amplitude * m_kSwing; + + // Clamp the accumulated impulse + btScalar temp = m_accSwingLimitImpulse; + m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); + impulseMag = m_accSwingLimitImpulse - temp; + + btVector3 impulse = m_swingAxis * impulseMag; + + // don't let cone response affect twist + // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit) + { + btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA; + btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple; + impulse = impulseNoTwistCouple; + } + + impulseMag = impulse.length(); + btVector3 noTwistSwingAxis = impulse / impulseMag; + + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag); + } + + + // solve twist limit + if (m_solveTwistLimit) + { + btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep; + btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis ); + if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important) + amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor; + btScalar impulseMag = amplitude * m_kTwist; + + // Clamp the accumulated impulse + btScalar temp = m_accTwistLimitImpulse; + m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); + impulseMag = m_accTwistLimitImpulse - temp; + + btVector3 impulse = m_twistAxis * impulseMag; + + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag); + } } } +} + +//----------------------------------------------------------------------------- + +void btConeTwistConstraint::updateRHS(btScalar timeStep) +{ + (void)timeStep; + +} + +//----------------------------------------------------------------------------- + +void btConeTwistConstraint::calcAngleInfo() +{ + m_swingCorrection = btScalar(0.); + m_twistLimitSign = btScalar(0.); + m_solveTwistLimit = false; + m_solveSwingLimit = false; + btVector3 b1Axis1,b1Axis2,b1Axis3; btVector3 b2Axis1,b2Axis2; @@ -122,20 +523,17 @@ void btConeTwistConstraint::buildJacobian() if (m_swingSpan1 >= btScalar(0.05f)) { b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1); -// swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis2); swing1 = btAtan2Fast(swy, swx); fact = (swy*swy + swx*swx) * thresh * thresh; fact = fact / (fact + btScalar(1.0)); swing1 *= fact; - } if (m_swingSpan2 >= btScalar(0.05f)) { b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); -// swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis3); swing2 = btAtan2Fast(swy, swx); @@ -152,17 +550,11 @@ void btConeTwistConstraint::buildJacobian() { m_swingCorrection = EllipseAngle-1.0f; m_solveSwingLimit = true; - // Calculate necessary axis & factors m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3)); m_swingAxis.normalize(); - btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; m_swingAxis *= swingAxisSign; - - m_kSwing = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) + - getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis)); - } // Twist limits @@ -172,118 +564,426 @@ void btConeTwistConstraint::buildJacobian() btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1); btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) ); + m_twistAngle = twist; - btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); +// btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); + btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.); if (twist <= -m_twistSpan*lockedFreeFactor) { m_twistCorrection = -(twist + m_twistSpan); m_solveTwistLimit = true; - m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; m_twistAxis.normalize(); m_twistAxis *= -1.0f; + } + else if (twist > m_twistSpan*lockedFreeFactor) + { + m_twistCorrection = (twist - m_twistSpan); + m_solveTwistLimit = true; + m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; + m_twistAxis.normalize(); + } + } +} // btConeTwistConstraint::calcAngleInfo() + + +static btVector3 vTwist(1,0,0); // twist axis in constraint's space + +//----------------------------------------------------------------------------- + +void btConeTwistConstraint::calcAngleInfo2() +{ + m_swingCorrection = btScalar(0.); + m_twistLimitSign = btScalar(0.); + m_solveTwistLimit = false; + m_solveSwingLimit = false; + + { + // compute rotation of A wrt B (in constraint space) + btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation(); + btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation(); + btQuaternion qAB = qB.inverse() * qA; + + // split rotation into cone and twist + // (all this is done from B's perspective. Maybe I should be averaging axes...) + btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); + btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize(); + btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize(); + + if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh) + { + btScalar swingAngle, swingLimit = 0; btVector3 swingAxis; + computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit); + + if (swingAngle > swingLimit * m_limitSoftness) + { + m_solveSwingLimit = true; + + // compute limit ratio: 0->1, where + // 0 == beginning of soft limit + // 1 == hard/real limit + m_swingLimitRatio = 1.f; + if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON) + { + m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/ + (swingLimit - swingLimit * m_limitSoftness); + } + + // swing correction tries to get back to soft limit + m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness); + + // adjustment of swing axis (based on ellipse normal) + adjustSwingAxisToUseEllipseNormal(swingAxis); + + // Calculate necessary axis & factors + m_swingAxis = quatRotate(qB, -swingAxis); + + m_twistAxisA.setValue(0,0,0); + + m_kSwing = btScalar(1.) / + (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) + + getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis)); + } + } + else + { + // you haven't set any limits; + // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?) + // anyway, we have either hinge or fixed joint + btVector3 ivA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0); + btVector3 jvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1); + btVector3 kvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + btVector3 ivB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(0); + btVector3 target; + btScalar x = ivB.dot(ivA); + btScalar y = ivB.dot(jvA); + btScalar z = ivB.dot(kvA); + if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) + { // fixed. We'll need to add one more row to constraint + if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) + { + m_solveSwingLimit = true; + m_swingAxis = -ivB.cross(ivA); + } + } + else + { + if(m_swingSpan1 < m_fixThresh) + { // hinge around Y axis + if(!(btFuzzyZero(y))) + { + m_solveSwingLimit = true; + if(m_swingSpan2 >= m_fixThresh) + { + y = btScalar(0.f); + btScalar span2 = btAtan2(z, x); + if(span2 > m_swingSpan2) + { + x = btCos(m_swingSpan2); + z = btSin(m_swingSpan2); + } + else if(span2 < -m_swingSpan2) + { + x = btCos(m_swingSpan2); + z = -btSin(m_swingSpan2); + } + } + } + } + else + { // hinge around Z axis + if(!btFuzzyZero(z)) + { + m_solveSwingLimit = true; + if(m_swingSpan1 >= m_fixThresh) + { + z = btScalar(0.f); + btScalar span1 = btAtan2(y, x); + if(span1 > m_swingSpan1) + { + x = btCos(m_swingSpan1); + y = btSin(m_swingSpan1); + } + else if(span1 < -m_swingSpan1) + { + x = btCos(m_swingSpan1); + y = -btSin(m_swingSpan1); + } + } + } + } + target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0]; + target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1]; + target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2]; + target.normalize(); + m_swingAxis = -ivB.cross(target); + m_swingCorrection = m_swingAxis.length(); + m_swingAxis.normalize(); + } + } - m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + - getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); + if (m_twistSpan >= btScalar(0.f)) + { + btVector3 twistAxis; + computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis); - } else - if (twist > m_twistSpan*lockedFreeFactor) + if (m_twistAngle > m_twistSpan*m_limitSoftness) { - m_twistCorrection = (twist - m_twistSpan); m_solveTwistLimit = true; - m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; - m_twistAxis.normalize(); + m_twistLimitRatio = 1.f; + if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON) + { + m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/ + (m_twistSpan - m_twistSpan * m_limitSoftness); + } - m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + - getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); + // twist correction tries to get back to soft limit + m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness); + m_twistAxis = quatRotate(qB, -twistAxis); + + m_kTwist = btScalar(1.) / + (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + + getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); } + + if (m_solveSwingLimit) + m_twistAxisA = quatRotate(qA, -twistAxis); + } + else + { + m_twistAngle = btScalar(0.f); + } + } +} // btConeTwistConstraint::calcAngleInfo2() + + + +// given a cone rotation in constraint space, (pre: twist must already be removed) +// this method computes its corresponding swing angle and axis. +// more interestingly, it computes the cone/swing limit (angle) for this cone "pose". +void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, + btScalar& swingAngle, // out + btVector3& vSwingAxis, // out + btScalar& swingLimit) // out +{ + swingAngle = qCone.getAngle(); + if (swingAngle > SIMD_EPSILON) + { + vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z()); + vSwingAxis.normalize(); + if (fabs(vSwingAxis.x()) > SIMD_EPSILON) + { + // non-zero twist?! this should never happen. + int wtf = 0; wtf = wtf; + } + + // Compute limit for given swing. tricky: + // Given a swing axis, we're looking for the intersection with the bounding cone ellipse. + // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.) + + // For starters, compute the direction from center to surface of ellipse. + // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis. + // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.) + btScalar xEllipse = vSwingAxis.y(); + btScalar yEllipse = -vSwingAxis.z(); + + // Now, we use the slope of the vector (using x/yEllipse) and find the length + // of the line that intersects the ellipse: + // x^2 y^2 + // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits) + // a^2 b^2 + // Do the math and it should be clear. + + swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1 + if (fabs(xEllipse) > SIMD_EPSILON) + { + btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); + btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); + norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); + btScalar swingLimit2 = (1 + surfaceSlope2) / norm; + swingLimit = sqrt(swingLimit2); + } + + // test! + /*swingLimit = m_swingSpan2; + if (fabs(vSwingAxis.z()) > SIMD_EPSILON) + { + btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2; + btScalar sinphi = m_swingSpan2 / sqrt(mag_2); + btScalar phi = asin(sinphi); + btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z())); + btScalar alpha = 3.14159f - theta - phi; + btScalar sinalpha = sin(alpha); + swingLimit = m_swingSpan1 * sinphi/sinalpha; + }*/ + } + else if (swingAngle < 0) + { + // this should never happen! + int wtf = 0; wtf = wtf; } } -void btConeTwistConstraint::solveConstraint(btScalar timeStep) +btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const { + // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone) + btScalar xEllipse = btCos(fAngleInRadians); + btScalar yEllipse = btSin(fAngleInRadians); + + // Use the slope of the vector (using x/yEllipse) and find the length + // of the line that intersects the ellipse: + // x^2 y^2 + // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits) + // a^2 b^2 + // Do the math and it should be clear. + + float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1) + if (fabs(xEllipse) > SIMD_EPSILON) + { + btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); + btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); + norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); + btScalar swingLimit2 = (1 + surfaceSlope2) / norm; + swingLimit = sqrt(swingLimit2); + } - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + // convert into point in constraint space: + // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively + btVector3 vSwingAxis(0, xEllipse, -yEllipse); + btQuaternion qSwing(vSwingAxis, swingLimit); + btVector3 vPointInConstraintSpace(fLength,0,0); + return quatRotate(qSwing, vPointInConstraintSpace); +} - btScalar tau = btScalar(0.3); +// given a twist rotation in constraint space, (pre: cone must already be removed) +// this method computes its corresponding angle and axis. +void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist, + btScalar& twistAngle, // out + btVector3& vTwistAxis) // out +{ + btQuaternion qMinTwist = qTwist; + twistAngle = qTwist.getAngle(); - //linear part - if (!m_angularOnly) + if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. { - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - for (int i=0;i<3;i++) - { - const btVector3& normal = m_jac[i].m_linearJointAxis; - btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); - - btScalar rel_vel; - rel_vel = normal.dot(vel); - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; - m_appliedImpulse += impulse; - btVector3 impulse_vector = normal * impulse; - m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); - m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); - } + qMinTwist = operator-(qTwist); + twistAngle = qMinTwist.getAngle(); } - + if (twistAngle < 0) { - ///solve angular part - const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); - const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); + // this should never happen + int wtf = 0; wtf = wtf; + } - // solve swing limit - if (m_solveSwingLimit) - { - btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor); - btScalar impulseMag = amplitude * m_kSwing; + vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z()); + if (twistAngle > SIMD_EPSILON) + vTwistAxis.normalize(); +} - // Clamp the accumulated impulse - btScalar temp = m_accSwingLimitImpulse; - m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); - impulseMag = m_accSwingLimitImpulse - temp; - btVector3 impulse = m_swingAxis * impulseMag; +void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const +{ + // the swing axis is computed as the "twist-free" cone rotation, + // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2). + // so, if we're outside the limits, the closest way back inside the cone isn't + // along the vector back to the center. better (and more stable) to use the ellipse normal. + + // convert swing axis to direction from center to surface of ellipse + // (ie. rotate 2D vector by PI/2) + btScalar y = -vSwingAxis.z(); + btScalar z = vSwingAxis.y(); + + // do the math... + if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0. + { + // compute gradient/normal of ellipse surface at current "point" + btScalar grad = y/z; + grad *= m_swingSpan2 / m_swingSpan1; - m_rbA.applyTorqueImpulse(impulse); - m_rbB.applyTorqueImpulse(-impulse); + // adjust y/z to represent normal at point (instead of vector to point) + if (y > 0) + y = fabs(grad * z); + else + y = -fabs(grad * z); - } + // convert ellipse direction back to swing axis + vSwingAxis.setZ(-y); + vSwingAxis.setY( z); + vSwingAxis.normalize(); + } +} + + + +void btConeTwistConstraint::setMotorTarget(const btQuaternion &q) +{ + btTransform trACur = m_rbA.getCenterOfMassTransform(); + btTransform trBCur = m_rbB.getCenterOfMassTransform(); + btTransform trABCur = trBCur.inverse() * trACur; + btQuaternion qABCur = trABCur.getRotation(); + btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); + btQuaternion qConstraintCur = trConstraintCur.getRotation(); + + btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation(); + setMotorTargetInConstraintSpace(qConstraint); +} - // solve twist limit - if (m_solveTwistLimit) - { - btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor ); - btScalar impulseMag = amplitude * m_kTwist; - // Clamp the accumulated impulse - btScalar temp = m_accTwistLimitImpulse; - m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); - impulseMag = m_accTwistLimitImpulse - temp; +void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q) +{ + m_qTarget = q; + + // clamp motor target to within limits + { + btScalar softness = 1.f;//m_limitSoftness; - btVector3 impulse = m_twistAxis * impulseMag; + // split into twist and cone + btVector3 vTwisted = quatRotate(m_qTarget, vTwist); + btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize(); + btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize(); - m_rbA.applyTorqueImpulse(impulse); - m_rbB.applyTorqueImpulse(-impulse); + // clamp cone + if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f)) + { + btScalar swingAngle, swingLimit; btVector3 swingAxis; + computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit); + if (fabs(swingAngle) > SIMD_EPSILON) + { + if (swingAngle > swingLimit*softness) + swingAngle = swingLimit*softness; + else if (swingAngle < -swingLimit*softness) + swingAngle = -swingLimit*softness; + qTargetCone = btQuaternion(swingAxis, swingAngle); + } } - - } -} + // clamp twist + if (m_twistSpan >= btScalar(0.05f)) + { + btScalar twistAngle; btVector3 twistAxis; + computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis); -void btConeTwistConstraint::updateRHS(btScalar timeStep) -{ - (void)timeStep; + if (fabs(twistAngle) > SIMD_EPSILON) + { + // eddy todo: limitSoftness used here??? + if (twistAngle > m_twistSpan*softness) + twistAngle = m_twistSpan*softness; + else if (twistAngle < -m_twistSpan*softness) + twistAngle = -m_twistSpan*softness; + qTargetTwist = btQuaternion(twistAxis, twistAngle); + } + } + m_qTarget = qTargetCone * qTargetTwist; + } } + + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index f121919c8f9..84ea9e04095 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -20,7 +20,7 @@ Written by: Marcus Hennix #ifndef CONETWISTCONSTRAINT_H #define CONETWISTCONSTRAINT_H -#include "../../LinearMath/btVector3.h" +#include "LinearMath/btVector3.h" #include "btJacobianEntry.h" #include "btTypedConstraint.h" @@ -42,10 +42,14 @@ public: btScalar m_biasFactor; btScalar m_relaxationFactor; + btScalar m_damping; + btScalar m_swingSpan1; btScalar m_swingSpan2; btScalar m_twistSpan; + btScalar m_fixThresh; + btVector3 m_swingAxis; btVector3 m_twistAxis; @@ -56,6 +60,8 @@ public: btScalar m_swingCorrection; btScalar m_twistCorrection; + btScalar m_twistAngle; + btScalar m_accSwingLimitImpulse; btScalar m_accTwistLimitImpulse; @@ -63,6 +69,19 @@ public: bool m_solveTwistLimit; bool m_solveSwingLimit; + bool m_useSolveConstraintObsolete; + + // not yet used... + btScalar m_swingLimitRatio; + btScalar m_twistLimitRatio; + btVector3 m_twistAxisA; + + // motor + bool m_bMotorEnabled; + bool m_bNormalizedMotorStrength; + btQuaternion m_qTarget; + btScalar m_maxMotorImpulse; + btVector3 m_accMotorImpulse; public: @@ -74,7 +93,12 @@ public: virtual void buildJacobian(); - virtual void solveConstraint(btScalar timeStep); + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + + + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); void updateRHS(btScalar timeStep); @@ -92,7 +116,32 @@ public: m_angularOnly = angularOnly; } - void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 0.8f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) + void setLimit(int limitIndex,btScalar limitValue) + { + switch (limitIndex) + { + case 3: + { + m_twistSpan = limitValue; + break; + } + case 4: + { + m_swingSpan2 = limitValue; + break; + } + case 5: + { + m_swingSpan1 = limitValue; + break; + } + default: + { + } + }; + } + + void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) { m_swingSpan1 = _swingSpan1; m_swingSpan2 = _swingSpan2; @@ -121,6 +170,60 @@ public: return m_twistLimitSign; } + void calcAngleInfo(); + void calcAngleInfo2(); + + inline btScalar getSwingSpan1() + { + return m_swingSpan1; + } + inline btScalar getSwingSpan2() + { + return m_swingSpan2; + } + inline btScalar getTwistSpan() + { + return m_twistSpan; + } + inline btScalar getTwistAngle() + { + return m_twistAngle; + } + bool isPastSwingLimit() { return m_solveSwingLimit; } + + + void setDamping(btScalar damping) { m_damping = damping; } + + void enableMotor(bool b) { m_bMotorEnabled = b; } + void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; } + void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; } + + btScalar getFixThresh() { return m_fixThresh; } + void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; } + + // setMotorTarget: + // q: the desired rotation of bodyA wrt bodyB. + // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability) + // note: don't forget to enableMotor() + void setMotorTarget(const btQuaternion &q); + + // same as above, but q is the desired rotation of frameA wrt frameB in constraint space + void setMotorTargetInConstraintSpace(const btQuaternion &q); + + btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const; + + + +protected: + void init(); + + void computeConeLimitInfo(const btQuaternion& qCone, // in + btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs + + void computeTwistLimitInfo(const btQuaternion& qTwist, // in + btScalar& twistAngle, btVector3& vTwistAxis); // all outs + + void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const; }; #endif //CONETWISTCONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index 4d7cd05feb7..012c321fd6d 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -22,7 +22,7 @@ subject to the following restrictions: #include "LinearMath/btMinMax.h" #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" -#define ASSERT2 assert +#define ASSERT2 btAssert #define USE_INTERNAL_APPLY_IMPULSE 1 @@ -52,7 +52,7 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, btVector3 vel = vel1 - vel2; - btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), + btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), body2.getCenterOfMassTransform().getBasis().transpose(), rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(), body2.getInvInertiaDiagLocal(),body2.getInvMass()); @@ -114,7 +114,7 @@ btScalar resolveSingleCollision( btScalar Kcor = Kerp *Kfps; btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; - assert(cpd); + btAssert(cpd); btScalar distance = cpd->m_penetration; btScalar positionalError = Kcor *-distance; btScalar velocityError = cpd->m_restitution - rel_vel;// * damping; @@ -166,7 +166,7 @@ btScalar resolveSingleFriction( btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; - assert(cpd); + btAssert(cpd); btScalar combinedFriction = cpd->m_friction; @@ -255,7 +255,7 @@ btScalar resolveSingleFrictionOriginal( btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; - assert(cpd); + btAssert(cpd); btScalar combinedFriction = cpd->m_friction; @@ -337,7 +337,7 @@ btScalar resolveSingleCollisionCombined( btScalar Kcor = Kerp *Kfps; btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; - assert(cpd); + btAssert(cpd); btScalar distance = cpd->m_penetration; btScalar positionalError = Kcor *-distance; btScalar velocityError = cpd->m_restitution - rel_vel;// * damping; @@ -425,5 +425,5 @@ btScalar resolveSingleFrictionEmpty( return btScalar(0.); -}; +} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h index 826e79f78bd..e8871f3860b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef CONTACT_CONSTRAINT_H #define CONTACT_CONSTRAINT_H -//todo: make into a proper class working with the iterative constraint solver +///@todo: make into a proper class working with the iterative constraint solver class btRigidBody; #include "LinearMath/btVector3.h" diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 916d4581f79..e99430c00de 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -21,7 +21,13 @@ enum btSolverMode SOLVER_RANDMIZE_ORDER = 1, SOLVER_FRICTION_SEPARATE = 2, SOLVER_USE_WARMSTARTING = 4, - SOLVER_CACHE_FRIENDLY = 8 + SOLVER_USE_FRICTION_WARMSTARTING = 8, + SOLVER_USE_2_FRICTION_DIRECTIONS = 16, + SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32, + SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64, + SOLVER_CACHE_FRIENDLY = 128, + SOLVER_SIMD = 256, //enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version + SOLVER_CUDA = 512 //will be open sourced during Game Developers Conference 2009. Much faster. }; struct btContactSolverInfoData @@ -38,12 +44,14 @@ struct btContactSolverInfoData btScalar m_sor; btScalar m_erp;//used as Baumgarte factor btScalar m_erp2;//used in Split Impulse + btScalar m_globalCfm;//constraint force mixing int m_splitImpulse; btScalar m_splitImpulsePenetrationThreshold; btScalar m_linearSlop; btScalar m_warmstartingFactor; int m_solverMode; + int m_restingContactRestitutionThreshold; }; @@ -63,12 +71,14 @@ struct btContactSolverInfo : public btContactSolverInfoData m_numIterations = 10; m_erp = btScalar(0.2); m_erp2 = btScalar(0.1); - m_sor = btScalar(1.3); + m_globalCfm = btScalar(0.); + m_sor = btScalar(1.); m_splitImpulse = false; m_splitImpulsePenetrationThreshold = -0.02f; m_linearSlop = btScalar(0.0); m_warmstartingFactor=btScalar(0.85); - m_solverMode = SOLVER_RANDMIZE_ORDER | SOLVER_CACHE_FRIENDLY | SOLVER_USE_WARMSTARTING; + m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD ;//SOLVER_RANDMIZE_ORDER + m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution } }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 077b326d13a..6cbfe61f700 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -19,18 +19,40 @@ email: projectileman@yahoo.com http://gimpact.sf.net */ - #include "btGeneric6DofConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" #include <new> -static const btScalar kSign[] = { btScalar(1.0), btScalar(-1.0), btScalar(1.0) }; -static const int kAxisA[] = { 1, 0, 0 }; -static const int kAxisB[] = { 2, 2, 1 }; +#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) { @@ -39,51 +61,48 @@ btScalar btGetMatrixElem(const btMatrix3x3& mat, int index) 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 -// + // // 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 + // - if (btGetMatrixElem(mat,2) < btScalar(1.0)) + btScalar fi = btGetMatrixElem(mat,2); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) { - if (btGetMatrixElem(mat,2) > btScalar(-1.0)) - { - 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; - } + 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. XAngle + ZAngle = atan2(r10,r11) - xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); - xyz[1] = SIMD_HALF_PI; - xyz[2] = 0.0; - + // 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) @@ -107,212 +126,239 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value) m_currentLimit = 0;//Free from violation return 0; - + } +//----------------------------------------------------------------------------- btScalar btRotationalLimitMotor::solveAngularLimits( - btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, - btRigidBody * body0, btRigidBody * body1) + btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, + btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB) { - if (needApplyTorques()==false) return 0.0f; + if (needApplyTorques()==false) return 0.0f; - btScalar target_velocity = m_targetVelocity; - btScalar maxMotorForce = m_maxMotorForce; + 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; - } + if (m_currentLimit!=0) + { + target_velocity = -m_ERP*m_currentLimitError/(timeStep); + maxMotorForce = m_maxLimitForce; + } - maxMotorForce *= timeStep; + maxMotorForce *= timeStep; - // current velocity difference - btVector3 vel_diff = body0->getAngularVelocity(); - if (body1) - { - vel_diff -= body1->getAngularVelocity(); - } + // 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); + + + btScalar rel_vel = axis.dot(vel_diff); // correction velocity - btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); + 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 - } + 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; + btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; // clip correction impulse - btScalar clippedMotorImpulse; + btScalar clippedMotorImpulse; - //todo: should clip against accumulated impulse - if (unclippedMotorImpulse>0.0f) - { - clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; - } - else - { - clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; - } + ///@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(-1e30); - btScalar hi = btScalar(1e30); - - btScalar oldaccumImpulse = m_accumulatedImpulse; - btScalar sum = oldaccumImpulse + clippedMotorImpulse; - m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; + btScalar lo = btScalar(-1e30); + btScalar hi = btScalar(1e30); - clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; + 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; - 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); - body0->applyTorqueImpulse(motorImp); - if (body1) body1->applyTorqueImpulse(-motorImp); - return clippedMotorImpulse; + return clippedMotorImpulse; } //////////////////////////// End btRotationalLimitMotor //////////////////////////////////// + + + //////////////////////////// btTranslationalLimitMotor //////////////////////////////////// -btScalar btTranslationalLimitMotor::solveLinearAxis( - btScalar timeStep, - btScalar jacDiagABInv, - btRigidBody& body1,const btVector3 &pointInA, - btRigidBody& body2,const btVector3 &pointInB, - int limit_index, - const btVector3 & axis_normal_on_a, - const btVector3 & anchorPos) + + +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; + } -///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(); + 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; + }; - btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; + m_currentLimit[limitIndex] = 0;//Free from violation + m_currentLimitError[limitIndex] = btScalar(0.f); + return 0; +} // btTranslationalLimitMotor::testLimitValue() - btScalar rel_vel = axis_normal_on_a.dot(vel); +//----------------------------------------------------------------------------- +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(); -/// apply displacement correction + btVector3 vel1; + bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); + btVector3 vel2; + bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); + btVector3 vel = vel1 - vel2; -//positional error (zeroth order error) - btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); - btScalar lo = btScalar(-1e30); - btScalar hi = btScalar(1e30); + btScalar rel_vel = axis_normal_on_a.dot(vel); - 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; - } - } - } - } + /// apply displacement correction - btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; + //positional error (zeroth order error) + btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); + btScalar lo = btScalar(-1e30); + btScalar hi = btScalar(1e30); + 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); - return normalImpulse; -} -//////////////////////////// btTranslationalLimitMotor //////////////////////////////////// + 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; -btGeneric6DofConstraint::btGeneric6DofConstraint() - :btTypedConstraint(D6_CONSTRAINT_TYPE), - m_useLinearReferenceFrameA(true) -{ -} + btVector3 impulse_vector = axis_normal_on_a * normalImpulse; + //body1.applyImpulse( impulse_vector, rel_pos1); + //body2.applyImpulse(-impulse_vector, rel_pos2); -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) -{ + 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. - + // 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); @@ -320,34 +366,29 @@ void btGeneric6DofConstraint::calculateAngleInfo() m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); - -// if(m_debugDrawer) -// { -// -// char buff[300]; -// sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ", -// m_calculatedAxisAngleDiff[0], -// m_calculatedAxisAngleDiff[1], -// m_calculatedAxisAngleDiff[2]); -// m_debugDrawer->reportErrorWarning(buff); -// } + m_calculatedAxis[0].normalize(); + m_calculatedAxis[1].normalize(); + m_calculatedAxis[2].normalize(); } +//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::calculateTransforms() { - m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA; - m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB; - - calculateAngleInfo(); + m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA; + m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB; + calculateLinearInfo(); + calculateAngleInfo(); } +//----------------------------------------------------------------------------- void btGeneric6DofConstraint::buildLinearJacobian( - btJacobianEntry & jacLinear,const btVector3 & normalWorld, - const btVector3 & pivotAInW,const btVector3 & pivotBInW) + btJacobianEntry & jacLinear,const btVector3 & normalWorld, + const btVector3 & pivotAInW,const btVector3 & pivotBInW) { - new (&jacLinear) btJacobianEntry( + new (&jacLinear) btJacobianEntry( m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), pivotAInW - m_rbA.getCenterOfMassPosition(), @@ -357,13 +398,14 @@ void btGeneric6DofConstraint::buildLinearJacobian( m_rbA.getInvMass(), m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvMass()); - } +//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::buildAngularJacobian( - btJacobianEntry & jacAngular,const btVector3 & jointAxisW) + btJacobianEntry & jacAngular,const btVector3 & jointAxisW) { - new (&jacAngular) btJacobianEntry(jointAxisW, + new (&jacAngular) btJacobianEntry(jointAxisW, m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), m_rbA.getInvInertiaDiagLocal(), @@ -371,142 +413,260 @@ void btGeneric6DofConstraint::buildAngularJacobian( } +//----------------------------------------------------------------------------- + bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) { - btScalar angle = m_calculatedAxisAngleDiff[axis_index]; - - //test limits - m_angularLimits[axis_index].testLimitValue(angle); - return m_angularLimits[axis_index].needApplyTorques(); + btScalar angle = m_calculatedAxisAngleDiff[axis_index]; + //test limits + m_angularLimits[axis_index].testLimitValue(angle); + return m_angularLimits[axis_index].needApplyTorques(); } +//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::buildJacobian() { + 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(); - -// 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); + // 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(); + + // 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); + 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); - } - } + // 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); + } + } + + } +} +//----------------------------------------------------------------------------- +void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) +{ + if (m_useSolveConstraintObsolete) + { + info->m_numConstraintRows = 0; + info->nub = 0; + } else + { + //prepare constraint + calculateTransforms(); + 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::getInfo2 (btConstraintInfo2* info) +{ + btAssert(!m_useSolveConstraintObsolete); + int row = setLinearLimits(info); + setAngularLimits(info, row); } +//----------------------------------------------------------------------------- -void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) +int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info) { - m_timeStep = timeStep; + btGeneric6DofConstraint * d6constraint = this; + 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_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, &m_rbA, &m_rbB, info, row, axis, 0); + } + } + return row; +} - //calculateTransforms(); +//----------------------------------------------------------------------------- - int i; +int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset) +{ + 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), + &m_rbA, + &m_rbB, + info,row,axis,1); + } + } - // linear + return row; +} - 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(); +void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) +{ + if (m_useSolveConstraintObsolete) + { - 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,pointInA, - m_rbB,pointInB, - i,linear_axis, m_AnchorPos); + m_timeStep = timeStep; - } - } + //calculateTransforms(); - // angular - btVector3 angular_axis; - btScalar angularJacDiagABInv; - for (i=0;i<3;i++) - { - if (m_angularLimits[i].needApplyTorques()) - { + int i; - // get axis - angular_axis = getAxis(i); + // linear - angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal(); + btVector3 pointInA = m_calculatedTransformA.getOrigin(); + btVector3 pointInB = m_calculatedTransformB.getOrigin(); - m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,&m_rbB); - } - } + 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; + (void)timeStep; } +//----------------------------------------------------------------------------- + btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const { - return m_calculatedAxis[axis_index]; + return m_calculatedAxis[axis_index]; } +//----------------------------------------------------------------------------- + btScalar btGeneric6DofConstraint::getAngle(int axis_index) const { - return m_calculatedAxisAngleDiff[axis_index]; + return m_calculatedAxisAngleDiff[axis_index]; } +//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::calcAnchorPos(void) { btScalar imA = m_rbA.getInvMass(); @@ -526,3 +686,144 @@ void btGeneric6DofConstraint::calcAnchorPos(void) return; } // btGeneric6DofConstraint::calcAnchorPos() +//----------------------------------------------------------------------------- + +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.testLimitValue(i, m_calculatedLinearDiff[i]); + } +} // btGeneric6DofConstraint::calculateLinearInfo() + +//----------------------------------------------------------------------------- + +int btGeneric6DofConstraint::get_limit_motor_info2( + btRotationalLimitMotor * limot, + btRigidBody * body0, btRigidBody * body1, + 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) && limit) + { + btVector3 ltd; // Linear Torque Decoupling vector + btVector3 c = m_calculatedTransformB.getOrigin() - body0->getCenterOfMassPosition(); + 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() - body1->getCenterOfMassPosition(); + 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) + { + info->m_constraintError[srow] += 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 = body0->getAngularVelocity().dot(ax1); + if (body1) + vel -= body1->getAngularVelocity().dot(ax1); + } + else + { + vel = body0->getLinearVelocity().dot(ax1); + if (body1) + vel -= body1->getLinearVelocity().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; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index f0718d2d4a0..0ae161d5bdf 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -30,6 +30,8 @@ http://gimpact.sf.net class btRigidBody; + + //! Rotation Limit structure for generic joints class btRotationalLimitMotor { @@ -92,7 +94,7 @@ public: //! Is limited bool isLimited() { - if(m_loLimit>=m_hiLimit) return false; + if(m_loLimit > m_hiLimit) return false; return true; } @@ -110,8 +112,7 @@ public: int testLimitValue(btScalar test_value); //! apply the correction impulses for two bodies - btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1); - + btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB); }; @@ -129,6 +130,11 @@ public: btScalar m_damping;//!< Damping for linear limit btScalar m_restitution;//! Bounce parameter for linear limit //!@} + bool m_enableMotor[3]; + btVector3 m_targetVelocity;//!< target motor velocity + btVector3 m_maxMotorForce;//!< max force on motor + btVector3 m_currentLimitError;//! How much is violated this limit + int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit btTranslationalLimitMotor() { @@ -139,6 +145,12 @@ public: m_limitSoftness = 0.7f; m_damping = btScalar(1.0f); m_restitution = btScalar(0.5f); + for(int i=0; i < 3; i++) + { + m_enableMotor[i] = false; + m_targetVelocity[i] = btScalar(0.f); + m_maxMotorForce[i] = btScalar(0.f); + } } btTranslationalLimitMotor(const btTranslationalLimitMotor & other ) @@ -150,6 +162,12 @@ public: m_limitSoftness = other.m_limitSoftness ; m_damping = other.m_damping; m_restitution = other.m_restitution; + for(int i=0; i < 3; i++) + { + m_enableMotor[i] = other.m_enableMotor[i]; + m_targetVelocity[i] = other.m_targetVelocity[i]; + m_maxMotorForce[i] = other.m_maxMotorForce[i]; + } } //! Test limit @@ -163,13 +181,19 @@ public: { return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); } + inline bool needApplyForce(int limitIndex) + { + if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false; + return true; + } + int testLimitValue(int limitIndex, btScalar test_value); btScalar solveLinearAxis( btScalar timeStep, btScalar jacDiagABInv, - btRigidBody& body1,const btVector3 &pointInA, - btRigidBody& body2,const btVector3 &pointInB, + 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); @@ -247,6 +271,7 @@ protected: btTransform m_calculatedTransformB; btVector3 m_calculatedAxisAngleDiff; btVector3 m_calculatedAxis[3]; + btVector3 m_calculatedLinearDiff; btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes @@ -262,6 +287,9 @@ protected: } + int setAngularLimits(btConstraintInfo2 *info, int row_offset); + + int setLinearLimits(btConstraintInfo2 *info); void buildLinearJacobian( btJacobianEntry & jacLinear,const btVector3 & normalWorld, @@ -269,6 +297,8 @@ protected: void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW); + // tests linear limits + void calculateLinearInfo(); //! calcs the euler angles between the two bodies. void calculateAngleInfo(); @@ -276,6 +306,10 @@ protected: public: + + ///for backwards compatibility during the transition to 'getInfo/getInfo2' + bool m_useSolveConstraintObsolete; + btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); btGeneric6DofConstraint(); @@ -330,7 +364,11 @@ public: //! performs Jacobian calculation, and also calculates angle differences and axis virtual void buildJacobian(); - virtual void solveConstraint(btScalar timeStep); + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); void updateRHS(btScalar timeStep); @@ -432,6 +470,11 @@ public: virtual void calcAnchorPos(void); // overridable + int get_limit_motor_info2( btRotationalLimitMotor * limot, + btRigidBody * body0, btRigidBody * body1, + btConstraintInfo2 *info, int row, btVector3& ax1, int rotational); + + }; #endif //GENERIC_6DOF_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index a0523a8c76b..b6b34305804 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -19,19 +19,33 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" #include "LinearMath/btMinMax.h" #include <new> +#include "btSolverBody.h" + +//----------------------------------------------------------------------------- + +#define HINGE_USE_OBSOLETE_SOLVER false + +//----------------------------------------------------------------------------- btHingeConstraint::btHingeConstraint() : btTypedConstraint (HINGE_CONSTRAINT_TYPE), -m_enableAngularMotor(false) +m_enableAngularMotor(false), +m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), +m_useReferenceFrameA(false) { + m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } +//----------------------------------------------------------------------------- + btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, - btVector3& axisInA,btVector3& axisInB) + btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), m_angularOnly(false), - m_enableAngularMotor(false) + m_enableAngularMotor(false), + m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), + m_useReferenceFrameA(useReferenceFrameA) { m_rbAFrame.getOrigin() = pivotInA; @@ -60,9 +74,9 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = pivotInB; - m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(), - rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(), - rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() ); + m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), + rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), + rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); //start with free m_lowerLimit = btScalar(1e30); @@ -71,32 +85,28 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; - + m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } +//----------------------------------------------------------------------------- -btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA) -:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false) +btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA) +:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), +m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), +m_useReferenceFrameA(useReferenceFrameA) { // since no frame is given, assume this to be zero angle and just pick rb transform axis // fixed axis in worldspace - btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); - btScalar projection = rbAxisA1.dot(axisInA); - if (projection > SIMD_EPSILON) - rbAxisA1 = rbAxisA1*projection - axisInA; - else - rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); - - btVector3 rbAxisA2 = axisInA.cross(rbAxisA1); + btVector3 rbAxisA1, rbAxisA2; + btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); m_rbAFrame.getOrigin() = pivotInA; m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); - - btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA; + btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); @@ -115,19 +125,19 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA, m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; + m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } +//----------------------------------------------------------------------------- + btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, - const btTransform& rbAFrame, const btTransform& rbBFrame) + const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), m_angularOnly(false), -m_enableAngularMotor(false) +m_enableAngularMotor(false), +m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), +m_useReferenceFrameA(useReferenceFrameA) { - // flip axis - m_rbBFrame.getBasis()[0][2] *= btScalar(-1.); - m_rbBFrame.getBasis()[1][2] *= btScalar(-1.); - m_rbBFrame.getBasis()[2][2] *= btScalar(-1.); - //start with free m_lowerLimit = btScalar(1e30); m_upperLimit = btScalar(-1e30); @@ -135,22 +145,20 @@ m_enableAngularMotor(false) m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; + m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } +//----------------------------------------------------------------------------- - -btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame) +btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), m_angularOnly(false), -m_enableAngularMotor(false) +m_enableAngularMotor(false), +m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), +m_useReferenceFrameA(useReferenceFrameA) { ///not providing rigidbody B means implicitly using worldspace for body B - // flip axis - m_rbBFrame.getBasis()[0][2] *= btScalar(-1.); - m_rbBFrame.getBasis()[1][2] *= btScalar(-1.); - m_rbBFrame.getBasis()[2][2] *= btScalar(-1.); - m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin()); //start with free @@ -160,33 +168,38 @@ m_enableAngularMotor(false) m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; + m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } +//----------------------------------------------------------------------------- + void btHingeConstraint::buildJacobian() { - m_appliedImpulse = btScalar(0.); - - if (!m_angularOnly) + if (m_useSolveConstraintObsolete) { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - btVector3 relPos = pivotBInW - pivotAInW; + m_appliedImpulse = btScalar(0.); - btVector3 normal[3]; - if (relPos.length2() > SIMD_EPSILON) + if (!m_angularOnly) { - normal[0] = relPos.normalized(); - } - else - { - normal[0].setValue(btScalar(1.0),0,0); - } + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + btVector3 relPos = pivotBInW - pivotAInW; + + btVector3 normal[3]; + if (relPos.length2() > SIMD_EPSILON) + { + normal[0] = relPos.normalized(); + } + else + { + normal[0].setValue(btScalar(1.0),0,0); + } - btPlaneSpace1(normal[0], normal[1], normal[2]); + btPlaneSpace1(normal[0], normal[1], normal[2]); - for (int i=0;i<3;i++) - { - new (&m_jac[i]) btJacobianEntry( + for (int i=0;i<3;i++) + { + new (&m_jac[i]) btJacobianEntry( m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), pivotAInW - m_rbA.getCenterOfMassPosition(), @@ -196,214 +209,458 @@ void btHingeConstraint::buildJacobian() m_rbA.getInvMass(), m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvMass()); + } } - } - //calculate two perpendicular jointAxis, orthogonal to hingeAxis - //these two jointAxis require equal angular velocities for both bodies + //calculate two perpendicular jointAxis, orthogonal to hingeAxis + //these two jointAxis require equal angular velocities for both bodies - //this is unused for now, it's a todo - btVector3 jointAxis0local; - btVector3 jointAxis1local; - - btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); - - getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; - btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; - btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + //this is unused for now, it's a todo + btVector3 jointAxis0local; + btVector3 jointAxis1local; - new (&m_jacAng[0]) btJacobianEntry(jointAxis0, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - - new (&m_jacAng[1]) btJacobianEntry(jointAxis1, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - - new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); + btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); + getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; + btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; + btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + + new (&m_jacAng[0]) btJacobianEntry(jointAxis0, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + new (&m_jacAng[1]) btJacobianEntry(jointAxis1, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + // clear accumulator + m_accLimitImpulse = btScalar(0.); + + // test angular limit + testLimit(); + + //Compute K = J*W*J' for hinge axis + btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + + getRigidBodyB().computeAngularImpulseDenominator(axisA)); - // Compute limit information - btScalar hingeAngle = getHingeAngle(); + } +} - //set bias, sign, clear accumulator - m_correction = btScalar(0.); - m_limitSign = btScalar(0.); - m_solveLimit = false; - m_accLimitImpulse = btScalar(0.); +//----------------------------------------------------------------------------- -// if (m_lowerLimit < m_upperLimit) - if (m_lowerLimit <= m_upperLimit) +void btHingeConstraint::getInfo1(btConstraintInfo1* info) +{ + if (m_useSolveConstraintObsolete) { -// if (hingeAngle <= m_lowerLimit*m_limitSoftness) - if (hingeAngle <= m_lowerLimit) - { - m_correction = (m_lowerLimit - hingeAngle); - m_limitSign = 1.0f; - m_solveLimit = true; - } -// else if (hingeAngle >= m_upperLimit*m_limitSoftness) - else if (hingeAngle >= m_upperLimit) + info->m_numConstraintRows = 0; + info->nub = 0; + } + else + { + info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular + info->nub = 1; + //prepare constraint + testLimit(); + if(getSolveLimit() || getEnableAngularMotor()) { - m_correction = m_upperLimit - hingeAngle; - m_limitSign = -1.0f; - m_solveLimit = true; + info->m_numConstraintRows++; // limit 3rd anguar as well + info->nub--; } } +} // btHingeConstraint::getInfo1 () - //Compute K = J*W*J' for hinge axis - btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + - getRigidBodyB().computeAngularImpulseDenominator(axisA)); +//----------------------------------------------------------------------------- +void btHingeConstraint::getInfo2 (btConstraintInfo2* info) +{ + btAssert(!m_useSolveConstraintObsolete); + int i, s = info->rowskip; + // transforms in world space + btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame; + btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame; + // pivot point + btVector3 pivotAInW = trA.getOrigin(); + btVector3 pivotBInW = trB.getOrigin(); + // linear (all fixed) + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[s + 1] = 1; + info->m_J1linearAxis[2 * s + 2] = 1; + btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin(); + { + btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s); + btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s); + btVector3 a1neg = -a1; + a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin(); + { + btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s); + btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s); + a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + // linear RHS + btScalar k = info->fps * info->erp; + for(i = 0; i < 3; i++) + { + info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]); + } + // make rotations around X and Y equal + // the hinge axis should be the only unconstrained + // rotational axis, the angular velocity of the two bodies perpendicular to + // the hinge axis should be equal. thus the constraint equations are + // p*w1 - p*w2 = 0 + // q*w1 - q*w2 = 0 + // where p and q are unit vectors normal to the hinge axis, and w1 and w2 + // are the angular velocity vectors of the two bodies. + // get hinge axis (Z) + btVector3 ax1 = trA.getBasis().getColumn(2); + // get 2 orthos to hinge axis (X, Y) + btVector3 p = trA.getBasis().getColumn(0); + btVector3 q = trA.getBasis().getColumn(1); + // set the two hinge angular rows + int s3 = 3 * info->rowskip; + int s4 = 4 * info->rowskip; + + info->m_J1angularAxis[s3 + 0] = p[0]; + info->m_J1angularAxis[s3 + 1] = p[1]; + info->m_J1angularAxis[s3 + 2] = p[2]; + info->m_J1angularAxis[s4 + 0] = q[0]; + info->m_J1angularAxis[s4 + 1] = q[1]; + info->m_J1angularAxis[s4 + 2] = q[2]; + + info->m_J2angularAxis[s3 + 0] = -p[0]; + info->m_J2angularAxis[s3 + 1] = -p[1]; + info->m_J2angularAxis[s3 + 2] = -p[2]; + info->m_J2angularAxis[s4 + 0] = -q[0]; + info->m_J2angularAxis[s4 + 1] = -q[1]; + info->m_J2angularAxis[s4 + 2] = -q[2]; + // compute the right hand side of the constraint equation. set relative + // body velocities along p and q to bring the hinge back into alignment. + // if ax1,ax2 are the unit length hinge axes as computed from body1 and + // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). + // if `theta' is the angle between ax1 and ax2, we need an angular velocity + // along u to cover angle erp*theta in one step : + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| + // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) + // ...as ax1 and ax2 are unit length. if theta is smallish, + // theta ~= sin(theta), so + // angular_velocity = (erp*fps) * (ax1 x ax2) + // ax1 x ax2 is in the plane space of ax1, so we project the angular + // velocity to p and q to find the right hand side. + btVector3 ax2 = trB.getBasis().getColumn(2); + btVector3 u = ax1.cross(ax2); + info->m_constraintError[s3] = k * u.dot(p); + info->m_constraintError[s4] = k * u.dot(q); + // check angular limits + int nrow = 4; // last filled row + int srow; + btScalar limit_err = btScalar(0.0); + int limit = 0; + if(getSolveLimit()) + { + limit_err = m_correction * m_referenceSign; + limit = (limit_err > btScalar(0.0)) ? 1 : 2; + } + // if the hinge has joint limits or motor, add in the extra row + int powered = 0; + if(getEnableAngularMotor()) + { + powered = 1; + } + if(limit || powered) + { + nrow++; + srow = nrow * info->rowskip; + info->m_J1angularAxis[srow+0] = ax1[0]; + info->m_J1angularAxis[srow+1] = ax1[1]; + info->m_J1angularAxis[srow+2] = ax1[2]; + + info->m_J2angularAxis[srow+0] = -ax1[0]; + info->m_J2angularAxis[srow+1] = -ax1[1]; + info->m_J2angularAxis[srow+2] = -ax1[2]; + + btScalar lostop = getLowerLimit(); + btScalar histop = getUpperLimit(); + if(limit && (lostop == histop)) + { // the joint motor is ineffective + powered = 0; + } + info->m_constraintError[srow] = btScalar(0.0f); + if(powered) + { + info->cfm[srow] = btScalar(0.0); + btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp); + info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; + info->m_lowerLimit[srow] = - m_maxMotorImpulse; + info->m_upperLimit[srow] = m_maxMotorImpulse; + } + if(limit) + { + k = info->fps * info->erp; + info->m_constraintError[srow] += k * limit_err; + info->cfm[srow] = btScalar(0.0); + if(lostop == histop) + { + // limited low and high simultaneously + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else if(limit == 1) + { // low limit + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else + { // high limit + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = 0; + } + // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) + btScalar bounce = m_relaxationFactor; + if(bounce > btScalar(0.0)) + { + btScalar vel = m_rbA.getAngularVelocity().dot(ax1); + vel -= m_rbB.getAngularVelocity().dot(ax1); + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if(limit == 1) + { // low limit + if(vel < 0) + { + btScalar newc = -bounce * vel; + if(newc > info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + else + { // high limit - all those computations are reversed + if(vel > 0) + { + btScalar newc = -bounce * vel; + if(newc < info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + } + info->m_constraintError[srow] *= m_biasFactor; + } // if(limit) + } // if angular limit or powered } -void btHingeConstraint::solveConstraint(btScalar timeStep) +//----------------------------------------------------------------------------- + +void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + ///for backwards compatibility during the transition to 'getInfo/getInfo2' + if (m_useSolveConstraintObsolete) + { + + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - btScalar tau = btScalar(0.3); + btScalar tau = btScalar(0.3); - //linear part - if (!m_angularOnly) - { - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - for (int i=0;i<3;i++) - { - const btVector3& normal = m_jac[i].m_linearJointAxis; - btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); - - btScalar rel_vel; - rel_vel = normal.dot(vel); - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; - m_appliedImpulse += impulse; - btVector3 impulse_vector = normal * impulse; - m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); - m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); + //linear part + if (!m_angularOnly) + { + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + btVector3 vel1,vel2; + bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); + bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); + btVector3 vel = vel1 - vel2; + + for (int i=0;i<3;i++) + { + const btVector3& normal = m_jac[i].m_linearJointAxis; + btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); + + btScalar rel_vel; + rel_vel = normal.dot(vel); + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; + m_appliedImpulse += impulse; + btVector3 impulse_vector = normal * impulse; + btVector3 ftorqueAxis1 = rel_pos1.cross(normal); + btVector3 ftorqueAxis2 = rel_pos2.cross(normal); + bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); + bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); + } } - } - - { - ///solve angular part + + { + ///solve angular part - // get axes in world space - btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); + // get axes in world space + btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); - const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); - const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); + btVector3 angVelA; + bodyA.getAngularVelocity(angVelA); + btVector3 angVelB; + bodyB.getAngularVelocity(angVelB); - btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); - btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); + btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); + btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); - btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; - btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; - btVector3 velrelOrthog = angAorthog-angBorthog; - { - //solve orthogonal angular velocity correction - btScalar relaxation = btScalar(1.); - btScalar len = velrelOrthog.length(); - if (len > btScalar(0.00001)) + btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; + btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; + btVector3 velrelOrthog = angAorthog-angBorthog; { - btVector3 normal = velrelOrthog.normalized(); - btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + - getRigidBodyB().computeAngularImpulseDenominator(normal); - // scale for mass and relaxation - //todo: expose this 0.9 factor to developer - velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; - } + - //solve angular positional correction - btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep); - btScalar len2 = angularError.length(); - if (len2>btScalar(0.00001)) - { - btVector3 normal2 = angularError.normalized(); - btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + - getRigidBodyB().computeAngularImpulseDenominator(normal2); - angularError *= (btScalar(1.)/denom2) * relaxation; - } + //solve orthogonal angular velocity correction + btScalar relaxation = btScalar(1.); + btScalar len = velrelOrthog.length(); + if (len > btScalar(0.00001)) + { + btVector3 normal = velrelOrthog.normalized(); + btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + + getRigidBodyB().computeAngularImpulseDenominator(normal); + // scale for mass and relaxation + //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; - m_rbA.applyTorqueImpulse(-velrelOrthog+angularError); - m_rbB.applyTorqueImpulse(velrelOrthog-angularError); + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom)); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom)); - // solve limit - if (m_solveLimit) - { - btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; + } - btScalar impulseMag = amplitude * m_kHinge; + //solve angular positional correction + btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep); + btScalar len2 = angularError.length(); + if (len2>btScalar(0.00001)) + { + btVector3 normal2 = angularError.normalized(); + btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + + getRigidBodyB().computeAngularImpulseDenominator(normal2); + //angularError *= (btScalar(1.)/denom2) * relaxation; + + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2)); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2)); - // Clamp the accumulated impulse - btScalar temp = m_accLimitImpulse; - m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); - impulseMag = m_accLimitImpulse - temp; + } + + - btVector3 impulse = axisA * impulseMag * m_limitSign; - m_rbA.applyTorqueImpulse(impulse); - m_rbB.applyTorqueImpulse(-impulse); - } - } - //apply motor - if (m_enableAngularMotor) - { - //todo: add limits too - btVector3 angularLimit(0,0,0); + // solve limit + if (m_solveLimit) + { + btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; + + btScalar impulseMag = amplitude * m_kHinge; + + // Clamp the accumulated impulse + btScalar temp = m_accLimitImpulse; + m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); + impulseMag = m_accLimitImpulse - temp; + + + + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign)); + + } + } - btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; - btScalar projRelVel = velrel.dot(axisA); + //apply motor + if (m_enableAngularMotor) + { + //todo: add limits too + btVector3 angularLimit(0,0,0); - btScalar desiredMotorVel = m_motorTargetVelocity; - btScalar motor_relvel = desiredMotorVel - projRelVel; + btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; + btScalar projRelVel = velrel.dot(axisA); - btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; - //todo: should clip against accumulated impulse - btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; - clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; - btVector3 motorImp = clippedMotorImpulse * axisA; + btScalar desiredMotorVel = m_motorTargetVelocity; + btScalar motor_relvel = desiredMotorVel - projRelVel; - m_rbA.applyTorqueImpulse(motorImp+angularLimit); - m_rbB.applyTorqueImpulse(-motorImp-angularLimit); + btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; + //todo: should clip against accumulated impulse + btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; + clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; + btVector3 motorImp = clippedMotorImpulse * axisA; + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse); + + } } } } +//----------------------------------------------------------------------------- + void btHingeConstraint::updateRHS(btScalar timeStep) { (void)timeStep; } +//----------------------------------------------------------------------------- + btScalar btHingeConstraint::getHingeAngle() { const btVector3 refAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0); const btVector3 refAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1); const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1); - - return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1) ); + btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); + return m_referenceSign * angle; } +//----------------------------------------------------------------------------- + +void btHingeConstraint::testLimit() +{ + // Compute limit information + m_hingeAngle = getHingeAngle(); + m_correction = btScalar(0.); + m_limitSign = btScalar(0.); + m_solveLimit = false; + if (m_lowerLimit <= m_upperLimit) + { + if (m_hingeAngle <= m_lowerLimit) + { + m_correction = (m_lowerLimit - m_hingeAngle); + m_limitSign = 1.0f; + m_solveLimit = true; + } + else if (m_hingeAngle >= m_upperLimit) + { + m_correction = m_upperLimit - m_hingeAngle; + m_limitSign = -1.0f; + m_solveLimit = true; + } + } + return; +} // btHingeConstraint::testLimit() + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 4fa9972f6d8..0af655f4409 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -53,27 +53,35 @@ public: btScalar m_correction; btScalar m_accLimitImpulse; + btScalar m_hingeAngle; + btScalar m_referenceSign; bool m_angularOnly; bool m_enableAngularMotor; bool m_solveLimit; + bool m_useSolveConstraintObsolete; + bool m_useReferenceFrameA; public: - btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB); + btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA = false); - btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA); + btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA = false); - btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame); + btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false); - btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame); + btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false); btHingeConstraint(); virtual void buildJacobian(); - virtual void solveConstraint(btScalar timeStep); + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); void updateRHS(btScalar timeStep); @@ -86,6 +94,16 @@ public: return m_rbB; } + btRigidBody& getRigidBodyA() + { + return m_rbA; + } + + btRigidBody& getRigidBodyB() + { + return m_rbB; + } + void setAngularOnly(bool angularOnly) { m_angularOnly = angularOnly; @@ -122,6 +140,8 @@ public: btScalar getHingeAngle(); + void testLimit(); + const btTransform& getAFrame() { return m_rbAFrame; }; const btTransform& getBFrame() { return m_rbBFrame; }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp index 2b69ad90438..1da749517e8 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -21,33 +21,38 @@ subject to the following restrictions: btPoint2PointConstraint::btPoint2PointConstraint() -:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE) +:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE), +m_useSolveConstraintObsolete(false) { } btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB) -:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB) +:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), +m_useSolveConstraintObsolete(false) { } btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA) -:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)) +:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), +m_useSolveConstraintObsolete(false) { } void btPoint2PointConstraint::buildJacobian() { - m_appliedImpulse = btScalar(0.); + ///we need it for both methods + { + m_appliedImpulse = btScalar(0.); - btVector3 normal(0,0,0); + btVector3 normal(0,0,0); - for (int i=0;i<3;i++) - { - normal[i] = 1; - new (&m_jac[i]) btJacobianEntry( + for (int i=0;i<3;i++) + { + normal[i] = 1; + new (&m_jac[i]) btJacobianEntry( m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), @@ -58,64 +63,162 @@ void btPoint2PointConstraint::buildJacobian() m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvMass()); normal[i] = 0; + } } } -void btPoint2PointConstraint::solveConstraint(btScalar timeStep) -{ - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; +void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info) +{ + if (m_useSolveConstraintObsolete) + { + info->m_numConstraintRows = 0; + info->nub = 0; + } else + { + info->m_numConstraintRows = 3; + info->nub = 3; + } +} - btVector3 normal(0,0,0); +void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info) +{ + btAssert(!m_useSolveConstraintObsolete); + + //retrieve matrices + btTransform body0_trans; + body0_trans = m_rbA.getCenterOfMassTransform(); + btTransform body1_trans; + body1_trans = m_rbB.getCenterOfMassTransform(); + + // anchor points in global coordinates with respect to body PORs. + + // set jacobian + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[info->rowskip+1] = 1; + info->m_J1linearAxis[2*info->rowskip+2] = 1; + + btVector3 a1 = body0_trans.getBasis()*getPivotInA(); + { + btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); + btVector3 a1neg = -a1; + a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + + /*info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[s+1] = -1; + info->m_J2linearAxis[2*s+2] = -1; + */ + btVector3 a2 = body1_trans.getBasis()*getPivotInB(); + + { + btVector3 a2n = -a2; + btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); + a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + -// btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); -// btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - - for (int i=0;i<3;i++) - { - normal[i] = 1; - btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - //this jacobian entry could be re-used for all iterations - - btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - btScalar rel_vel; - rel_vel = normal.dot(vel); + // set right hand side + btScalar k = info->fps * info->erp; + int j; - /* - //velocity error (first order error) - btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, - m_rbB.getLinearVelocity(),angvelB); - */ - - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - - btScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv; + for (j=0; j<3; j++) + { + info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); + //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); + } - btScalar impulseClamp = m_setting.m_impulseClamp; - if (impulseClamp > 0) + btScalar impulseClamp = m_setting.m_impulseClamp;// + for (j=0; j<3; j++) + { + if (m_setting.m_impulseClamp > 0) { - if (impulse < -impulseClamp) - impulse = -impulseClamp; - if (impulse > impulseClamp) - impulse = impulseClamp; + info->m_lowerLimit[j*info->rowskip] = -impulseClamp; + info->m_upperLimit[j*info->rowskip] = impulseClamp; } + } + +} + - m_appliedImpulse+=impulse; - btVector3 impulse_vector = normal * impulse; - m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); - m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); +void btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) +{ + if (m_useSolveConstraintObsolete) + { + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; + + + btVector3 normal(0,0,0); - normal[i] = 0; + + // btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + // btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + for (int i=0;i<3;i++) + { + normal[i] = 1; + btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); + + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + //this jacobian entry could be re-used for all iterations + + btVector3 vel1,vel2; + bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); + bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); + btVector3 vel = vel1 - vel2; + + btScalar rel_vel; + rel_vel = normal.dot(vel); + + /* + //velocity error (first order error) + btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + */ + + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + + btScalar deltaImpulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv; + + btScalar impulseClamp = m_setting.m_impulseClamp; + + const btScalar sum = btScalar(m_appliedImpulse) + deltaImpulse; + if (sum < -impulseClamp) + { + deltaImpulse = -impulseClamp-m_appliedImpulse; + m_appliedImpulse = -impulseClamp; + } + else if (sum > impulseClamp) + { + deltaImpulse = impulseClamp-m_appliedImpulse; + m_appliedImpulse = impulseClamp; + } + else + { + m_appliedImpulse = sum; + } + + + btVector3 impulse_vector = normal * deltaImpulse; + + btVector3 ftorqueAxis1 = rel_pos1.cross(normal); + btVector3 ftorqueAxis2 = rel_pos2.cross(normal); + bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,deltaImpulse); + bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-deltaImpulse); + + + normal[i] = 0; + } } } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h index c9d5968530c..e2b865cd484 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -50,6 +50,9 @@ public: public: + ///for backwards compatibility during the transition to 'getInfo/getInfo2' + bool m_useSolveConstraintObsolete; + btConstraintSetting m_setting; btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB); @@ -60,8 +63,12 @@ public: virtual void buildJacobian(); + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + - virtual void solveConstraint(btScalar timeStep); + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); void updateRHS(btScalar timeStep); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 41e336c9d17..6928bdb966b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -15,7 +15,6 @@ subject to the following restrictions: //#define COMPUTE_IMPULSE_DENOM 1 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms. -//#define FORCE_REFESH_CONTACT_MANIFOLDS 1 #include "btSequentialImpulseConstraintSolver.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" @@ -32,441 +31,268 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" #include "btSolverBody.h" #include "btSolverConstraint.h" - - #include "LinearMath/btAlignedObjectArray.h" +#include <string.h> //for memset +btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() +:m_btSeed2(0) +{ -int totalCpd = 0; +} -int gTotalContactPoints = 0; +btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() +{ +} -struct btOrderIndex +#ifdef USE_SIMD +#include <emmintrin.h> +#define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e)) +static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 ) { - int m_manifoldIndex; - int m_pointIndex; -}; + __m128 result = _mm_mul_ps( vec0, vec1); + return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) ); +} +#endif//USE_SIMD +// Project Gauss Seidel or the equivalent Sequential Impulse +void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +{ +#ifdef USE_SIMD + __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); + __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); + __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); + __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); + __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128)); + __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128)); + deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); + btSimdScalar resultLowerLess,resultUpperLess; + resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); + resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); + __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); + deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); + c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); + __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp); + deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); + c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) ); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass)); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass)); + __m128 impulseMagnitude = deltaImpulse; + body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); + body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); + body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); +#else + resolveSingleConstraintRowGeneric(body1,body2,c); +#endif +} +// Project Gauss Seidel or the equivalent Sequential Impulse + void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +{ + btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; + const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity); + const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity); -#define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384 -static btOrderIndex gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS]; + const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; + deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; + const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_lowerLimit; + } + else if (sum > c.m_upperLimit) + { + deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_upperLimit; + } + else + { + c.m_appliedImpulse = sum; + } + if (body1.m_invMass) + body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); + if (body2.m_invMass) + body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse); +} -unsigned long btSequentialImpulseConstraintSolver::btRand2() + void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { - m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; - return m_btSeed2; +#ifdef USE_SIMD + __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); + __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); + __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); + __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); + __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128)); + __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128)); + deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); + btSimdScalar resultLowerLess,resultUpperLess; + resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); + resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); + __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); + deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); + c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass)); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass)); + __m128 impulseMagnitude = deltaImpulse; + body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); + body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); + body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); +#else + resolveSingleConstraintRowLowerLimit(body1,body2,c); +#endif } - - -//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) -int btSequentialImpulseConstraintSolver::btRandInt2 (int n) +// Project Gauss Seidel or the equivalent Sequential Impulse + void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { - // seems good; xor-fold and modulus - const unsigned long un = static_cast<unsigned long>(n); - unsigned long r = btRand2(); - - // note: probably more aggressive than it needs to be -- might be - // able to get away without one or two of the innermost branches. - if (un <= 0x00010000UL) { - r ^= (r >> 16); - if (un <= 0x00000100UL) { - r ^= (r >> 8); - if (un <= 0x00000010UL) { - r ^= (r >> 4); - if (un <= 0x00000004UL) { - r ^= (r >> 2); - if (un <= 0x00000002UL) { - r ^= (r >> 1); - } - } - } - } - } - - return (int) (r % un); + btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; + const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity); + const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity); + + deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; + const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_lowerLimit; + } + else + { + c.m_appliedImpulse = sum; + } + if (body1.m_invMass) + body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); + if (body2.m_invMass) + body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse); } - -bool MyContactDestroyedCallback(void* userPersistentData); -bool MyContactDestroyedCallback(void* userPersistentData) +unsigned long btSequentialImpulseConstraintSolver::btRand2() { - assert (userPersistentData); - btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData; - btAlignedFree(cpd); - totalCpd--; - //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData); - return true; + m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; + return m_btSeed2; } -btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() -:m_btSeed2(0) +//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) +int btSequentialImpulseConstraintSolver::btRandInt2 (int n) { - gContactDestroyedCallback = &MyContactDestroyedCallback; - - //initialize default friction/contact funcs - int i,j; - for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++) - for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++) - { - - m_contactDispatch[i][j] = resolveSingleCollision; - m_frictionDispatch[i][j] = resolveSingleFriction; + // seems good; xor-fold and modulus + const unsigned long un = static_cast<unsigned long>(n); + unsigned long r = btRand2(); + + // note: probably more aggressive than it needs to be -- might be + // able to get away without one or two of the innermost branches. + if (un <= 0x00010000UL) { + r ^= (r >> 16); + if (un <= 0x00000100UL) { + r ^= (r >> 8); + if (un <= 0x00000010UL) { + r ^= (r >> 4); + if (un <= 0x00000004UL) { + r ^= (r >> 2); + if (un <= 0x00000002UL) { + r ^= (r >> 1); + } + } + } } + } + + return (int) (r % un); } -btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() -{ -} -void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject); -void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject) +void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject) { - btRigidBody* rb = btRigidBody::upcast(collisionObject); + btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0; + + solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); + solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); + if (rb) { - solverBody->m_angularVelocity = rb->getAngularVelocity() ; - solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin(); - solverBody->m_friction = collisionObject->getFriction(); solverBody->m_invMass = rb->getInvMass(); - solverBody->m_linearVelocity = rb->getLinearVelocity(); solverBody->m_originalBody = rb; solverBody->m_angularFactor = rb->getAngularFactor(); } else { - solverBody->m_angularVelocity.setValue(0,0,0); - solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin(); - solverBody->m_friction = collisionObject->getFriction(); solverBody->m_invMass = 0.f; - solverBody->m_linearVelocity.setValue(0,0,0); solverBody->m_originalBody = 0; solverBody->m_angularFactor = 1.f; } - - solverBody->m_pushVelocity.setValue(0.f,0.f,0.f); - solverBody->m_turnVelocity.setValue(0.f,0.f,0.f); } int gNumSplitImpulseRecoveries = 0; -btScalar restitutionCurve(btScalar rel_vel, btScalar restitution); -btScalar restitutionCurve(btScalar rel_vel, btScalar restitution) +btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution) { btScalar rest = restitution * -rel_vel; return rest; } -void resolveSplitPenetrationImpulseCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo); - -//SIMD_FORCE_INLINE -void resolveSplitPenetrationImpulseCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo) -{ - (void)solverInfo; - - if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold) - { - - gNumSplitImpulseRecoveries++; - btScalar normalImpulse; - - // Optimized version of projected relative velocity, use precomputed cross products with normal - // body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1); - // body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2); - // btVector3 vel = vel1 - vel2; - // btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel); - - btScalar rel_vel; - btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity) - + contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity); - btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity) - + contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity); - - rel_vel = vel1Dotn-vel2Dotn; - - btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep; - // btScalar positionalError = contactConstraint.m_penetration; - - btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping; - - btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv; - normalImpulse = penetrationImpulse+velocityImpulse; - - // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse - btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse; - btScalar sum = oldNormalImpulse + normalImpulse; - contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum; - - normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse; - - body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse); - - body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse); - - } - -} - - -//velocity + friction -//response between two dynamic objects with friction - -btScalar resolveSingleCollisionCombinedCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo); - -//SIMD_FORCE_INLINE -btScalar resolveSingleCollisionCombinedCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo) +void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection); +void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection) { - (void)solverInfo; - - btScalar normalImpulse; - + if (colObj && colObj->hasAnisotropicFriction()) { - - - // Optimized version of projected relative velocity, use precomputed cross products with normal - // body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1); - // body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2); - // btVector3 vel = vel1 - vel2; - // btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel); - - btScalar rel_vel; - btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) - + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity); - btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) - + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity); - - rel_vel = vel1Dotn-vel2Dotn; - - btScalar positionalError = 0.f; - if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold)) - { - positionalError = -contactConstraint.m_penetration * solverInfo.m_erp/solverInfo.m_timeStep; - } - - btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping; - - btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv; - normalImpulse = penetrationImpulse+velocityImpulse; - - - // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse - btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse; - btScalar sum = oldNormalImpulse + normalImpulse; - contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum; - - normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse; - - body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass, - contactConstraint.m_angularComponentA,normalImpulse); - - body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass, - contactConstraint.m_angularComponentB,-normalImpulse); + // transform to local coordinates + btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis(); + const btVector3& friction_scaling = colObj->getAnisotropicFriction(); + //apply anisotropic friction + loc_lateral *= friction_scaling; + // ... and transform it back to global coordinates + frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral; } - - return normalImpulse; } -//#define NO_FRICTION_TANGENTIALS 1 -#ifndef NO_FRICTION_TANGENTIALS - -btScalar resolveSingleFrictionCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo, - btScalar appliedNormalImpulse); - -//SIMD_FORCE_INLINE -btScalar resolveSingleFrictionCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo, - btScalar appliedNormalImpulse) -{ - (void)solverInfo; - - const btScalar combinedFriction = contactConstraint.m_friction; - - const btScalar limit = appliedNormalImpulse * combinedFriction; - - if (appliedNormalImpulse>btScalar(0.)) - //friction - { - - btScalar j1; - { - btScalar rel_vel; - const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) - + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity); - const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) - + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity); - rel_vel = vel1Dotn-vel2Dotn; - - // calculate j that moves us to zero relative velocity - j1 = -rel_vel * contactConstraint.m_jacDiagABInv; -#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1 -#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE - btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse; - contactConstraint.m_appliedImpulse = oldTangentImpulse + j1; - - if (limit < contactConstraint.m_appliedImpulse) - { - contactConstraint.m_appliedImpulse = limit; - } else - { - if (contactConstraint.m_appliedImpulse < -limit) - contactConstraint.m_appliedImpulse = -limit; - } - j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse; -#else - if (limit < j1) - { - j1 = limit; - } else - { - if (j1 < -limit) - j1 = -limit; - } - -#endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE - - //GEN_set_min(contactConstraint.m_appliedImpulse, limit); - //GEN_set_max(contactConstraint.m_appliedImpulse, -limit); - - - - } - - body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1); - - body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1); - - } - return 0.f; -} - - -#else - -//velocity + friction -//response between two dynamic objects with friction -btScalar resolveSingleFrictionCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo) +btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation) { - btVector3 vel1; - btVector3 vel2; - btScalar normalImpulse(0.f); - - { - const btVector3& normal = contactConstraint.m_contactNormal; - if (contactConstraint.m_penetration < 0.f) - return 0.f; - - - body1.getVelocityInLocalPoint(contactConstraint.m_relpos1CrossNormal,vel1); - body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2); - btVector3 vel = vel1 - vel2; - btScalar rel_vel; - rel_vel = normal.dot(vel); - - btVector3 lat_vel = vel - normal * rel_vel; - btScalar lat_rel_vel = lat_vel.length2(); - - btScalar combinedFriction = contactConstraint.m_friction; - const btVector3& rel_pos1 = contactConstraint.m_rel_posA; - const btVector3& rel_pos2 = contactConstraint.m_rel_posB; - - - if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON) - { - lat_rel_vel = btSqrt(lat_rel_vel); - - lat_vel /= lat_rel_vel; - btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel); - btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel); - btScalar friction_impulse = lat_rel_vel / - (body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2))); - btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction; - - btSetMin(friction_impulse, normal_impulse); - btSetMin(friction_impulse, -normal_impulse); - body1.internalApplyImpulse(lat_vel * -friction_impulse, rel_pos1); - body2.applyImpulse(lat_vel * friction_impulse, rel_pos2); - } - } - - return normalImpulse; -} - -#endif //NO_FRICTION_TANGENTIALS - - - - - -void btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation) -{ btRigidBody* body0=btRigidBody::upcast(colObj0); btRigidBody* body1=btRigidBody::upcast(colObj1); - btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand(); + btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand(); + memset(&solverConstraint,0xff,sizeof(btSolverConstraint)); solverConstraint.m_contactNormal = normalAxis; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D; solverConstraint.m_frictionIndex = frictionIndex; solverConstraint.m_friction = cp.m_combinedFriction; solverConstraint.m_originalContactPoint = 0; - solverConstraint.m_appliedImpulse = btScalar(0.); - solverConstraint.m_appliedPushImpulse = 0.f; - solverConstraint.m_penetration = 0.f; + solverConstraint.m_appliedImpulse = 0.f; + // solverConstraint.m_appliedPushImpulse = 0.f; + { btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0); + solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0); } { - btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal); + btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal); solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0); + solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0); } #ifdef COMPUTE_IMPULSE_DENOM @@ -483,7 +309,7 @@ void btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& } if (body1) { - vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2); + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); denom1 = body1->getInvMass() + normalAxis.dot(vec); } @@ -492,377 +318,495 @@ void btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& btScalar denom = relaxation/(denom0+denom1); solverConstraint.m_jacDiagABInv = denom; +#ifdef _USE_JACOBIAN + solverConstraint.m_jac = btJacobianEntry ( + rel_pos1,rel_pos2,solverConstraint.m_contactNormal, + body0->getInvInertiaDiagLocal(), + body0->getInvMass(), + body1->getInvInertiaDiagLocal(), + body1->getInvMass()); +#endif //_USE_JACOBIAN -} + { + btScalar rel_vel; + btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0)) + + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0)); + btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0)); -void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection); -void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection) + rel_vel = vel1Dotn+vel2Dotn; + + btScalar positionalError = 0.f; + + btSimdScalar velocityError = - rel_vel; + btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + } + + return solverConstraint; +} + +int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body) { - if (colObj && colObj->hasAnisotropicFriction()) + int solverBodyIdA = -1; + + if (body.getCompanionId() >= 0) { - // transform to local coordinates - btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis(); - const btVector3& friction_scaling = colObj->getAnisotropicFriction(); - //apply anisotropic friction - loc_lateral *= friction_scaling; - // ... and transform it back to global coordinates - frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral; + //body has already been converted + solverBodyIdA = body.getCompanionId(); + } else + { + btRigidBody* rb = btRigidBody::upcast(&body); + if (rb && rb->getInvMass()) + { + solverBodyIdA = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,&body); + body.setCompanionId(solverBodyIdA); + } else + { + return 0;//assume first one is a fixed solver body + } } + return solverBodyIdA; } +#include <stdio.h> - -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) { - BT_PROFILE("solveGroupCacheFriendlySetup"); - (void)stackAlloc; - (void)debugDrawer; + btCollisionObject* colObj0=0,*colObj1=0; + colObj0 = (btCollisionObject*)manifold->getBody0(); + colObj1 = (btCollisionObject*)manifold->getBody1(); - if (!(numConstraints + numManifolds)) + int solverBodyIdA=-1; + int solverBodyIdB=-1; + + if (manifold->getNumContacts()) { -// printf("empty\n"); - return 0.f; + solverBodyIdA = getOrInitSolverBody(*colObj0); + solverBodyIdB = getOrInitSolverBody(*colObj1); } - btPersistentManifold* manifold = 0; - btCollisionObject* colObj0=0,*colObj1=0; - //btRigidBody* rb0=0,*rb1=0; + btVector3 rel_pos1; + btVector3 rel_pos2; + btScalar relaxation; + for (int j=0;j<manifold->getNumContacts();j++) + { -#ifdef FORCE_REFESH_CONTACT_MANIFOLDS + btManifoldPoint& cp = manifold->getContactPoint(j); - BEGIN_PROFILE("refreshManifolds"); + if (cp.getDistance() <= manifold->getContactProcessingThreshold()) + { - int i; - - + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); - for (i=0;i<numManifolds;i++) - { - manifold = manifoldPtr[i]; - rb1 = (btRigidBody*)manifold->getBody1(); - rb0 = (btRigidBody*)manifold->getBody0(); - - manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform()); + rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); + rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - } - END_PROFILE("refreshManifolds"); -#endif //FORCE_REFESH_CONTACT_MANIFOLDS + relaxation = 1.f; + btScalar rel_vel; + btVector3 vel; - + int frictionIndex = m_tmpSolverContactConstraintPool.size(); + + { + btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand(); + btRigidBody* rb0 = btRigidBody::upcast(colObj0); + btRigidBody* rb1 = btRigidBody::upcast(colObj1); + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + + solverConstraint.m_originalContactPoint = &cp; + + btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + { +#ifdef COMPUTE_IMPULSE_DENOM + btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); +#else + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + if (rb0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); + } + if (rb1) + { + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); + } +#endif //COMPUTE_IMPULSE_DENOM + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + } + solverConstraint.m_contactNormal = cp.m_normalWorldOnB; + solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB); - //int sizeofSB = sizeof(btSolverBody); - //int sizeofSC = sizeof(btSolverConstraint); + btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); + btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - //if (1) - { - //if m_stackAlloc, try to pack bodies/constraints to speed up solving -// btBlock* sablock; -// sablock = stackAlloc->beginBlock(); + vel = vel1 - vel2; - // int memsize = 16; -// unsigned char* stackMemory = stackAlloc->allocate(memsize); + rel_vel = cp.m_normalWorldOnB.dot(vel); - - //todo: use stack allocator for this temp memory -// int minReservation = numManifolds*2; + btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop; - //m_tmpSolverBodyPool.reserve(minReservation); - //don't convert all bodies, only the one we need so solver the constraints -/* - { - for (int i=0;i<numBodies;i++) - { - btRigidBody* rb = btRigidBody::upcast(bodies[i]); - if (rb && (rb->getIslandTag() >= 0)) + solverConstraint.m_friction = cp.m_combinedFriction; + + btScalar restitution = 0.f; + + if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold) { - btAssert(rb->getCompanionId() < 0); - int solverBodyId = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,rb); - rb->setCompanionId(solverBodyId); - } - } - } -*/ - - //m_tmpSolverConstraintPool.reserve(minReservation); - //m_tmpSolverFrictionConstraintPool.reserve(minReservation); + restitution = 0.f; + } else + { + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); + if (restitution <= btScalar(0.)) + { + restitution = 0.f; + }; + } - { - int i; - for (i=0;i<numManifolds;i++) - { - manifold = manifoldPtr[i]; - colObj0 = (btCollisionObject*)manifold->getBody0(); - colObj1 = (btCollisionObject*)manifold->getBody1(); - - int solverBodyIdA=-1; - int solverBodyIdB=-1; + ///warm starting (or zero if disabled) + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + if (rb0) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + if (rb1) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse); + } else + { + solverConstraint.m_appliedImpulse = 0.f; + } + + // solverConstraint.m_appliedPushImpulse = 0.f; - if (manifold->getNumContacts()) { + btScalar rel_vel; + btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0)) + + solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0)); + btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0)); + + rel_vel = vel1Dotn+vel2Dotn; + + btScalar positionalError = 0.f; + positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep; + btScalar velocityError = restitution - rel_vel;// * damping; + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + } + + + /////setup the friction constraints - - if (colObj0->getIslandTag() >= 0) + + if (1) + { + solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); + if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) { - if (colObj0->getCompanionId() >= 0) + cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; + btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); + if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) { - //body has already been converted - solverBodyIdA = colObj0->getCompanionId(); + cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); + cp.m_lateralFrictionDir2.normalize();//?? + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + } + cp.m_lateralFrictionInitialized = true; } else { - solverBodyIdA = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,colObj0); - colObj0->setCompanionId(solverBodyIdA); + //re-calculate friction direction every frame, todo: check if this is really needed + btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); + + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + } + cp.m_lateralFrictionInitialized = true; } + } else { - //create a static body - solverBodyIdA = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,colObj0); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } - if (colObj1->getIslandTag() >= 0) + if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) { - if (colObj1->getCompanionId() >= 0) { - solverBodyIdB = colObj1->getCompanionId(); - } else + btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; + if (rb0) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); + if (rb1) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse); + } else + { + frictionConstraint1.m_appliedImpulse = 0.f; + } + } + + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - solverBodyIdB = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,colObj1); - colObj1->setCompanionId(solverBodyIdB); + btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; + if (rb0) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); + if (rb1) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse); + } else + { + frictionConstraint2.m_appliedImpulse = 0.f; + } } } else { - //create a static body - solverBodyIdB = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,colObj1); + btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; + frictionConstraint1.m_appliedImpulse = 0.f; + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; + frictionConstraint2.m_appliedImpulse = 0.f; + } } } + } - btVector3 rel_pos1; - btVector3 rel_pos2; - btScalar relaxation; - for (int j=0;j<manifold->getNumContacts();j++) - { - - btManifoldPoint& cp = manifold->getContactPoint(j); - - if (cp.getDistance() <= btScalar(0.)) - { - - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); + } + } +} - rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); - rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - - relaxation = 1.f; - btScalar rel_vel; - btVector3 vel; +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +{ + BT_PROFILE("solveGroupCacheFriendlySetup"); + (void)stackAlloc; + (void)debugDrawer; - int frictionIndex = m_tmpSolverConstraintPool.size(); - { - btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand(); - btRigidBody* rb0 = btRigidBody::upcast(colObj0); - btRigidBody* rb1 = btRigidBody::upcast(colObj1); + if (!(numConstraints + numManifolds)) + { + // printf("empty\n"); + return 0.f; + } - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D; + if (1) + { + int j; + for (j=0;j<numConstraints;j++) + { + btTypedConstraint* constraint = constraints[j]; + constraint->buildJacobian(); + } + } - solverConstraint.m_originalContactPoint = &cp; + btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&fixedBody,0); - btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0); - btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0); - { -#ifdef COMPUTE_IMPULSE_DENOM - btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); - btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); -#else - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - if (rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); - } - if (rb1) - { - vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); - } -#endif //COMPUTE_IMPULSE_DENOM - - btScalar denom = relaxation/(denom0+denom1); - solverConstraint.m_jacDiagABInv = denom; - } + //btRigidBody* rb0=0,*rb1=0; - solverConstraint.m_contactNormal = cp.m_normalWorldOnB; - solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB); + //if (1) + { + { + int totalNumRows = 0; + int i; + //calculate the total number of contraint rows + for (i=0;i<numConstraints;i++) + { - btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); - btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - - vel = vel1 - vel2; - - rel_vel = cp.m_normalWorldOnB.dot(vel); - - solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,btScalar(0.)); - //solverConstraint.m_penetration = cp.getDistance(); - - solverConstraint.m_friction = cp.m_combinedFriction; - solverConstraint.m_restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); - if (solverConstraint.m_restitution <= btScalar(0.)) - { - solverConstraint.m_restitution = 0.f; - }; + btTypedConstraint::btConstraintInfo1 info1; + constraints[i]->getInfo1(&info1); + totalNumRows += info1.m_numConstraintRows; + } + m_tmpSolverNonContactConstraintPool.resize(totalNumRows); - - btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep; + btTypedConstraint::btConstraintInfo1 info1; + info1.m_numConstraintRows = 0; - - if (solverConstraint.m_restitution > penVel) - { - solverConstraint.m_penetration = btScalar(0.); - } - - - - ///warm starting (or zero if disabled) - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - if (rb0) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - if (rb1) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse); - } else - { - solverConstraint.m_appliedImpulse = 0.f; - } + ///setup the btSolverConstraints + int currentRow = 0; - solverConstraint.m_appliedPushImpulse = 0.f; - - solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size(); - if (!cp.m_lateralFrictionInitialized) - { - cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; - - //scale anisotropic friction - - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); - - btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); - - - if (lat_rel_vel > SIMD_EPSILON)//0.0f) - { - cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); - cp.m_lateralFrictionDir2.normalize(); - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); - - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - } else - { - //re-calculate friction direction every frame, todo: check if this is really needed - btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - } - cp.m_lateralFrictionInitialized = true; - - } else - { - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - } + for (i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows) + { + constraints[i]->getInfo1(&info1); + if (info1.m_numConstraintRows) + { + btAssert(currentRow<totalNumRows); - { - btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex]; - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; - if (rb0) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); - if (rb1) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse); - } else - { - frictionConstraint1.m_appliedImpulse = 0.f; - } - } - { - btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; - if (rb0) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); - if (rb1) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse); - } else - { - frictionConstraint2.m_appliedImpulse = 0.f; - } - } + btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; + btTypedConstraint* constraint = constraints[i]; + + + + btRigidBody& rbA = constraint->getRigidBodyA(); + btRigidBody& rbB = constraint->getRigidBodyB(); + + int solverBodyIdA = getOrInitSolverBody(rbA); + int solverBodyIdB = getOrInitSolverBody(rbB); + + btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; + + int j; + for ( j=0;j<info1.m_numConstraintRows;j++) + { + memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint)); + currentConstraintRow[j].m_lowerLimit = -FLT_MAX; + currentConstraintRow[j].m_upperLimit = FLT_MAX; + currentConstraintRow[j].m_appliedImpulse = 0.f; + currentConstraintRow[j].m_appliedPushImpulse = 0.f; + currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA; + currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB; + } + + bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); + bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); + bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); + bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); + + + + btTypedConstraint::btConstraintInfo2 info2; + info2.fps = 1.f/infoGlobal.m_timeStep; + info2.erp = infoGlobal.m_erp; + info2.m_J1linearAxis = currentConstraintRow->m_contactNormal; + info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; + info2.m_J2linearAxis = 0; + info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; + info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this + ///the size of btSolverConstraint needs be a multiple of btScalar + btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint)); + info2.m_constraintError = ¤tConstraintRow->m_rhs; + info2.cfm = ¤tConstraintRow->m_cfm; + info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; + info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; + constraints[i]->getInfo2(&info2); + + ///finalize the constraint setup + for ( j=0;j<info1.m_numConstraintRows;j++) + { + btSolverConstraint& solverConstraint = currentConstraintRow[j]; + + { + const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; + solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor(); + } + { + const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; + solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor(); } + { + btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass(); + btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal; + btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal? + btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal; + btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal); + sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); + sum += iMJlB.dot(solverConstraint.m_contactNormal); + sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); + + solverConstraint.m_jacDiagABInv = btScalar(1.)/sum; + } + + + ///fix rhs + ///todo: add force/torque accelerators + { + btScalar rel_vel; + btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()); + btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()); + + rel_vel = vel1Dotn+vel2Dotn; + + btScalar restitution = 0.f; + btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 + btScalar velocityError = restitution - rel_vel;// * damping; + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_appliedImpulse = 0.f; + + } } } } } - } - - btContactSolverInfo info = infoGlobal; - { - int j; - for (j=0;j<numConstraints;j++) { - btTypedConstraint* constraint = constraints[j]; - constraint->buildJacobian(); + int i; + btPersistentManifold* manifold = 0; + btCollisionObject* colObj0=0,*colObj1=0; + + + for (i=0;i<numManifolds;i++) + { + manifold = manifoldPtr[i]; + convertContact(manifold,infoGlobal); + } } } - - - int numConstraintPool = m_tmpSolverConstraintPool.size(); - int numFrictionPool = m_tmpSolverFrictionConstraintPool.size(); + btContactSolverInfo info = infoGlobal; + + - ///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints + int numConstraintPool = m_tmpSolverContactConstraintPool.size(); + int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); + + ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints m_orderTmpConstraintPool.resize(numConstraintPool); m_orderFrictionConstraintPool.resize(numFrictionPool); { @@ -877,8 +821,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } } - - return 0.f; } @@ -886,8 +828,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) { BT_PROFILE("solveGroupCacheFriendlyIterations"); - int numConstraintPool = m_tmpSolverConstraintPool.size(); - int numFrictionPool = m_tmpSolverFrictionConstraintPool.size(); + + int numConstraintPool = m_tmpSolverContactConstraintPool.size(); + int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); //should traverse the contacts random order... int iteration; @@ -915,110 +858,134 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations( } } - for (j=0;j<numConstraints;j++) + if (infoGlobal.m_solverMode & SOLVER_SIMD) { - btTypedConstraint* constraint = constraints[j]; - ///todo: use solver bodies, so we don't need to copy from/to btRigidBody - - if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0)) + ///solve all joint constraints, using SIMD, if available + for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) { - m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity(); + btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); } - if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0)) + + for (j=0;j<numConstraints;j++) { - m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity(); + int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA()); + int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); + btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; + btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; + constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); } - constraint->solveConstraint(infoGlobal.m_timeStep); - - if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0)) + ///solve all contact constraints using SIMD, if available + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + for (j=0;j<numPoolConstraints;j++) { - m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity(); + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + } - if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0)) + ///solve all friction constraints, using SIMD, if available + int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); + for (j=0;j<numFrictionPoolConstraints;j++) { - m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity(); - } + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; + btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - } + if (totalImpulse>btScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + } + } + } else { - int numPoolConstraints = m_tmpSolverConstraintPool.size(); - for (j=0;j<numPoolConstraints;j++) + + ///solve all joint constraints + for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) { - - const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], - m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal); + btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; + resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); } - } - { - int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size(); - - for (j=0;j<numFrictionPoolConstraints;j++) + for (j=0;j<numConstraints;j++) { - const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; - btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+ - m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse; + int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA()); + int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); + btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; + btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; - resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], - m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal, - totalImpulse); + constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); } - } - - - } - - if (infoGlobal.m_splitImpulse) - { - - for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) - { + ///solve all contact constraints + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + for (j=0;j<numPoolConstraints;j++) { - int numPoolConstraints = m_tmpSolverConstraintPool.size(); - int j; - for (j=0;j<numPoolConstraints;j++) + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + } + ///solve all friction constraints + int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); + for (j=0;j<numFrictionPoolConstraints;j++) + { + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; + btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + + if (totalImpulse>btScalar(0)) { - const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]]; + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], - m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal); + resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); } } } - } - } + } + } return 0.f; } -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) + +/// btSequentialImpulseConstraintSolver Sequentially applies impulses +btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/) { + + + + BT_PROFILE("solveGroup"); + //we only implement SOLVER_CACHE_FRIENDLY now + //you need to provide at least some bodies + btAssert(bodies); + btAssert(numBodies); + int i; solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); - int numPoolConstraints = m_tmpSolverConstraintPool.size(); + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); int j; + for (j=0;j<numPoolConstraints;j++) { - - const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j]; + + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j]; btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint; btAssert(pt); pt->m_appliedImpulse = solveManifold.m_appliedImpulse; - pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; + if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) + { + pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; + } //do a callback here? - } if (infoGlobal.m_splitImpulse) @@ -1030,418 +997,26 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio } else { for ( i=0;i<m_tmpSolverBodyPool.size();i++) - { - m_tmpSolverBodyPool[i].writebackVelocity(); - } - } - -// printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size()); - -/* - printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size()); - printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size()); - printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size()); - - - printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity()); - printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity()); - printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity()); -*/ - - m_tmpSolverBodyPool.resize(0); - m_tmpSolverConstraintPool.resize(0); - m_tmpSolverFrictionConstraintPool.resize(0); - - - return 0.f; -} - -/// btSequentialImpulseConstraintSolver Sequentially applies impulses -btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/) -{ - BT_PROFILE("solveGroup"); - if (infoGlobal.m_solverMode & SOLVER_CACHE_FRIENDLY) - { - //you need to provide at least some bodies - //btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY - btAssert(bodies); - btAssert(numBodies); - return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); - } - - - - btContactSolverInfo info = infoGlobal; - - int numiter = infoGlobal.m_numIterations; - - int totalPoints = 0; - - - { - short j; - for (j=0;j<numManifolds;j++) - { - btPersistentManifold* manifold = manifoldPtr[j]; - prepareConstraints(manifold,info,debugDrawer); - - for (short p=0;p<manifoldPtr[j]->getNumContacts();p++) - { - gOrder[totalPoints].m_manifoldIndex = j; - gOrder[totalPoints].m_pointIndex = p; - totalPoints++; - } - } - } - - { - int j; - for (j=0;j<numConstraints;j++) - { - btTypedConstraint* constraint = constraints[j]; - constraint->buildJacobian(); - } - } - - - //should traverse the contacts random order... - int iteration; - - { - for ( iteration = 0;iteration<numiter;iteration++) { - int j; - if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) - { - if ((iteration & 7) == 0) { - for (j=0; j<totalPoints; ++j) { - btOrderIndex tmp = gOrder[j]; - int swapi = btRandInt2(j+1); - gOrder[j] = gOrder[swapi]; - gOrder[swapi] = tmp; - } - } - } - - for (j=0;j<numConstraints;j++) - { - btTypedConstraint* constraint = constraints[j]; - constraint->solveConstraint(info.m_timeStep); - } - - for (j=0;j<totalPoints;j++) - { - btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex]; - solve( (btRigidBody*)manifold->getBody0(), - (btRigidBody*)manifold->getBody1() - ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); - } - - for (j=0;j<totalPoints;j++) - { - btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex]; - solveFriction((btRigidBody*)manifold->getBody0(), - (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); - } - + m_tmpSolverBodyPool[i].writebackVelocity(); } } - - - - - return btScalar(0.); -} - - - - - - - -void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer) -{ - - (void)debugDrawer; - - btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0(); - btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1(); - - - //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop - { -#ifdef FORCE_REFESH_CONTACT_MANIFOLDS - manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform()); -#endif //FORCE_REFESH_CONTACT_MANIFOLDS - int numpoints = manifoldPtr->getNumContacts(); - - gTotalContactPoints += numpoints; - - - for (int i=0;i<numpoints ;i++) - { - btManifoldPoint& cp = manifoldPtr->getContactPoint(i); - if (cp.getDistance() <= btScalar(0.)) - { - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); - - btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); - btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition(); - - - //this jacobian entry is re-used for all iterations - btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(), - body1->getCenterOfMassTransform().getBasis().transpose(), - rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(), - body1->getInvInertiaDiagLocal(),body1->getInvMass()); - - - btScalar jacDiagAB = jac.getDiagonal(); - - btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; - if (cpd) - { - //might be invalid - cpd->m_persistentLifeTime++; - if (cpd->m_persistentLifeTime != cp.getLifeTime()) - { - //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); - new (cpd) btConstraintPersistentData; - cpd->m_persistentLifeTime = cp.getLifeTime(); - - } else - { - //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); - - } - } else - { - - //todo: should this be in a pool? - void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16); - cpd = new (mem)btConstraintPersistentData; - assert(cpd); - - totalCpd ++; - //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); - cp.m_userPersistentData = cpd; - cpd->m_persistentLifeTime = cp.getLifeTime(); - //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime()); - - } - assert(cpd); - - cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB; - - //Dependent on Rigidbody A and B types, fetch the contact/friction response func - //perhaps do a similar thing for friction/restutution combiner funcs... - - cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType]; - cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType]; - - btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - btScalar rel_vel; - rel_vel = cp.m_normalWorldOnB.dot(vel); - - btScalar combinedRestitution = cp.m_combinedRestitution; - - cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations); - cpd->m_friction = cp.m_combinedFriction; - cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution); - if (cpd->m_restitution <= btScalar(0.)) - { - cpd->m_restitution = btScalar(0.0); - - }; - - //restitution and penetration work in same direction so - //rel_vel - - btScalar penVel = -cpd->m_penetration/info.m_timeStep; - - if (cpd->m_restitution > penVel) - { - cpd->m_penetration = btScalar(0.); - } - - - btScalar relaxation = info.m_damping; - if (info.m_solverMode & SOLVER_USE_WARMSTARTING) - { - cpd->m_appliedImpulse *= relaxation; - } else - { - cpd->m_appliedImpulse =btScalar(0.); - } - - //for friction - cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse; - - //re-calculate friction direction every frame, todo: check if this is really needed - btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1); - - -#define NO_FRICTION_WARMSTART 1 - - #ifdef NO_FRICTION_WARMSTART - cpd->m_accumulatedTangentImpulse0 = btScalar(0.); - cpd->m_accumulatedTangentImpulse1 = btScalar(0.); - #endif //NO_FRICTION_WARMSTART - btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0); - btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0); - btScalar denom = relaxation/(denom0+denom1); - cpd->m_jacDiagABInvTangent0 = denom; - - denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1); - denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1); - denom = relaxation/(denom0+denom1); - cpd->m_jacDiagABInvTangent1 = denom; - btVector3 totalImpulse = - #ifndef NO_FRICTION_WARMSTART - cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+ - cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+ - #endif //NO_FRICTION_WARMSTART - cp.m_normalWorldOnB*cpd->m_appliedImpulse; - - - - /// - { - btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); - cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0; - btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); - cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1; - } - { - btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0); - cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0; - } - { - btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1); - cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1; - } - { - btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0); - cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0; - } - { - btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1); - cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1; - } - - /// - - - - //apply previous frames impulse on both bodies - body0->applyImpulse(totalImpulse, rel_pos1); - body1->applyImpulse(-totalImpulse, rel_pos2); - } - - } - } -} - - -btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) -{ - btScalar maxImpulse = btScalar(0.); - - { - - - { - if (cp.getDistance() <= btScalar(0.)) - { - - - - { - - //btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; - btScalar impulse = resolveSingleCollisionCombined( - *body0,*body1, - cp, - info); - - if (maxImpulse < impulse) - maxImpulse = impulse; - - } - } - } - } - return maxImpulse; -} - - - -btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) -{ - - btScalar maxImpulse = btScalar(0.); - - { - - - { - if (cp.getDistance() <= btScalar(0.)) - { - - - - { - - btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; - btScalar impulse = cpd->m_contactSolverFunc( - *body0,*body1, - cp, - info); - - if (maxImpulse < impulse) - maxImpulse = impulse; + m_tmpSolverBodyPool.resize(0); + m_tmpSolverContactConstraintPool.resize(0); + m_tmpSolverNonContactConstraintPool.resize(0); + m_tmpSolverContactFrictionConstraintPool.resize(0); - } - } - } - } - return maxImpulse; + return 0.f; } -btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) -{ - (void)debugDrawer; - (void)iter; - { - - { - - if (cp.getDistance() <= btScalar(0.)) - { - btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; - cpd->m_frictionSolverFunc( - *body0,*body1, - cp, - info); - - } - } - - - } - return btScalar(0.); -} void btSequentialImpulseConstraintSolver::reset() diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 7143bc41991..467e37bb91f 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -23,67 +23,62 @@ class btIDebugDraw; #include "btSolverConstraint.h" -/// btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses -/// The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com -/// Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP) -/// Applies impulses for combined restitution and penetration recovery and to simulate friction + +///The btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses +///The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com +///Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP) +///Applies impulses for combined restitution and penetration recovery and to simulate friction class btSequentialImpulseConstraintSolver : public btConstraintSolver { +protected: btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool; - btAlignedObjectArray<btSolverConstraint> m_tmpSolverConstraintPool; - btAlignedObjectArray<btSolverConstraint> m_tmpSolverFrictionConstraintPool; + btConstraintArray m_tmpSolverContactConstraintPool; + btConstraintArray m_tmpSolverNonContactConstraintPool; + btConstraintArray m_tmpSolverContactFrictionConstraintPool; btAlignedObjectArray<int> m_orderTmpConstraintPool; btAlignedObjectArray<int> m_orderFrictionConstraintPool; - -protected: - btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); - btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); - void prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer); - void addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation); - - ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; - ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; - + btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation); ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction unsigned long m_btSeed2; -public: + void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject); + btScalar restitutionCurve(btScalar rel_vel, btScalar restitution); - - btSequentialImpulseConstraintSolver(); + void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); - ///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody - ///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType - void setContactSolverFunc(ContactSolverFunc func,int type0,int type1) - { - m_contactDispatch[type0][type1] = func; - } + void resolveSplitPenetrationImpulseCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + const btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo); + + //internal method + int getOrInitSolverBody(btCollisionObject& body); + + void resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); + + void resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); - ///Advanced: Override the default friction solving function for contacts, for certain types of rigidbody - ///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType - void SetFrictionSolverFunc(ContactSolverFunc func,int type0,int type1) - { - m_frictionDispatch[type0][type1] = func; - } + void resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); + + void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); + +public: - virtual ~btSequentialImpulseConstraintSolver(); - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher); + btSequentialImpulseConstraintSolver(); + virtual ~btSequentialImpulseConstraintSolver(); - virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); - btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher); + btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); - + btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); ///clear internal cached data and reset random seed virtual void reset(); - - btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); - - unsigned long btRand2(); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index 4128f504bf1..50d06960379 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -68,7 +68,9 @@ void btSliderConstraint::initParams() btSliderConstraint::btSliderConstraint() :btTypedConstraint(SLIDER_CONSTRAINT_TYPE), - m_useLinearReferenceFrameA(true) + m_useLinearReferenceFrameA(true), + m_useSolveConstraintObsolete(false) +// m_useSolveConstraintObsolete(true) { initParams(); } // btSliderConstraint::btSliderConstraint() @@ -79,7 +81,9 @@ btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB) , m_frameInA(frameInA) , m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA) + m_useLinearReferenceFrameA(useLinearReferenceFrameA), + m_useSolveConstraintObsolete(false) +// m_useSolveConstraintObsolete(true) { initParams(); } // btSliderConstraint::btSliderConstraint() @@ -88,6 +92,10 @@ btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const void btSliderConstraint::buildJacobian() { + if (!m_useSolveConstraintObsolete) + { + return; + } if(m_useLinearReferenceFrameA) { buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB); @@ -155,27 +163,372 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co //----------------------------------------------------------------------------- -void btSliderConstraint::solveConstraint(btScalar timeStep) +void btSliderConstraint::getInfo1(btConstraintInfo1* info) { - m_timeStep = timeStep; - if(m_useLinearReferenceFrameA) + if (m_useSolveConstraintObsolete) { - solveConstraintInt(m_rbA, m_rbB); + info->m_numConstraintRows = 0; + info->nub = 0; } else { - solveConstraintInt(m_rbB, m_rbA); + info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular + info->nub = 2; + //prepare constraint + calculateTransforms(); + testLinLimits(); + if(getSolveLinLimit() || getPoweredLinMotor()) + { + info->m_numConstraintRows++; // limit 3rd linear as well + info->nub--; + } + testAngLimits(); + if(getSolveAngLimit() || getPoweredAngMotor()) + { + info->m_numConstraintRows++; // limit 3rd angular as well + info->nub--; + } + } +} // btSliderConstraint::getInfo1() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::getInfo2(btConstraintInfo2* info) +{ + btAssert(!m_useSolveConstraintObsolete); + int i, s = info->rowskip; + const btTransform& trA = getCalculatedTransformA(); + const btTransform& trB = getCalculatedTransformB(); + btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f); + // make rotations around Y and Z equal + // the slider axis should be the only unconstrained + // rotational axis, the angular velocity of the two bodies perpendicular to + // the slider axis should be equal. thus the constraint equations are + // p*w1 - p*w2 = 0 + // q*w1 - q*w2 = 0 + // where p and q are unit vectors normal to the slider axis, and w1 and w2 + // are the angular velocity vectors of the two bodies. + // get slider axis (X) + btVector3 ax1 = trA.getBasis().getColumn(0); + // get 2 orthos to slider axis (Y, Z) + btVector3 p = trA.getBasis().getColumn(1); + btVector3 q = trA.getBasis().getColumn(2); + // set the two slider rows + info->m_J1angularAxis[0] = p[0]; + info->m_J1angularAxis[1] = p[1]; + info->m_J1angularAxis[2] = p[2]; + info->m_J1angularAxis[s+0] = q[0]; + info->m_J1angularAxis[s+1] = q[1]; + info->m_J1angularAxis[s+2] = q[2]; + + info->m_J2angularAxis[0] = -p[0]; + info->m_J2angularAxis[1] = -p[1]; + info->m_J2angularAxis[2] = -p[2]; + info->m_J2angularAxis[s+0] = -q[0]; + info->m_J2angularAxis[s+1] = -q[1]; + info->m_J2angularAxis[s+2] = -q[2]; + // compute the right hand side of the constraint equation. set relative + // body velocities along p and q to bring the slider back into alignment. + // if ax1,ax2 are the unit length slider axes as computed from body1 and + // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). + // if "theta" is the angle between ax1 and ax2, we need an angular velocity + // along u to cover angle erp*theta in one step : + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| + // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) + // ...as ax1 and ax2 are unit length. if theta is smallish, + // theta ~= sin(theta), so + // angular_velocity = (erp*fps) * (ax1 x ax2) + // ax1 x ax2 is in the plane space of ax1, so we project the angular + // velocity to p and q to find the right hand side. + btScalar k = info->fps * info->erp * getSoftnessOrthoAng(); + btVector3 ax2 = trB.getBasis().getColumn(0); + btVector3 u = ax1.cross(ax2); + info->m_constraintError[0] = k * u.dot(p); + info->m_constraintError[s] = k * u.dot(q); + // pull out pos and R for both bodies. also get the connection + // vector c = pos2-pos1. + // next two rows. we want: vel2 = vel1 + w1 x c ... but this would + // result in three equations, so we project along the planespace vectors + // so that sliding along the slider axis is disregarded. for symmetry we + // also consider rotation around center of mass of two bodies (factA and factB). + btTransform bodyA_trans = m_rbA.getCenterOfMassTransform(); + btTransform bodyB_trans = m_rbB.getCenterOfMassTransform(); + int s2 = 2 * s, s3 = 3 * s; + btVector3 c; + btScalar miA = m_rbA.getInvMass(); + btScalar miB = m_rbB.getInvMass(); + btScalar miS = miA + miB; + btScalar factA, factB; + if(miS > btScalar(0.f)) + { + factA = miB / miS; + } + else + { + factA = btScalar(0.5f); + } + if(factA > 0.99f) factA = 0.99f; + if(factA < 0.01f) factA = 0.01f; + factB = btScalar(1.0f) - factA; + c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin(); + btVector3 tmp = c.cross(p); + for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i]; + tmp = c.cross(q); + for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i]; + + for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; + for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; + // compute two elements of right hand side. we want to align the offset + // point (in body 2's frame) with the center of body 1. + btVector3 ofs; // offset point in global coordinates + ofs = trB.getOrigin() - trA.getOrigin(); + k = info->fps * info->erp * getSoftnessOrthoLin(); + info->m_constraintError[s2] = k * p.dot(ofs); + info->m_constraintError[s3] = k * q.dot(ofs); + int nrow = 3; // last filled row + int srow; + // check linear limits linear + btScalar limit_err = btScalar(0.0); + int limit = 0; + if(getSolveLinLimit()) + { + limit_err = getLinDepth() * signFact; + limit = (limit_err > btScalar(0.0)) ? 2 : 1; + } + int powered = 0; + if(getPoweredLinMotor()) + { + powered = 1; + } + // if the slider has joint limits or motor, add in the extra row + if (limit || powered) + { + nrow++; + srow = nrow * info->rowskip; + info->m_J1linearAxis[srow+0] = ax1[0]; + info->m_J1linearAxis[srow+1] = ax1[1]; + info->m_J1linearAxis[srow+2] = ax1[2]; + // linear torque decoupling step: + // + // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies + // do not create a torque couple. in other words, the points that the + // constraint force is applied at must lie along the same ax1 axis. + // a torque couple will result in limited slider-jointed free + // bodies from gaining angular momentum. + // the solution used here is to apply the constraint forces at the center of mass of the two bodies + btVector3 ltd; // Linear Torque Decoupling vector (a torque) +// c = btScalar(0.5) * c; + ltd = c.cross(ax1); + info->m_J1angularAxis[srow+0] = factA*ltd[0]; + info->m_J1angularAxis[srow+1] = factA*ltd[1]; + info->m_J1angularAxis[srow+2] = factA*ltd[2]; + info->m_J2angularAxis[srow+0] = factB*ltd[0]; + info->m_J2angularAxis[srow+1] = factB*ltd[1]; + info->m_J2angularAxis[srow+2] = factB*ltd[2]; + // right-hand part + btScalar lostop = getLowerLinLimit(); + btScalar histop = getUpperLinLimit(); + if(limit && (lostop == histop)) + { // the joint motor is ineffective + powered = 0; + } + info->m_constraintError[srow] = 0.; + info->m_lowerLimit[srow] = 0.; + info->m_upperLimit[srow] = 0.; + if(powered) + { + info->cfm[nrow] = btScalar(0.0); + btScalar tag_vel = getTargetLinMotorVelocity(); + btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp); +// info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity(); + info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity(); + info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps; + info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps; + } + if(limit) + { + k = info->fps * info->erp; + info->m_constraintError[srow] += k * limit_err; + info->cfm[srow] = btScalar(0.0); // stop_cfm; + if(lostop == histop) + { // limited low and high simultaneously + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else if(limit == 1) + { // low limit + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = 0; + } + else + { // high limit + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that) + btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin()); + if(bounce > btScalar(0.0)) + { + btScalar vel = m_rbA.getLinearVelocity().dot(ax1); + vel -= m_rbB.getLinearVelocity().dot(ax1); + vel *= signFact; + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if(limit == 1) + { // low limit + if(vel < 0) + { + btScalar newc = -bounce * vel; + if (newc > info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + else + { // high limit - all those computations are reversed + if(vel > 0) + { + btScalar newc = -bounce * vel; + if(newc < info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + } + info->m_constraintError[srow] *= getSoftnessLimLin(); + } // if(limit) + } // if linear limit + // check angular limits + limit_err = btScalar(0.0); + limit = 0; + if(getSolveAngLimit()) + { + limit_err = getAngDepth(); + limit = (limit_err > btScalar(0.0)) ? 1 : 2; + } + // if the slider has joint limits, add in the extra row + powered = 0; + if(getPoweredAngMotor()) + { + powered = 1; + } + if(limit || powered) + { + nrow++; + srow = nrow * info->rowskip; + info->m_J1angularAxis[srow+0] = ax1[0]; + info->m_J1angularAxis[srow+1] = ax1[1]; + info->m_J1angularAxis[srow+2] = ax1[2]; + + info->m_J2angularAxis[srow+0] = -ax1[0]; + info->m_J2angularAxis[srow+1] = -ax1[1]; + info->m_J2angularAxis[srow+2] = -ax1[2]; + + btScalar lostop = getLowerAngLimit(); + btScalar histop = getUpperAngLimit(); + if(limit && (lostop == histop)) + { // the joint motor is ineffective + powered = 0; + } + if(powered) + { + info->cfm[srow] = btScalar(0.0); + btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp); + info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity(); + info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps; + info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps; + } + if(limit) + { + k = info->fps * info->erp; + info->m_constraintError[srow] += k * limit_err; + info->cfm[srow] = btScalar(0.0); // stop_cfm; + if(lostop == histop) + { + // limited low and high simultaneously + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else if(limit == 1) + { // low limit + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else + { // high limit + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = 0; + } + // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) + btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng()); + if(bounce > btScalar(0.0)) + { + btScalar vel = m_rbA.getAngularVelocity().dot(ax1); + vel -= m_rbB.getAngularVelocity().dot(ax1); + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if(limit == 1) + { // low limit + if(vel < 0) + { + btScalar newc = -bounce * vel; + if(newc > info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + else + { // high limit - all those computations are reversed + if(vel > 0) + { + btScalar newc = -bounce * vel; + if(newc < info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + } + info->m_constraintError[srow] *= getSoftnessLimAng(); + } // if(limit) + } // if angular limit or powered +} // btSliderConstraint::getInfo2() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) +{ + if (m_useSolveConstraintObsolete) + { + m_timeStep = timeStep; + if(m_useLinearReferenceFrameA) + { + solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB); + } + else + { + solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA); + } } } // btSliderConstraint::solveConstraint() //----------------------------------------------------------------------------- -void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) +void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB) { int i; // linear - btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA); - btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB); + btVector3 velA; + bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA); + btVector3 velB; + bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB); btVector3 vel = velA - velB; for(i = 0; i < 3; i++) { @@ -190,8 +543,18 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) // calcutate and apply impulse btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i]; btVector3 impulse_vector = normal * normalImpulse; - rbA.applyImpulse( impulse_vector, m_relPosA); - rbB.applyImpulse(-impulse_vector, m_relPosB); + + //rbA.applyImpulse( impulse_vector, m_relPosA); + //rbB.applyImpulse(-impulse_vector, m_relPosB); + { + btVector3 ftorqueAxis1 = m_relPosA.cross(normal); + btVector3 ftorqueAxis2 = m_relPosB.cross(normal); + bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); + bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); + } + + + if(m_poweredLinMotor && (!i)) { // apply linear motor if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce) @@ -217,8 +580,18 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) m_accumulatedLinMotorImpulse = new_acc; // apply clamped impulse impulse_vector = normal * normalImpulse; - rbA.applyImpulse( impulse_vector, m_relPosA); - rbB.applyImpulse(-impulse_vector, m_relPosB); + //rbA.applyImpulse( impulse_vector, m_relPosA); + //rbB.applyImpulse(-impulse_vector, m_relPosB); + + { + btVector3 ftorqueAxis1 = m_relPosA.cross(normal); + btVector3 ftorqueAxis2 = m_relPosB.cross(normal); + bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); + bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); + } + + + } } } @@ -227,8 +600,10 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0); - const btVector3& angVelA = rbA.getAngularVelocity(); - const btVector3& angVelB = rbB.getAngularVelocity(); + btVector3 angVelA; + bodyA.getAngularVelocity(angVelA); + btVector3 angVelB; + bodyB.getAngularVelocity(angVelB); btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); @@ -238,24 +613,38 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) btVector3 velrelOrthog = angAorthog-angBorthog; //solve orthogonal angular velocity correction btScalar len = velrelOrthog.length(); + btScalar orthorImpulseMag = 0.f; + if (len > btScalar(0.00001)) { btVector3 normal = velrelOrthog.normalized(); btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal); - velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; + //velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; + orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; } //solve angular positional correction btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep); + btVector3 angularAxis = angularError; + btScalar angularImpulseMag = 0; + btScalar len2 = angularError.length(); if (len2>btScalar(0.00001)) { btVector3 normal2 = angularError.normalized(); btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2); - angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; + angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; + angularError *= angularImpulseMag; } // apply impulse - rbA.applyTorqueImpulse(-velrelOrthog+angularError); - rbB.applyTorqueImpulse(velrelOrthog-angularError); + //rbA.applyTorqueImpulse(-velrelOrthog+angularError); + //rbB.applyTorqueImpulse(velrelOrthog-angularError); + + bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag); + bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag); + bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag); + bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag); + + btScalar impulseMag; //solve angular limits if(m_solveAngLim) @@ -269,8 +658,14 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) impulseMag *= m_kAngle * m_softnessDirAng; } btVector3 impulse = axisA * impulseMag; - rbA.applyTorqueImpulse(impulse); - rbB.applyTorqueImpulse(-impulse); + //rbA.applyTorqueImpulse(impulse); + //rbB.applyTorqueImpulse(-impulse); + + bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag); + bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag); + + + //apply angular motor if(m_poweredAngMotor) { @@ -301,8 +696,11 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) m_accumulatedAngMotorImpulse = new_acc; // apply clamped impulse btVector3 motorImp = angImpulse * axisA; - m_rbA.applyTorqueImpulse(motorImp); - m_rbB.applyTorqueImpulse(-motorImp); + //rbA.applyTorqueImpulse(motorImp); + //rbB.applyTorqueImpulse(-motorImp); + + bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse); + bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse); } } } // btSliderConstraint::solveConstraint() @@ -312,7 +710,7 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) //----------------------------------------------------------------------------- void btSliderConstraint::calculateTransforms(void){ - if(m_useLinearReferenceFrameA) + if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete)) { m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA; m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB; @@ -325,7 +723,14 @@ void btSliderConstraint::calculateTransforms(void){ m_realPivotAInW = m_calculatedTransformA.getOrigin(); m_realPivotBInW = m_calculatedTransformB.getOrigin(); m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X - m_delta = m_realPivotBInW - m_realPivotAInW; + if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete) + { + m_delta = m_realPivotBInW - m_realPivotAInW; + } + else + { + m_delta = m_realPivotAInW - m_realPivotBInW; + } m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; btVector3 normalWorld; int i; @@ -367,7 +772,6 @@ void btSliderConstraint::testLinLimits(void) } // btSliderConstraint::testLinLimits() //----------------------------------------------------------------------------- - void btSliderConstraint::testAngLimits(void) { @@ -379,6 +783,7 @@ void btSliderConstraint::testAngLimits(void) const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2); const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1); btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); + m_angPos = rot; if(rot < m_lowerAngLimit) { m_angDepth = rot - m_lowerAngLimit; @@ -391,12 +796,9 @@ void btSliderConstraint::testAngLimits(void) } } } // btSliderConstraint::testAngLimits() - //----------------------------------------------------------------------------- - - btVector3 btSliderConstraint::getAncorInA(void) { btVector3 ancorInA; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index 580dfa1178d..70fbce5d9b2 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -46,6 +46,8 @@ class btRigidBody; class btSliderConstraint : public btTypedConstraint { protected: + ///for backwards compatibility during the transition to 'getInfo/getInfo2' + bool m_useSolveConstraintObsolete; btTransform m_frameInA; btTransform m_frameInB; // use frameA fo define limits, if true @@ -104,6 +106,7 @@ protected: btVector3 m_relPosB; btScalar m_linPos; + btScalar m_angPos; btScalar m_angDepth; btScalar m_kAngle; @@ -126,7 +129,13 @@ public: btSliderConstraint(); // overrides virtual void buildJacobian(); - virtual void solveConstraint(btScalar timeStep); + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); + + // access const btRigidBody& getRigidBodyA() const { return m_rbA; } const btRigidBody& getRigidBodyB() const { return m_rbB; } @@ -194,6 +203,7 @@ public: void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; } btScalar getLinearPos() { return m_linPos; } + // access for ODE solver bool getSolveLinLimit() { return m_solveLinLim; } @@ -202,10 +212,11 @@ public: btScalar getAngDepth() { return m_angDepth; } // internal void buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB); - void solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB); + void solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB); // shared code used by ODE solver void calculateTransforms(void); void testLinLimits(void); + void testLinLimits2(btConstraintInfo2* info); void testAngLimits(void); // access for PE Solver btVector3 getAncorInA(void); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h index b3f0c9d7444..98c9876ae46 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h @@ -23,86 +23,152 @@ class btRigidBody; #include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btTransformUtil.h" +///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision +#ifdef BT_USE_SSE +#define USE_SIMD 1 +#endif // -///btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. + +#ifdef USE_SIMD + +struct btSimdScalar +{ + SIMD_FORCE_INLINE btSimdScalar() + { + + } + + SIMD_FORCE_INLINE btSimdScalar(float fl) + :m_vec128 (_mm_set1_ps(fl)) + { + } + + SIMD_FORCE_INLINE btSimdScalar(__m128 v128) + :m_vec128(v128) + { + } + union + { + __m128 m_vec128; + float m_floats[4]; + int m_ints[4]; + btScalar m_unusedPadding; + }; + SIMD_FORCE_INLINE __m128 get128() + { + return m_vec128; + } + + SIMD_FORCE_INLINE const __m128 get128() const + { + return m_vec128; + } + + SIMD_FORCE_INLINE void set128(__m128 v128) + { + m_vec128 = v128; + } + + SIMD_FORCE_INLINE operator __m128() + { + return m_vec128; + } + SIMD_FORCE_INLINE operator const __m128() const + { + return m_vec128; + } + + SIMD_FORCE_INLINE operator float() const + { + return m_floats[0]; + } + +}; + +///@brief Return the elementwise product of two btSimdScalar +SIMD_FORCE_INLINE btSimdScalar +operator*(const btSimdScalar& v1, const btSimdScalar& v2) +{ + return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128())); +} + +///@brief Return the elementwise product of two btSimdScalar +SIMD_FORCE_INLINE btSimdScalar +operator+(const btSimdScalar& v1, const btSimdScalar& v2) +{ + return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128())); +} + + +#else +#define btSimdScalar btScalar +#endif + +///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. ATTRIBUTE_ALIGNED16 (struct) btSolverBody { BT_DECLARE_ALIGNED_ALLOCATOR(); - - btVector3 m_angularVelocity; - float m_angularFactor; - float m_invMass; - float m_friction; + btVector3 m_deltaLinearVelocity; + btVector3 m_deltaAngularVelocity; + btScalar m_angularFactor; + btScalar m_invMass; + btScalar m_friction; btRigidBody* m_originalBody; - btVector3 m_linearVelocity; - btVector3 m_centerOfMassPosition; - btVector3 m_pushVelocity; - btVector3 m_turnVelocity; - + //btVector3 m_turnVelocity; + - SIMD_FORCE_INLINE void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const + SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const { - velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos); + if (m_originalBody) + velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos); + else + velocity.setValue(0,0,0); } - //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const { - if (m_invMass) - { - m_linearVelocity += linearComponent*impulseMagnitude; - m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); - } + if (m_originalBody) + angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity; + else + angVel.setValue(0,0,0); } - - SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude) { - if (m_invMass) + //if (m_invMass) { - m_pushVelocity += linearComponent*impulseMagnitude; - m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + m_deltaLinearVelocity += linearComponent*impulseMagnitude; + m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); } } +/* + void writebackVelocity() { if (m_invMass) { - m_originalBody->setLinearVelocity(m_linearVelocity); - m_originalBody->setAngularVelocity(m_angularVelocity); + m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity); + m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); //m_originalBody->setCompanionId(-1); } } - + */ - void writebackVelocity(btScalar timeStep) + void writebackVelocity(btScalar timeStep=0) { if (m_invMass) { - m_originalBody->setLinearVelocity(m_linearVelocity); - m_originalBody->setAngularVelocity(m_angularVelocity); - - //correct the position/orientation based on push/turn recovery - btTransform newTransform; - btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform); - m_originalBody->setWorldTransform(newTransform); - + m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity); + m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); //m_originalBody->setCompanionId(-1); } } - - void readVelocity() - { - if (m_invMass) - { - m_linearVelocity = m_originalBody->getLinearVelocity(); - m_angularVelocity = m_originalBody->getAngularVelocity(); - } - } - diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h index 2c71360c5b9..867d62a301d 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h @@ -19,40 +19,64 @@ subject to the following restrictions: class btRigidBody; #include "LinearMath/btVector3.h" #include "LinearMath/btMatrix3x3.h" +#include "btJacobianEntry.h" //#define NO_FRICTION_TANGENTIALS 1 +#include "btSolverBody.h" + ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint { BT_DECLARE_ALIGNED_ALLOCATOR(); - btVector3 m_relpos1CrossNormal; - btVector3 m_contactNormal; - - btVector3 m_relpos2CrossNormal; - btVector3 m_angularComponentA; + btVector3 m_relpos1CrossNormal; + btVector3 m_contactNormal; - btVector3 m_angularComponentB; + btVector3 m_relpos2CrossNormal; + //btVector3 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal - mutable btScalar m_appliedPushImpulse; + btVector3 m_angularComponentA; + btVector3 m_angularComponentB; + + mutable btSimdScalar m_appliedPushImpulse; + mutable btSimdScalar m_appliedImpulse; - mutable btScalar m_appliedImpulse; - int m_solverBodyIdA; - int m_solverBodyIdB; btScalar m_friction; - btScalar m_restitution; btScalar m_jacDiagABInv; - btScalar m_penetration; - + union + { + int m_numConsecutiveRowsPerKernel; + btScalar m_unusedPadding0; + }; + union + { + int m_frictionIndex; + btScalar m_unusedPadding1; + }; + union + { + int m_solverBodyIdA; + btScalar m_unusedPadding2; + }; + union + { + int m_solverBodyIdB; + btScalar m_unusedPadding3; + }; - int m_constraintType; - int m_frictionIndex; - void* m_originalContactPoint; - int m_unusedPadding[1]; + union + { + void* m_originalContactPoint; + btScalar m_unusedPadding4; + }; + btScalar m_rhs; + btScalar m_cfm; + btScalar m_lowerLimit; + btScalar m_upperLimit; enum btSolverConstraintType { @@ -61,9 +85,7 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint }; }; - - - +typedef btAlignedObjectArray<btSolverConstraint> btConstraintArray; #endif //BT_SOLVER_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index 6e8b552dbbc..3fa98ee4c88 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -19,13 +19,16 @@ subject to the following restrictions: static btRigidBody s_fixed(0, 0,0); +#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f) + btTypedConstraint::btTypedConstraint(btTypedConstraintType type) :m_userConstraintType(-1), m_userConstraintId(-1), m_constraintType (type), m_rbA(s_fixed), m_rbB(s_fixed), -m_appliedImpulse(btScalar(0.)) +m_appliedImpulse(btScalar(0.)), +m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) { s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); } @@ -35,7 +38,8 @@ m_userConstraintId(-1), m_constraintType (type), m_rbA(rbA), m_rbB(s_fixed), -m_appliedImpulse(btScalar(0.)) +m_appliedImpulse(btScalar(0.)), +m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) { s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); @@ -48,9 +52,63 @@ m_userConstraintId(-1), m_constraintType (type), m_rbA(rbA), m_rbB(rbB), -m_appliedImpulse(btScalar(0.)) +m_appliedImpulse(btScalar(0.)), +m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) { s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); } + +//----------------------------------------------------------------------------- + +btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact) +{ + if(lowLim > uppLim) + { + return btScalar(1.0f); + } + else if(lowLim == uppLim) + { + return btScalar(0.0f); + } + btScalar lim_fact = btScalar(1.0f); + btScalar delta_max = vel / timeFact; + if(delta_max < btScalar(0.0f)) + { + if((pos >= lowLim) && (pos < (lowLim - delta_max))) + { + lim_fact = (lowLim - pos) / delta_max; + } + else if(pos < lowLim) + { + lim_fact = btScalar(0.0f); + } + else + { + lim_fact = btScalar(1.0f); + } + } + else if(delta_max > btScalar(0.0f)) + { + if((pos <= uppLim) && (pos > (uppLim - delta_max))) + { + lim_fact = (uppLim - pos) / delta_max; + } + else if(pos > uppLim) + { + lim_fact = btScalar(0.0f); + } + else + { + lim_fact = btScalar(1.0f); + } + } + else + { + lim_fact = btScalar(0.0f); + } + return lim_fact; +} // btTypedConstraint::getMotorFactor() + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index c50ec6ec579..78a770231b3 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -18,6 +18,11 @@ subject to the following restrictions: class btRigidBody; #include "LinearMath/btScalar.h" +#include "btSolverConstraint.h" +struct btSolverBody; + + + enum btTypedConstraintType { @@ -48,6 +53,7 @@ protected: btRigidBody& m_rbA; btRigidBody& m_rbB; btScalar m_appliedImpulse; + btScalar m_dbgDrawSize; public: @@ -55,13 +61,55 @@ public: btTypedConstraint(btTypedConstraintType type); virtual ~btTypedConstraint() {}; btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA); - btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB); + struct btConstraintInfo1 { + int m_numConstraintRows,nub; + }; + + struct btConstraintInfo2 { + // integrator parameters: frames per second (1/stepsize), default error + // reduction parameter (0..1). + btScalar fps,erp; + + // for the first and second body, pointers to two (linear and angular) + // n*3 jacobian sub matrices, stored by rows. these matrices will have + // been initialized to 0 on entry. if the second body is zero then the + // J2xx pointers may be 0. + btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis; + + // elements to jump from one row to the next in J's + int rowskip; + + // right hand sides of the equation J*v = c + cfm * lambda. cfm is the + // "constraint force mixing" vector. c is set to zero on entry, cfm is + // set to a constant value (typically very small or zero) value on entry. + btScalar *m_constraintError,*cfm; + + // lo and hi limits for variables (set to -/+ infinity on entry). + btScalar *m_lowerLimit,*m_upperLimit; + + // findex vector for variables. see the LCP solver interface for a + // description of what this does. this is set to -1 on entry. + // note that the returned indexes are relative to the first index of + // the constraint. + int *findex; + }; + + virtual void buildJacobian() = 0; - virtual void solveConstraint(btScalar timeStep) = 0; + virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep) + { + } + virtual void getInfo1 (btConstraintInfo1* info)=0; + + virtual void getInfo2 (btConstraintInfo2* info)=0; + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) = 0; + + btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact); + const btRigidBody& getRigidBodyA() const { return m_rbA; @@ -94,7 +142,7 @@ public: { m_userConstraintId = uid; } - + int getUserConstraintId() const { return m_userConstraintId; @@ -114,7 +162,16 @@ public: { return m_constraintType; } - + + void setDbgDrawSize(btScalar dbgDrawSize) + { + m_dbgDrawSize = dbgDrawSize; + } + btScalar getDbgDrawSize() + { + return m_dbgDrawSize; + } + }; #endif //TYPED_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp index c2fd71d67fe..47addbac45b 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp @@ -38,7 +38,7 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" #include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" @@ -110,16 +110,16 @@ void plDeleteDynamicsWorld(plDynamicsWorldHandle world) void plStepSimulation(plDynamicsWorldHandle world, plReal timeStep) { btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - assert(dynamicsWorld); + btAssert(dynamicsWorld); dynamicsWorld->stepSimulation(timeStep); } void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object) { btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - assert(dynamicsWorld); + btAssert(dynamicsWorld); btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - assert(body); + btAssert(body); dynamicsWorld->addRigidBody(body); } @@ -127,9 +127,9 @@ void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object) void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object) { btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - assert(dynamicsWorld); + btAssert(dynamicsWorld); btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - assert(body); + btAssert(body); dynamicsWorld->removeRigidBody(body); } @@ -142,7 +142,7 @@ plRigidBodyHandle plCreateRigidBody( void* user_data, float mass, plCollisionSh trans.setIdentity(); btVector3 localInertia(0,0,0); btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape); - assert(shape); + btAssert(shape); if (mass) { shape->calculateLocalInertia(mass,localInertia); @@ -158,7 +158,7 @@ plRigidBodyHandle plCreateRigidBody( void* user_data, float mass, plCollisionSh void plDeleteRigidBody(plRigidBodyHandle cbody) { btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody); - assert(body); + btAssert(body); btAlignedFree( body); } @@ -255,20 +255,20 @@ void plAddVertex(plCollisionShapeHandle cshape, plReal x,plReal y,plReal z) (void)colShape; btAssert(colShape->getShapeType()==CONVEX_HULL_SHAPE_PROXYTYPE); btConvexHullShape* convexHullShape = reinterpret_cast<btConvexHullShape*>( cshape); - convexHullShape->addPoint(btPoint3(x,y,z)); + convexHullShape->addPoint(btVector3(x,y,z)); } void plDeleteShape(plCollisionShapeHandle cshape) { btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape); - assert(shape); + btAssert(shape); btAlignedFree(shape); } void plSetScaling(plCollisionShapeHandle cshape, plVector3 cscaling) { btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape); - assert(shape); + btAssert(shape); btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]); shape->setLocalScaling(scaling); } diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 745694e2f70..ba48f404929 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -29,6 +29,11 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" +#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h" +#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" +#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" //for debug rendering #include "BulletCollision/CollisionShapes/btBoxShape.h" @@ -51,6 +56,9 @@ subject to the following restrictions: #include "BulletDynamics/Vehicle/btRaycastVehicle.h" #include "BulletDynamics/Vehicle/btVehicleRaycaster.h" #include "BulletDynamics/Vehicle/btWheelInfo.h" +//character +#include "BulletDynamics/Character/btCharacterControllerInterface.h" + #include "LinearMath/btIDebugDraw.h" #include "LinearMath/btQuickprof.h" #include "LinearMath/btMotionState.h" @@ -110,7 +118,6 @@ void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) btRigidBody* body = btRigidBody::upcast(colObj); if (body) { - btTransform predictedTrans; if (body->getActivationState() != ISLAND_SLEEPING) { if (body->isKinematicObject()) @@ -125,6 +132,7 @@ void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) void btDiscreteDynamicsWorld::debugDrawWorld() { + BT_PROFILE("debugDrawWorld"); if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) { @@ -144,13 +152,30 @@ void btDiscreteDynamicsWorld::debugDrawWorld() } } } + bool drawConstraints = false; + if (getDebugDrawer()) + { + int mode = getDebugDrawer()->getDebugMode(); + if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) + { + drawConstraints = true; + } + } + if(drawConstraints) + { + for(int i = getNumConstraints()-1; i>=0 ;i--) + { + btTypedConstraint* constraint = getConstraint(i); + debugDrawConstraint(constraint); + } + } + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)) { int i; - //todo: iterate over awake simulation islands! for ( i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; @@ -179,7 +204,7 @@ void btDiscreteDynamicsWorld::debugDrawWorld() } if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) { - btPoint3 minAabb,maxAabb; + btVector3 minAabb,maxAabb; btVector3 colorvec(1,0,0); colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); @@ -220,7 +245,7 @@ void btDiscreteDynamicsWorld::debugDrawWorld() void btDiscreteDynamicsWorld::clearForces() { - //todo: iterate over awake simulation islands! + ///@todo: iterate over awake simulation islands! for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; @@ -236,7 +261,7 @@ void btDiscreteDynamicsWorld::clearForces() ///apply gravity, call this once per timestep void btDiscreteDynamicsWorld::applyGravity() { - //todo: iterate over awake simulation islands! + ///@todo: iterate over awake simulation islands! for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; @@ -250,9 +275,29 @@ void btDiscreteDynamicsWorld::applyGravity() } +void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) +{ + btAssert(body); + + if (body->getMotionState() && !body->isStaticOrKinematicObject()) + { + //we need to call the update at least once, even for sleeping objects + //otherwise the 'graphics' transform never updates properly + ///@todo: add 'dirty' flag + //if (body->getActivationState() != ISLAND_SLEEPING) + { + btTransform interpolatedTransform; + btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), + body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform); + body->getMotionState()->setWorldTransform(interpolatedTransform); + } + } +} + void btDiscreteDynamicsWorld::synchronizeMotionStates() { + BT_PROFILE("synchronizeMotionStates"); { //todo: iterate over awake simulation islands! for ( int i=0;i<m_collisionObjects.size();i++) @@ -260,19 +305,8 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); - if (body && body->getMotionState() && !body->isStaticOrKinematicObject()) - { - //we need to call the update at least once, even for sleeping objects - //otherwise the 'graphics' transform never updates properly - //so todo: add 'dirty' flag - //if (body->getActivationState() != ISLAND_SLEEPING) - { - btTransform interpolatedTransform; - btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), - body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform); - body->getMotionState()->setWorldTransform(interpolatedTransform); - } - } + if (body) + synchronizeSingleMotionState(body); } } @@ -327,7 +361,8 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, //process some debugging flags if (getDebugDrawer()) { - gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; + btIDebugDraw* debugDrawer = getDebugDrawer (); + gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; } if (numSimulationSubSteps) { @@ -392,7 +427,8 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) ///update vehicle simulation updateVehicles(timeStep); - + + updateCharacters(timeStep); updateActivationState( timeStep ); @@ -423,13 +459,6 @@ btVector3 btDiscreteDynamicsWorld::getGravity () const void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) { - //remove all constraints too - while (body->getNumConstraintRefs()) - { - btTypedConstraint* constraint = body->getConstraintRef(0); - removeConstraint(constraint); - } - removeCollisionObject(body); } @@ -475,6 +504,20 @@ void btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep) } } +void btDiscreteDynamicsWorld::updateCharacters(btScalar timeStep) +{ + BT_PROFILE("updateCharacters"); + + for ( int i=0;i<m_characters.size();i++) + { + btCharacterControllerInterface* character = m_characters[i]; + character->preStep (this); + character->playerStep (this,timeStep); + } +} + + + void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) { BT_PROFILE("updateActivationState"); @@ -539,6 +582,17 @@ void btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle) m_vehicles.remove(vehicle); } +void btDiscreteDynamicsWorld::addCharacter(btCharacterControllerInterface* character) +{ + m_characters.push_back(character); +} + +void btDiscreteDynamicsWorld::removeCharacter(btCharacterControllerInterface* character) +{ + m_characters.remove(character); +} + + SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) { int islandId; @@ -611,8 +665,11 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { if (islandId<0) { - ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + if (numManifolds + m_numConstraints) + { + ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + } } else { //also add all non-contact constraints/joints for this island @@ -658,7 +715,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) sortedConstraints[i] = m_constraints[i]; } -// assert(0); +// btAssert(0); @@ -671,7 +728,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); /// solve all the constraints for this island - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback); + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback); m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc); } @@ -722,21 +779,27 @@ class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConve btCollisionObject* m_me; btScalar m_allowedPenetration; btOverlappingPairCache* m_pairCache; + btDispatcher* m_dispatcher; public: - btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache) : + btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : btCollisionWorld::ClosestConvexResultCallback(fromA,toA), m_allowedPenetration(0.0f), m_me(me), - m_pairCache(pairCache) + m_pairCache(pairCache), + m_dispatcher(dispatcher) { } virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) { if (convexResult.m_hitCollisionObject == m_me) - return 1.0; + return 1.0f; + + //ignore result if there is no contact response + if(!convexResult.m_hitCollisionObject->hasContactResponse()) + return 1.0f; btVector3 linVelA,linVelB; linVelA = m_convexToWorld-m_convexFromWorld; @@ -757,23 +820,29 @@ public: return false; ///don't do CCD when the collision filters are not matching - if (!btCollisionWorld::ClosestConvexResultCallback::needsCollision(proxy0)) + if (!ClosestConvexResultCallback::needsCollision(proxy0)) return false; - ///don't do CCD when there are already contact points (touching contact/penetration) - btAlignedObjectArray<btPersistentManifold*> manifoldArray; - btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0); - if (collisionPair) + btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + + //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179 + if (m_dispatcher->needsResponse(m_me,otherObj)) { - if (collisionPair->m_algorithm) + ///don't do CCD when there are already contact points (touching contact/penetration) + btAlignedObjectArray<btPersistentManifold*> manifoldArray; + btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0); + if (collisionPair) { - manifoldArray.resize(0); - collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); - for (int j=0;j<manifoldArray.size();j++) + if (collisionPair->m_algorithm) { - btPersistentManifold* manifold = manifoldArray[j]; - if (manifold->getNumContacts()>0) - return false; + manifoldArray.resize(0); + collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); + for (int j=0;j<manifoldArray.size();j++) + { + btPersistentManifold* manifold = manifoldArray[j]; + if (manifold->getNumContacts()>0) + return false; + } } } } @@ -811,9 +880,13 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) { gNumClampedCcdMotions++; - btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache()); + btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); + + sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; + sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; + convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults); if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) { @@ -833,6 +906,8 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) + + void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { BT_PROFILE("predictUnconstraintMotion"); @@ -844,14 +919,12 @@ void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { if (!body->isStaticOrKinematicObject()) { - if (body->isActive()) - { - body->integrateVelocities( timeStep); - //damping - body->applyDamping(timeStep); + + body->integrateVelocities( timeStep); + //damping + body->applyDamping(timeStep); - body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); - } + body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); } } } @@ -1103,7 +1176,7 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, { btConcaveShape* concaveMesh = (btConcaveShape*) shape; - //todo pass camera, for some culling + ///@todo pass camera, for some culling? no -> we are not a graphics lib btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); @@ -1132,7 +1205,7 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, int i; for (i=0;i<polyshape->getNumEdges();i++) { - btPoint3 a,b; + btVector3 a,b; polyshape->getEdge(i,a,b); btVector3 wa = worldTransform * a; btVector3 wb = worldTransform * b; @@ -1148,6 +1221,189 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, } +void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) +{ + bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0; + bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0; + btScalar dbgDrawSize = constraint->getDbgDrawSize(); + if(dbgDrawSize <= btScalar(0.f)) + { + return; + } + + switch(constraint->getConstraintType()) + { + case POINT2POINT_CONSTRAINT_TYPE: + { + btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint; + btTransform tr; + tr.setIdentity(); + btVector3 pivot = p2pC->getPivotInA(); + pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; + tr.setOrigin(pivot); + getDebugDrawer()->drawTransform(tr, dbgDrawSize); + // that ideally should draw the same frame + pivot = p2pC->getPivotInB(); + pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; + tr.setOrigin(pivot); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + } + break; + case HINGE_CONSTRAINT_TYPE: + { + btHingeConstraint* pHinge = (btHingeConstraint*)constraint; + btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + btScalar minAng = pHinge->getLowerLimit(); + btScalar maxAng = pHinge->getUpperLimit(); + if(minAng == maxAng) + { + break; + } + bool drawSect = true; + if(minAng > maxAng) + { + minAng = btScalar(0.f); + maxAng = SIMD_2_PI; + drawSect = false; + } + if(drawLimits) + { + btVector3& center = tr.getOrigin(); + btVector3 normal = tr.getBasis().getColumn(2); + btVector3 axis = tr.getBasis().getColumn(0); + getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect); + } + } + break; + case CONETWIST_CONSTRAINT_TYPE: + { + btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint; + btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if(drawLimits) + { + //const btScalar length = btScalar(5); + const btScalar length = dbgDrawSize; + static int nSegments = 8*4; + btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments); + btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length); + pPrev = tr * pPrev; + for (int i=0; i<nSegments; i++) + { + fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments); + btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length); + pCur = tr * pCur; + getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0)); + + if (i%(nSegments/8) == 0) + getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0)); + + pPrev = pCur; + } + btScalar tws = pCT->getTwistSpan(); + btScalar twa = pCT->getTwistAngle(); + bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f)); + if(useFrameB) + { + tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); + } + else + { + tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); + } + btVector3 pivot = tr.getOrigin(); + btVector3 normal = tr.getBasis().getColumn(0); + btVector3 axis1 = tr.getBasis().getColumn(1); + getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true); + + } + } + break; + case D6_CONSTRAINT_TYPE: + { + btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint; + btTransform tr = p6DOF->getCalculatedTransformA(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = p6DOF->getCalculatedTransformB(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if(drawLimits) + { + tr = p6DOF->getCalculatedTransformA(); + const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin(); + btVector3 up = tr.getBasis().getColumn(2); + btVector3 axis = tr.getBasis().getColumn(0); + btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit; + btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit; + btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; + btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; + getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0)); + axis = tr.getBasis().getColumn(1); + btScalar ay = p6DOF->getAngle(1); + btScalar az = p6DOF->getAngle(2); + btScalar cy = btCos(ay); + btScalar sy = btSin(ay); + btScalar cz = btCos(az); + btScalar sz = btSin(az); + btVector3 ref; + ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2]; + ref[1] = -sz*axis[0] + cz*axis[1]; + ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2]; + tr = p6DOF->getCalculatedTransformB(); + btVector3 normal = -tr.getBasis().getColumn(0); + btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; + btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit; + if(minFi > maxFi) + { + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false); + } + else if(minFi < maxFi) + { + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true); + } + tr = p6DOF->getCalculatedTransformA(); + btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit; + btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit; + getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0)); + } + } + break; + case SLIDER_CONSTRAINT_TYPE: + { + btSliderConstraint* pSlider = (btSliderConstraint*)constraint; + btTransform tr = pSlider->getCalculatedTransformA(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pSlider->getCalculatedTransformB(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if(drawLimits) + { + btTransform tr = pSlider->getCalculatedTransformA(); + btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f); + btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f); + getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0)); + btVector3 normal = tr.getBasis().getColumn(0); + btVector3 axis = tr.getBasis().getColumn(1); + btScalar a_min = pSlider->getLowerAngLimit(); + btScalar a_max = pSlider->getUpperAngLimit(); + const btVector3& center = pSlider->getCalculatedTransformB().getOrigin(); + getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true); + } + } + break; + default : + break; + } + return; +} // btDiscreteDynamicsWorld::debugDrawConstraint() + + + + + void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) { if (m_ownsConstraintSolver) @@ -1176,3 +1432,5 @@ const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const { return m_constraints[index]; } + + diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index d9e2652aaf6..34d4e6b353f 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -26,6 +26,7 @@ class btTypedConstraint; class btRaycastVehicle; +class btCharacterControllerInterface; class btIDebugDraw; #include "LinearMath/btAlignedObjectArray.h" @@ -42,7 +43,6 @@ protected: btAlignedObjectArray<btTypedConstraint*> m_constraints; - btVector3 m_gravity; //for variable timesteps @@ -54,6 +54,9 @@ protected: btAlignedObjectArray<btRaycastVehicle*> m_vehicles; + + btAlignedObjectArray<btCharacterControllerInterface*> m_characters; + int m_profileTimings; @@ -61,24 +64,26 @@ protected: virtual void integrateTransforms(btScalar timeStep); - void calculateSimulationIslands(); + virtual void calculateSimulationIslands(); - void solveConstraints(btContactSolverInfo& solverInfo); + virtual void solveConstraints(btContactSolverInfo& solverInfo); void updateActivationState(btScalar timeStep); void updateVehicles(btScalar timeStep); + void updateCharacters(btScalar timeStep); + void startProfiling(btScalar timeStep); virtual void internalSingleStepSimulation( btScalar timeStep); - void synchronizeMotionStates(); - void saveKinematicState(btScalar timeStep); + virtual void saveKinematicState(btScalar timeStep); void debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color); + public: @@ -91,13 +96,23 @@ public: virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); - void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); + virtual void synchronizeMotionStates(); + + ///this can be useful to synchronize a single rigid body -> graphics object + void synchronizeSingleMotionState(btRigidBody* body); - void removeConstraint(btTypedConstraint* constraint); + virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); - void addVehicle(btRaycastVehicle* vehicle); + virtual void removeConstraint(btTypedConstraint* constraint); - void removeVehicle(btRaycastVehicle* vehicle); + virtual void addVehicle(btRaycastVehicle* vehicle); + + virtual void removeVehicle(btRaycastVehicle* vehicle); + + virtual void addCharacter(btCharacterControllerInterface* character); + + virtual void removeCharacter(btCharacterControllerInterface* character); + btSimulationIslandManager* getSimulationIslandManager() { @@ -114,7 +129,6 @@ public: return this; } - virtual void setGravity(const btVector3& gravity); virtual btVector3 getGravity () const; @@ -126,6 +140,8 @@ public: void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); + void debugDrawConstraint(btTypedConstraint* constraint); + virtual void debugDrawWorld(); virtual void setConstraintSolver(btConstraintSolver* solver); @@ -152,6 +168,7 @@ public: virtual void setNumTasks(int numTasks) { + (void) numTasks; } }; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h index 929e24d337c..2d90e212f7a 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -23,6 +23,7 @@ class btTypedConstraint; class btRaycastVehicle; class btConstraintSolver; class btDynamicsWorld; +class btCharacterControllerInterface; /// Type for the callback for each tick typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep); @@ -75,11 +76,18 @@ public: virtual void removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;} + virtual void addCharacter(btCharacterControllerInterface* character) {(void)character;} + + virtual void removeCharacter(btCharacterControllerInterface* character) {(void)character;} + + //once a rigidbody is added to the dynamics world, it will get this gravity assigned //existing rigidbodies in the world get gravity assigned too, during this method virtual void setGravity(const btVector3& gravity) = 0; virtual btVector3 getGravity () const = 0; + virtual void synchronizeMotionStates() = 0; + virtual void addRigidBody(btRigidBody* body) = 0; virtual void removeRigidBody(btRigidBody* body) = 0; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp index 93d70de39d8..a2b8a7dade4 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -45,8 +45,8 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); m_angularFactor = btScalar(1.); - m_anisotropicFriction.setValue(1.f,1.f,1.f); m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); + m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)), m_linearDamping = btScalar(0.); @@ -125,6 +125,7 @@ void btRigidBody::setGravity(const btVector3& acceleration) { m_gravity = acceleration * (btScalar(1.0) / m_inverseMass); } + m_gravity_acceleration = acceleration; } @@ -144,8 +145,17 @@ void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping) ///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping void btRigidBody::applyDamping(btScalar timeStep) { + //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74 + //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway + +//#define USE_OLD_DAMPING_METHOD 1 +#ifdef USE_OLD_DAMPING_METHOD m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); +#else + m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep); + m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep); +#endif if (m_additionalDamping) { diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h index 4596f90a00f..ec570cab875 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h @@ -17,7 +17,6 @@ subject to the following restrictions: #define RIGIDBODY_H #include "LinearMath/btAlignedObjectArray.h" -#include "LinearMath/btPoint3.h" #include "LinearMath/btTransform.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" @@ -31,7 +30,7 @@ extern btScalar gDeactivationTime; extern bool gDisableDeactivation; -///btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape. +///The btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape. ///It is recommended for performance and memory use to share btCollisionShape objects whenever possible. ///There are 3 types of rigid bodies: ///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics. @@ -49,6 +48,7 @@ class btRigidBody : public btCollisionObject btScalar m_angularFactor; btVector3 m_gravity; + btVector3 m_gravity_acceleration; btVector3 m_invInertiaLocal; btVector3 m_totalForce; btVector3 m_totalTorque; @@ -75,7 +75,7 @@ class btRigidBody : public btCollisionObject public: - ///btRigidBodyConstructionInfo provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body. + ///The btRigidBodyConstructionInfo structure provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body. ///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument) ///You can use the motion state to synchronize the world transform between physics and graphics objects. ///And if the motion state is provided, the rigid body will initialize its initial world transform from the motion state, @@ -182,7 +182,7 @@ public: const btVector3& getGravity() const { - return m_gravity; + return m_gravity_acceleration; } void setDamping(btScalar lin_damping, btScalar ang_damping); @@ -232,8 +232,18 @@ public: { m_totalForce += force; } + + const btVector3& getTotalForce() + { + return m_totalForce; + }; + + const btVector3& getTotalTorque() + { + return m_totalTorque; + }; - const btVector3& getInvInertiaDiagLocal() + const btVector3& getInvInertiaDiagLocal() const { return m_invInertiaLocal; }; @@ -303,7 +313,7 @@ public: void updateInertiaTensor(); - const btPoint3& getCenterOfMassPosition() const { + const btVector3& getCenterOfMassPosition() const { return m_worldTransform.getOrigin(); } btQuaternion getOrientation() const; @@ -321,15 +331,12 @@ public: inline void setLinearVelocity(const btVector3& lin_vel) { - assert (m_collisionFlags != btCollisionObject::CF_STATIC_OBJECT); m_linearVelocity = lin_vel; } - inline void setAngularVelocity(const btVector3& ang_vel) { - assert (m_collisionFlags != btCollisionObject::CF_STATIC_OBJECT); - { - m_angularVelocity = ang_vel; - } + inline void setAngularVelocity(const btVector3& ang_vel) + { + m_angularVelocity = ang_vel; } btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const @@ -353,7 +360,7 @@ public: - SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const + SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3& pos, const btVector3& normal) const { btVector3 r0 = pos - getCenterOfMassPosition(); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 3be04d1a4ad..3f6141463c3 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -97,7 +97,7 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b void btSimpleDynamicsWorld::clearForces() { - //todo: iterate over awake simulation islands! + ///@todo: iterate over awake simulation islands! for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; @@ -156,7 +156,7 @@ void btSimpleDynamicsWorld::updateAabbs() { if (body->isActive() && (!body->isStaticObject())) { - btPoint3 minAabb,maxAabb; + btVector3 minAabb,maxAabb; colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); btBroadphaseInterface* bp = getBroadphase(); bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); @@ -210,7 +210,7 @@ void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) void btSimpleDynamicsWorld::synchronizeMotionStates() { - //todo: iterate over awake simulation islands! + ///@todo: iterate over awake simulation islands! for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h index 5c56fdf1327..ea1a2a1ccc7 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h @@ -22,7 +22,7 @@ class btDispatcher; class btOverlappingPairCache; class btConstraintSolver; -///btSimpleDynamicsWorld serves as unit-test and to verify more complicated and optimized dynamics worlds. +///The btSimpleDynamicsWorld serves as unit-test and to verify more complicated and optimized dynamics worlds. ///Please use btDiscreteDynamicsWorld instead (or btContinuousDynamicsWorld once it is finished). class btSimpleDynamicsWorld : public btDynamicsWorld { @@ -60,7 +60,7 @@ public: virtual void updateAabbs(); - void synchronizeMotionStates(); + virtual void synchronizeMotionStates(); virtual void setConstraintSolver(btConstraintSolver* solver); diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp index fe65245c2a1..4982e673053 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -87,7 +87,7 @@ btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, con const btTransform& btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const { - assert(wheelIndex < getNumWheels()); + btAssert(wheelIndex < getNumWheels()); const btWheelInfo& wheel = m_wheelInfo[wheelIndex]; return wheel.m_worldTransform; @@ -175,7 +175,7 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) btVehicleRaycaster::btVehicleRaycasterResult rayResults; - assert(m_vehicleRaycaster); + btAssert(m_vehicleRaycaster); void* object = m_vehicleRaycaster->castRay(source,target,rayResults); @@ -188,7 +188,7 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld; wheel.m_raycastInfo.m_isInContact = true; - wheel.m_raycastInfo.m_groundObject = &s_fixedObject;//todo for driving on dynamic/movable objects!; + wheel.m_raycastInfo.m_groundObject = &s_fixedObject;///@todo for driving on dynamic/movable objects!; //wheel.m_raycastInfo.m_groundObject = object; @@ -359,7 +359,7 @@ void btRaycastVehicle::updateVehicle( btScalar step ) void btRaycastVehicle::setSteeringValue(btScalar steering,int wheel) { - assert(wheel>=0 && wheel < getNumWheels()); + btAssert(wheel>=0 && wheel < getNumWheels()); btWheelInfo& wheelInfo = getWheelInfo(wheel); wheelInfo.m_steering = steering; @@ -375,7 +375,7 @@ btScalar btRaycastVehicle::getSteeringValue(int wheel) const void btRaycastVehicle::applyEngineForce(btScalar force, int wheel) { - assert(wheel>=0 && wheel < getNumWheels()); + btAssert(wheel>=0 && wheel < getNumWheels()); btWheelInfo& wheelInfo = getWheelInfo(wheel); wheelInfo.m_engineForce = force; } @@ -691,7 +691,7 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel]; - rel_pos[2] *= wheelInfo.m_rollInfluence; + rel_pos[m_indexForwardAxis] *= wheelInfo.m_rollInfluence; m_chassisBody->applyImpulse(sideImp,rel_pos); //apply friction impulse on the ground diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h index 8361dcabe4b..bfe0d7df2fb 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -180,7 +180,18 @@ public: //not yet } - virtual void solveConstraint(btScalar timeStep) + virtual void getInfo1 (btConstraintInfo1* info) + { + info->m_numConstraintRows = 0; + info->nub = 0; + } + + virtual void getInfo2 (btConstraintInfo2* info) + { + btAssert(0); + } + + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { (void)timeStep; //not yet |