|
Blender
V2.59
|
00001 /* 00002 Bullet Continuous Collision Detection and Physics Library 00003 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ 00004 00005 This software is provided 'as-is', without any express or implied warranty. 00006 In no event will the authors be held liable for any damages arising from the use of this software. 00007 Permission is granted to anyone to use this software for any purpose, 00008 including commercial applications, and to alter it and redistribute it freely, 00009 subject to the following restrictions: 00010 00011 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 00013 3. This notice may not be removed or altered from any source distribution. 00014 */ 00015 /* 00016 2007-09-09 00017 Refactored by Francisco Le?n 00018 email: projectileman@yahoo.com 00019 http://gimpact.sf.net 00020 */ 00021 00022 #include "btGeneric6DofConstraint.h" 00023 #include "BulletDynamics/Dynamics/btRigidBody.h" 00024 #include "LinearMath/btTransformUtil.h" 00025 #include "LinearMath/btTransformUtil.h" 00026 #include <new> 00027 00028 00029 00030 #define D6_USE_OBSOLETE_METHOD false 00031 #define D6_USE_FRAME_OFFSET true 00032 00033 00034 00035 00036 00037 00038 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) 00039 : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB) 00040 , m_frameInA(frameInA) 00041 , m_frameInB(frameInB), 00042 m_useLinearReferenceFrameA(useLinearReferenceFrameA), 00043 m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), 00044 m_flags(0), 00045 m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) 00046 { 00047 calculateTransforms(); 00048 } 00049 00050 00051 00052 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB) 00053 : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB), 00054 m_frameInB(frameInB), 00055 m_useLinearReferenceFrameA(useLinearReferenceFrameB), 00056 m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), 00057 m_flags(0), 00058 m_useSolveConstraintObsolete(false) 00059 { 00061 m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; 00062 calculateTransforms(); 00063 } 00064 00065 00066 00067 00068 #define GENERIC_D6_DISABLE_WARMSTARTING 1 00069 00070 00071 00072 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); 00073 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index) 00074 { 00075 int i = index%3; 00076 int j = index/3; 00077 return mat[i][j]; 00078 } 00079 00080 00081 00083 bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz); 00084 bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) 00085 { 00086 // // rot = cy*cz -cy*sz sy 00087 // // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx 00088 // // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy 00089 // 00090 00091 btScalar fi = btGetMatrixElem(mat,2); 00092 if (fi < btScalar(1.0f)) 00093 { 00094 if (fi > btScalar(-1.0f)) 00095 { 00096 xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8)); 00097 xyz[1] = btAsin(btGetMatrixElem(mat,2)); 00098 xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); 00099 return true; 00100 } 00101 else 00102 { 00103 // WARNING. Not unique. XA - ZA = -atan2(r10,r11) 00104 xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); 00105 xyz[1] = -SIMD_HALF_PI; 00106 xyz[2] = btScalar(0.0); 00107 return false; 00108 } 00109 } 00110 else 00111 { 00112 // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) 00113 xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); 00114 xyz[1] = SIMD_HALF_PI; 00115 xyz[2] = 0.0; 00116 } 00117 return false; 00118 } 00119 00121 00122 int btRotationalLimitMotor::testLimitValue(btScalar test_value) 00123 { 00124 if(m_loLimit>m_hiLimit) 00125 { 00126 m_currentLimit = 0;//Free from violation 00127 return 0; 00128 } 00129 if (test_value < m_loLimit) 00130 { 00131 m_currentLimit = 1;//low limit violation 00132 m_currentLimitError = test_value - m_loLimit; 00133 return 1; 00134 } 00135 else if (test_value> m_hiLimit) 00136 { 00137 m_currentLimit = 2;//High limit violation 00138 m_currentLimitError = test_value - m_hiLimit; 00139 return 2; 00140 }; 00141 00142 m_currentLimit = 0;//Free from violation 00143 return 0; 00144 00145 } 00146 00147 00148 00149 btScalar btRotationalLimitMotor::solveAngularLimits( 00150 btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, 00151 btRigidBody * body0, btRigidBody * body1 ) 00152 { 00153 if (needApplyTorques()==false) return 0.0f; 00154 00155 btScalar target_velocity = m_targetVelocity; 00156 btScalar maxMotorForce = m_maxMotorForce; 00157 00158 //current error correction 00159 if (m_currentLimit!=0) 00160 { 00161 target_velocity = -m_stopERP*m_currentLimitError/(timeStep); 00162 maxMotorForce = m_maxLimitForce; 00163 } 00164 00165 maxMotorForce *= timeStep; 00166 00167 // current velocity difference 00168 00169 btVector3 angVelA; 00170 body0->internalGetAngularVelocity(angVelA); 00171 btVector3 angVelB; 00172 body1->internalGetAngularVelocity(angVelB); 00173 00174 btVector3 vel_diff; 00175 vel_diff = angVelA-angVelB; 00176 00177 00178 00179 btScalar rel_vel = axis.dot(vel_diff); 00180 00181 // correction velocity 00182 btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); 00183 00184 00185 if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON ) 00186 { 00187 return 0.0f;//no need for applying force 00188 } 00189 00190 00191 // correction impulse 00192 btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; 00193 00194 // clip correction impulse 00195 btScalar clippedMotorImpulse; 00196 00198 if (unclippedMotorImpulse>0.0f) 00199 { 00200 clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; 00201 } 00202 else 00203 { 00204 clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; 00205 } 00206 00207 00208 // sort with accumulated impulses 00209 btScalar lo = btScalar(-BT_LARGE_FLOAT); 00210 btScalar hi = btScalar(BT_LARGE_FLOAT); 00211 00212 btScalar oldaccumImpulse = m_accumulatedImpulse; 00213 btScalar sum = oldaccumImpulse + clippedMotorImpulse; 00214 m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; 00215 00216 clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; 00217 00218 btVector3 motorImp = clippedMotorImpulse * axis; 00219 00220 //body0->applyTorqueImpulse(motorImp); 00221 //body1->applyTorqueImpulse(-motorImp); 00222 00223 body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse); 00224 body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse); 00225 00226 00227 return clippedMotorImpulse; 00228 00229 00230 } 00231 00233 00234 00235 00236 00238 00239 00240 int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value) 00241 { 00242 btScalar loLimit = m_lowerLimit[limitIndex]; 00243 btScalar hiLimit = m_upperLimit[limitIndex]; 00244 if(loLimit > hiLimit) 00245 { 00246 m_currentLimit[limitIndex] = 0;//Free from violation 00247 m_currentLimitError[limitIndex] = btScalar(0.f); 00248 return 0; 00249 } 00250 00251 if (test_value < loLimit) 00252 { 00253 m_currentLimit[limitIndex] = 2;//low limit violation 00254 m_currentLimitError[limitIndex] = test_value - loLimit; 00255 return 2; 00256 } 00257 else if (test_value> hiLimit) 00258 { 00259 m_currentLimit[limitIndex] = 1;//High limit violation 00260 m_currentLimitError[limitIndex] = test_value - hiLimit; 00261 return 1; 00262 }; 00263 00264 m_currentLimit[limitIndex] = 0;//Free from violation 00265 m_currentLimitError[limitIndex] = btScalar(0.f); 00266 return 0; 00267 } 00268 00269 00270 00271 btScalar btTranslationalLimitMotor::solveLinearAxis( 00272 btScalar timeStep, 00273 btScalar jacDiagABInv, 00274 btRigidBody& body1,const btVector3 &pointInA, 00275 btRigidBody& body2,const btVector3 &pointInB, 00276 int limit_index, 00277 const btVector3 & axis_normal_on_a, 00278 const btVector3 & anchorPos) 00279 { 00280 00282 // btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); 00283 // btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); 00284 btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition(); 00285 btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); 00286 00287 btVector3 vel1; 00288 body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); 00289 btVector3 vel2; 00290 body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2); 00291 btVector3 vel = vel1 - vel2; 00292 00293 btScalar rel_vel = axis_normal_on_a.dot(vel); 00294 00295 00296 00298 00299 //positional error (zeroth order error) 00300 btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); 00301 btScalar lo = btScalar(-BT_LARGE_FLOAT); 00302 btScalar hi = btScalar(BT_LARGE_FLOAT); 00303 00304 btScalar minLimit = m_lowerLimit[limit_index]; 00305 btScalar maxLimit = m_upperLimit[limit_index]; 00306 00307 //handle the limits 00308 if (minLimit < maxLimit) 00309 { 00310 { 00311 if (depth > maxLimit) 00312 { 00313 depth -= maxLimit; 00314 lo = btScalar(0.); 00315 00316 } 00317 else 00318 { 00319 if (depth < minLimit) 00320 { 00321 depth -= minLimit; 00322 hi = btScalar(0.); 00323 } 00324 else 00325 { 00326 return 0.0f; 00327 } 00328 } 00329 } 00330 } 00331 00332 btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; 00333 00334 00335 00336 00337 btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index]; 00338 btScalar sum = oldNormalImpulse + normalImpulse; 00339 m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; 00340 normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; 00341 00342 btVector3 impulse_vector = axis_normal_on_a * normalImpulse; 00343 //body1.applyImpulse( impulse_vector, rel_pos1); 00344 //body2.applyImpulse(-impulse_vector, rel_pos2); 00345 00346 btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a); 00347 btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a); 00348 body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); 00349 body2.internalApplyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); 00350 00351 00352 00353 00354 return normalImpulse; 00355 } 00356 00358 00359 void btGeneric6DofConstraint::calculateAngleInfo() 00360 { 00361 btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis(); 00362 matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); 00363 // in euler angle mode we do not actually constrain the angular velocity 00364 // along the axes axis[0] and axis[2] (although we do use axis[1]) : 00365 // 00366 // to get constrain w2-w1 along ...not 00367 // ------ --------------------- ------ 00368 // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] 00369 // d(angle[1])/dt = 0 ax[1] 00370 // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] 00371 // 00372 // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. 00373 // to prove the result for angle[0], write the expression for angle[0] from 00374 // GetInfo1 then take the derivative. to prove this for angle[2] it is 00375 // easier to take the euler rate expression for d(angle[2])/dt with respect 00376 // to the components of w and set that to 0. 00377 btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0); 00378 btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2); 00379 00380 m_calculatedAxis[1] = axis2.cross(axis0); 00381 m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); 00382 m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); 00383 00384 m_calculatedAxis[0].normalize(); 00385 m_calculatedAxis[1].normalize(); 00386 m_calculatedAxis[2].normalize(); 00387 00388 } 00389 00390 void btGeneric6DofConstraint::calculateTransforms() 00391 { 00392 calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); 00393 } 00394 00395 void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB) 00396 { 00397 m_calculatedTransformA = transA * m_frameInA; 00398 m_calculatedTransformB = transB * m_frameInB; 00399 calculateLinearInfo(); 00400 calculateAngleInfo(); 00401 if(m_useOffsetForConstraintFrame) 00402 { // get weight factors depending on masses 00403 btScalar miA = getRigidBodyA().getInvMass(); 00404 btScalar miB = getRigidBodyB().getInvMass(); 00405 m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); 00406 btScalar miS = miA + miB; 00407 if(miS > btScalar(0.f)) 00408 { 00409 m_factA = miB / miS; 00410 } 00411 else 00412 { 00413 m_factA = btScalar(0.5f); 00414 } 00415 m_factB = btScalar(1.0f) - m_factA; 00416 } 00417 } 00418 00419 00420 00421 void btGeneric6DofConstraint::buildLinearJacobian( 00422 btJacobianEntry & jacLinear,const btVector3 & normalWorld, 00423 const btVector3 & pivotAInW,const btVector3 & pivotBInW) 00424 { 00425 new (&jacLinear) btJacobianEntry( 00426 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 00427 m_rbB.getCenterOfMassTransform().getBasis().transpose(), 00428 pivotAInW - m_rbA.getCenterOfMassPosition(), 00429 pivotBInW - m_rbB.getCenterOfMassPosition(), 00430 normalWorld, 00431 m_rbA.getInvInertiaDiagLocal(), 00432 m_rbA.getInvMass(), 00433 m_rbB.getInvInertiaDiagLocal(), 00434 m_rbB.getInvMass()); 00435 } 00436 00437 00438 00439 void btGeneric6DofConstraint::buildAngularJacobian( 00440 btJacobianEntry & jacAngular,const btVector3 & jointAxisW) 00441 { 00442 new (&jacAngular) btJacobianEntry(jointAxisW, 00443 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 00444 m_rbB.getCenterOfMassTransform().getBasis().transpose(), 00445 m_rbA.getInvInertiaDiagLocal(), 00446 m_rbB.getInvInertiaDiagLocal()); 00447 00448 } 00449 00450 00451 00452 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) 00453 { 00454 btScalar angle = m_calculatedAxisAngleDiff[axis_index]; 00455 angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit); 00456 m_angularLimits[axis_index].m_currentPosition = angle; 00457 //test limits 00458 m_angularLimits[axis_index].testLimitValue(angle); 00459 return m_angularLimits[axis_index].needApplyTorques(); 00460 } 00461 00462 00463 00464 void btGeneric6DofConstraint::buildJacobian() 00465 { 00466 #ifndef __SPU__ 00467 if (m_useSolveConstraintObsolete) 00468 { 00469 00470 // Clear accumulated impulses for the next simulation step 00471 m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); 00472 int i; 00473 for(i = 0; i < 3; i++) 00474 { 00475 m_angularLimits[i].m_accumulatedImpulse = btScalar(0.); 00476 } 00477 //calculates transform 00478 calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); 00479 00480 // const btVector3& pivotAInW = m_calculatedTransformA.getOrigin(); 00481 // const btVector3& pivotBInW = m_calculatedTransformB.getOrigin(); 00482 calcAnchorPos(); 00483 btVector3 pivotAInW = m_AnchorPos; 00484 btVector3 pivotBInW = m_AnchorPos; 00485 00486 // not used here 00487 // btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 00488 // btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); 00489 00490 btVector3 normalWorld; 00491 //linear part 00492 for (i=0;i<3;i++) 00493 { 00494 if (m_linearLimits.isLimited(i)) 00495 { 00496 if (m_useLinearReferenceFrameA) 00497 normalWorld = m_calculatedTransformA.getBasis().getColumn(i); 00498 else 00499 normalWorld = m_calculatedTransformB.getBasis().getColumn(i); 00500 00501 buildLinearJacobian( 00502 m_jacLinear[i],normalWorld , 00503 pivotAInW,pivotBInW); 00504 00505 } 00506 } 00507 00508 // angular part 00509 for (i=0;i<3;i++) 00510 { 00511 //calculates error angle 00512 if (testAngularLimitMotor(i)) 00513 { 00514 normalWorld = this->getAxis(i); 00515 // Create angular atom 00516 buildAngularJacobian(m_jacAng[i],normalWorld); 00517 } 00518 } 00519 00520 } 00521 #endif //__SPU__ 00522 00523 } 00524 00525 00526 void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) 00527 { 00528 if (m_useSolveConstraintObsolete) 00529 { 00530 info->m_numConstraintRows = 0; 00531 info->nub = 0; 00532 } else 00533 { 00534 //prepare constraint 00535 calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); 00536 info->m_numConstraintRows = 0; 00537 info->nub = 6; 00538 int i; 00539 //test linear limits 00540 for(i = 0; i < 3; i++) 00541 { 00542 if(m_linearLimits.needApplyForce(i)) 00543 { 00544 info->m_numConstraintRows++; 00545 info->nub--; 00546 } 00547 } 00548 //test angular limits 00549 for (i=0;i<3 ;i++ ) 00550 { 00551 if(testAngularLimitMotor(i)) 00552 { 00553 info->m_numConstraintRows++; 00554 info->nub--; 00555 } 00556 } 00557 } 00558 } 00559 00560 void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info) 00561 { 00562 if (m_useSolveConstraintObsolete) 00563 { 00564 info->m_numConstraintRows = 0; 00565 info->nub = 0; 00566 } else 00567 { 00568 //pre-allocate all 6 00569 info->m_numConstraintRows = 6; 00570 info->nub = 0; 00571 } 00572 } 00573 00574 00575 void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info) 00576 { 00577 btAssert(!m_useSolveConstraintObsolete); 00578 00579 const btTransform& transA = m_rbA.getCenterOfMassTransform(); 00580 const btTransform& transB = m_rbB.getCenterOfMassTransform(); 00581 const btVector3& linVelA = m_rbA.getLinearVelocity(); 00582 const btVector3& linVelB = m_rbB.getLinearVelocity(); 00583 const btVector3& angVelA = m_rbA.getAngularVelocity(); 00584 const btVector3& angVelB = m_rbB.getAngularVelocity(); 00585 00586 if(m_useOffsetForConstraintFrame) 00587 { // for stability better to solve angular limits first 00588 int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB); 00589 setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB); 00590 } 00591 else 00592 { // leave old version for compatibility 00593 int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB); 00594 setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB); 00595 } 00596 00597 } 00598 00599 00600 void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) 00601 { 00602 00603 btAssert(!m_useSolveConstraintObsolete); 00604 //prepare constraint 00605 calculateTransforms(transA,transB); 00606 00607 int i; 00608 for (i=0;i<3 ;i++ ) 00609 { 00610 testAngularLimitMotor(i); 00611 } 00612 00613 if(m_useOffsetForConstraintFrame) 00614 { // for stability better to solve angular limits first 00615 int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB); 00616 setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB); 00617 } 00618 else 00619 { // leave old version for compatibility 00620 int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB); 00621 setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB); 00622 } 00623 } 00624 00625 00626 00627 int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) 00628 { 00629 // int row = 0; 00630 //solve linear limits 00631 btRotationalLimitMotor limot; 00632 for (int i=0;i<3 ;i++ ) 00633 { 00634 if(m_linearLimits.needApplyForce(i)) 00635 { // re-use rotational motor code 00636 limot.m_bounce = btScalar(0.f); 00637 limot.m_currentLimit = m_linearLimits.m_currentLimit[i]; 00638 limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i]; 00639 limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i]; 00640 limot.m_damping = m_linearLimits.m_damping; 00641 limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; 00642 limot.m_hiLimit = m_linearLimits.m_upperLimit[i]; 00643 limot.m_limitSoftness = m_linearLimits.m_limitSoftness; 00644 limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; 00645 limot.m_maxLimitForce = btScalar(0.f); 00646 limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; 00647 limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; 00648 btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i); 00649 int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT); 00650 limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0]; 00651 limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0]; 00652 limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp; 00653 if(m_useOffsetForConstraintFrame) 00654 { 00655 int indx1 = (i + 1) % 3; 00656 int indx2 = (i + 2) % 3; 00657 int rotAllowed = 1; // rotations around orthos to current axis 00658 if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit) 00659 { 00660 rotAllowed = 0; 00661 } 00662 row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed); 00663 } 00664 else 00665 { 00666 row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0); 00667 } 00668 } 00669 } 00670 return row; 00671 } 00672 00673 00674 00675 int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) 00676 { 00677 btGeneric6DofConstraint * d6constraint = this; 00678 int row = row_offset; 00679 //solve angular limits 00680 for (int i=0;i<3 ;i++ ) 00681 { 00682 if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) 00683 { 00684 btVector3 axis = d6constraint->getAxis(i); 00685 int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT); 00686 if(!(flags & BT_6DOF_FLAGS_CFM_NORM)) 00687 { 00688 m_angularLimits[i].m_normalCFM = info->cfm[0]; 00689 } 00690 if(!(flags & BT_6DOF_FLAGS_CFM_STOP)) 00691 { 00692 m_angularLimits[i].m_stopCFM = info->cfm[0]; 00693 } 00694 if(!(flags & BT_6DOF_FLAGS_ERP_STOP)) 00695 { 00696 m_angularLimits[i].m_stopERP = info->erp; 00697 } 00698 row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i), 00699 transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1); 00700 } 00701 } 00702 00703 return row; 00704 } 00705 00706 00707 00708 00709 void btGeneric6DofConstraint::updateRHS(btScalar timeStep) 00710 { 00711 (void)timeStep; 00712 00713 } 00714 00715 00716 void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB) 00717 { 00718 m_frameInA = frameA; 00719 m_frameInB = frameB; 00720 buildJacobian(); 00721 calculateTransforms(); 00722 } 00723 00724 00725 00726 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const 00727 { 00728 return m_calculatedAxis[axis_index]; 00729 } 00730 00731 00732 btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const 00733 { 00734 return m_calculatedLinearDiff[axisIndex]; 00735 } 00736 00737 00738 btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const 00739 { 00740 return m_calculatedAxisAngleDiff[axisIndex]; 00741 } 00742 00743 00744 00745 void btGeneric6DofConstraint::calcAnchorPos(void) 00746 { 00747 btScalar imA = m_rbA.getInvMass(); 00748 btScalar imB = m_rbB.getInvMass(); 00749 btScalar weight; 00750 if(imB == btScalar(0.0)) 00751 { 00752 weight = btScalar(1.0); 00753 } 00754 else 00755 { 00756 weight = imA / (imA + imB); 00757 } 00758 const btVector3& pA = m_calculatedTransformA.getOrigin(); 00759 const btVector3& pB = m_calculatedTransformB.getOrigin(); 00760 m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight); 00761 return; 00762 } 00763 00764 00765 00766 void btGeneric6DofConstraint::calculateLinearInfo() 00767 { 00768 m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin(); 00769 m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff; 00770 for(int i = 0; i < 3; i++) 00771 { 00772 m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i]; 00773 m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]); 00774 } 00775 } 00776 00777 00778 00779 int btGeneric6DofConstraint::get_limit_motor_info2( 00780 btRotationalLimitMotor * limot, 00781 const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, 00782 btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed) 00783 { 00784 int srow = row * info->rowskip; 00785 int powered = limot->m_enableMotor; 00786 int limit = limot->m_currentLimit; 00787 if (powered || limit) 00788 { // if the joint is powered, or has joint limits, add in the extra row 00789 btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; 00790 btScalar *J2 = rotational ? info->m_J2angularAxis : 0; 00791 J1[srow+0] = ax1[0]; 00792 J1[srow+1] = ax1[1]; 00793 J1[srow+2] = ax1[2]; 00794 if(rotational) 00795 { 00796 J2[srow+0] = -ax1[0]; 00797 J2[srow+1] = -ax1[1]; 00798 J2[srow+2] = -ax1[2]; 00799 } 00800 if((!rotational)) 00801 { 00802 if (m_useOffsetForConstraintFrame) 00803 { 00804 btVector3 tmpA, tmpB, relA, relB; 00805 // get vector from bodyB to frameB in WCS 00806 relB = m_calculatedTransformB.getOrigin() - transB.getOrigin(); 00807 // get its projection to constraint axis 00808 btVector3 projB = ax1 * relB.dot(ax1); 00809 // get vector directed from bodyB to constraint axis (and orthogonal to it) 00810 btVector3 orthoB = relB - projB; 00811 // same for bodyA 00812 relA = m_calculatedTransformA.getOrigin() - transA.getOrigin(); 00813 btVector3 projA = ax1 * relA.dot(ax1); 00814 btVector3 orthoA = relA - projA; 00815 // get desired offset between frames A and B along constraint axis 00816 btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError; 00817 // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis 00818 btVector3 totalDist = projA + ax1 * desiredOffs - projB; 00819 // get offset vectors relA and relB 00820 relA = orthoA + totalDist * m_factA; 00821 relB = orthoB - totalDist * m_factB; 00822 tmpA = relA.cross(ax1); 00823 tmpB = relB.cross(ax1); 00824 if(m_hasStaticBody && (!rotAllowed)) 00825 { 00826 tmpA *= m_factA; 00827 tmpB *= m_factB; 00828 } 00829 int i; 00830 for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i]; 00831 for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i]; 00832 } else 00833 { 00834 btVector3 ltd; // Linear Torque Decoupling vector 00835 btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin(); 00836 ltd = c.cross(ax1); 00837 info->m_J1angularAxis[srow+0] = ltd[0]; 00838 info->m_J1angularAxis[srow+1] = ltd[1]; 00839 info->m_J1angularAxis[srow+2] = ltd[2]; 00840 00841 c = m_calculatedTransformB.getOrigin() - transB.getOrigin(); 00842 ltd = -c.cross(ax1); 00843 info->m_J2angularAxis[srow+0] = ltd[0]; 00844 info->m_J2angularAxis[srow+1] = ltd[1]; 00845 info->m_J2angularAxis[srow+2] = ltd[2]; 00846 } 00847 } 00848 // if we're limited low and high simultaneously, the joint motor is 00849 // ineffective 00850 if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0; 00851 info->m_constraintError[srow] = btScalar(0.f); 00852 if (powered) 00853 { 00854 info->cfm[srow] = limot->m_normalCFM; 00855 if(!limit) 00856 { 00857 btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity; 00858 00859 btScalar mot_fact = getMotorFactor( limot->m_currentPosition, 00860 limot->m_loLimit, 00861 limot->m_hiLimit, 00862 tag_vel, 00863 info->fps * limot->m_stopERP); 00864 info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity; 00865 info->m_lowerLimit[srow] = -limot->m_maxMotorForce; 00866 info->m_upperLimit[srow] = limot->m_maxMotorForce; 00867 } 00868 } 00869 if(limit) 00870 { 00871 btScalar k = info->fps * limot->m_stopERP; 00872 if(!rotational) 00873 { 00874 info->m_constraintError[srow] += k * limot->m_currentLimitError; 00875 } 00876 else 00877 { 00878 info->m_constraintError[srow] += -k * limot->m_currentLimitError; 00879 } 00880 info->cfm[srow] = limot->m_stopCFM; 00881 if (limot->m_loLimit == limot->m_hiLimit) 00882 { // limited low and high simultaneously 00883 info->m_lowerLimit[srow] = -SIMD_INFINITY; 00884 info->m_upperLimit[srow] = SIMD_INFINITY; 00885 } 00886 else 00887 { 00888 if (limit == 1) 00889 { 00890 info->m_lowerLimit[srow] = 0; 00891 info->m_upperLimit[srow] = SIMD_INFINITY; 00892 } 00893 else 00894 { 00895 info->m_lowerLimit[srow] = -SIMD_INFINITY; 00896 info->m_upperLimit[srow] = 0; 00897 } 00898 // deal with bounce 00899 if (limot->m_bounce > 0) 00900 { 00901 // calculate joint velocity 00902 btScalar vel; 00903 if (rotational) 00904 { 00905 vel = angVelA.dot(ax1); 00906 //make sure that if no body -> angVelB == zero vec 00907 // if (body1) 00908 vel -= angVelB.dot(ax1); 00909 } 00910 else 00911 { 00912 vel = linVelA.dot(ax1); 00913 //make sure that if no body -> angVelB == zero vec 00914 // if (body1) 00915 vel -= linVelB.dot(ax1); 00916 } 00917 // only apply bounce if the velocity is incoming, and if the 00918 // resulting c[] exceeds what we already have. 00919 if (limit == 1) 00920 { 00921 if (vel < 0) 00922 { 00923 btScalar newc = -limot->m_bounce* vel; 00924 if (newc > info->m_constraintError[srow]) 00925 info->m_constraintError[srow] = newc; 00926 } 00927 } 00928 else 00929 { 00930 if (vel > 0) 00931 { 00932 btScalar newc = -limot->m_bounce * vel; 00933 if (newc < info->m_constraintError[srow]) 00934 info->m_constraintError[srow] = newc; 00935 } 00936 } 00937 } 00938 } 00939 } 00940 return 1; 00941 } 00942 else return 0; 00943 } 00944 00945 00946 00947 00948 00949 00952 void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis) 00953 { 00954 if((axis >= 0) && (axis < 3)) 00955 { 00956 switch(num) 00957 { 00958 case BT_CONSTRAINT_STOP_ERP : 00959 m_linearLimits.m_stopERP[axis] = value; 00960 m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); 00961 break; 00962 case BT_CONSTRAINT_STOP_CFM : 00963 m_linearLimits.m_stopCFM[axis] = value; 00964 m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); 00965 break; 00966 case BT_CONSTRAINT_CFM : 00967 m_linearLimits.m_normalCFM[axis] = value; 00968 m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); 00969 break; 00970 default : 00971 btAssertConstrParams(0); 00972 } 00973 } 00974 else if((axis >=3) && (axis < 6)) 00975 { 00976 switch(num) 00977 { 00978 case BT_CONSTRAINT_STOP_ERP : 00979 m_angularLimits[axis - 3].m_stopERP = value; 00980 m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); 00981 break; 00982 case BT_CONSTRAINT_STOP_CFM : 00983 m_angularLimits[axis - 3].m_stopCFM = value; 00984 m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); 00985 break; 00986 case BT_CONSTRAINT_CFM : 00987 m_angularLimits[axis - 3].m_normalCFM = value; 00988 m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); 00989 break; 00990 default : 00991 btAssertConstrParams(0); 00992 } 00993 } 00994 else 00995 { 00996 btAssertConstrParams(0); 00997 } 00998 } 00999 01001 btScalar btGeneric6DofConstraint::getParam(int num, int axis) const 01002 { 01003 btScalar retVal = 0; 01004 if((axis >= 0) && (axis < 3)) 01005 { 01006 switch(num) 01007 { 01008 case BT_CONSTRAINT_STOP_ERP : 01009 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); 01010 retVal = m_linearLimits.m_stopERP[axis]; 01011 break; 01012 case BT_CONSTRAINT_STOP_CFM : 01013 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); 01014 retVal = m_linearLimits.m_stopCFM[axis]; 01015 break; 01016 case BT_CONSTRAINT_CFM : 01017 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); 01018 retVal = m_linearLimits.m_normalCFM[axis]; 01019 break; 01020 default : 01021 btAssertConstrParams(0); 01022 } 01023 } 01024 else if((axis >=3) && (axis < 6)) 01025 { 01026 switch(num) 01027 { 01028 case BT_CONSTRAINT_STOP_ERP : 01029 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); 01030 retVal = m_angularLimits[axis - 3].m_stopERP; 01031 break; 01032 case BT_CONSTRAINT_STOP_CFM : 01033 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); 01034 retVal = m_angularLimits[axis - 3].m_stopCFM; 01035 break; 01036 case BT_CONSTRAINT_CFM : 01037 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); 01038 retVal = m_angularLimits[axis - 3].m_normalCFM; 01039 break; 01040 default : 01041 btAssertConstrParams(0); 01042 } 01043 } 01044 else 01045 { 01046 btAssertConstrParams(0); 01047 } 01048 return retVal; 01049 } 01050 01051 01052 01053 void btGeneric6DofConstraint::setAxis(const btVector3& axis1,const btVector3& axis2) 01054 { 01055 btVector3 zAxis = axis1.normalized(); 01056 btVector3 yAxis = axis2.normalized(); 01057 btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system 01058 01059 btTransform frameInW; 01060 frameInW.setIdentity(); 01061 frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], 01062 xAxis[1], yAxis[1], zAxis[1], 01063 xAxis[2], yAxis[2], zAxis[2]); 01064 01065 // now get constraint frame in local coordinate systems 01066 m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW; 01067 m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW; 01068 01069 calculateTransforms(); 01070 }