|
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 00017 #include "btHingeConstraint.h" 00018 #include "BulletDynamics/Dynamics/btRigidBody.h" 00019 #include "LinearMath/btTransformUtil.h" 00020 #include "LinearMath/btMinMax.h" 00021 #include <new> 00022 #include "btSolverBody.h" 00023 00024 00025 00026 //#define HINGE_USE_OBSOLETE_SOLVER false 00027 #define HINGE_USE_OBSOLETE_SOLVER false 00028 00029 #define HINGE_USE_FRAME_OFFSET true 00030 00031 #ifndef __SPU__ 00032 00033 00034 00035 00036 00037 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, 00038 const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA) 00039 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), 00040 m_angularOnly(false), 00041 m_enableAngularMotor(false), 00042 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), 00043 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), 00044 m_useReferenceFrameA(useReferenceFrameA), 00045 m_flags(0) 00046 #ifdef _BT_USE_CENTER_LIMIT_ 00047 ,m_limit() 00048 #endif 00049 { 00050 m_rbAFrame.getOrigin() = pivotInA; 00051 00052 // since no frame is given, assume this to be zero angle and just pick rb transform axis 00053 btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); 00054 00055 btVector3 rbAxisA2; 00056 btScalar projection = axisInA.dot(rbAxisA1); 00057 if (projection >= 1.0f - SIMD_EPSILON) { 00058 rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2); 00059 rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); 00060 } else if (projection <= -1.0f + SIMD_EPSILON) { 00061 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2); 00062 rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); 00063 } else { 00064 rbAxisA2 = axisInA.cross(rbAxisA1); 00065 rbAxisA1 = rbAxisA2.cross(axisInA); 00066 } 00067 00068 m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), 00069 rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), 00070 rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); 00071 00072 btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); 00073 btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); 00074 btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); 00075 00076 m_rbBFrame.getOrigin() = pivotInB; 00077 m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), 00078 rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), 00079 rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); 00080 00081 #ifndef _BT_USE_CENTER_LIMIT_ 00082 //start with free 00083 m_lowerLimit = btScalar(1.0f); 00084 m_upperLimit = btScalar(-1.0f); 00085 m_biasFactor = 0.3f; 00086 m_relaxationFactor = 1.0f; 00087 m_limitSoftness = 0.9f; 00088 m_solveLimit = false; 00089 #endif 00090 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); 00091 } 00092 00093 00094 00095 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA) 00096 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), 00097 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), 00098 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), 00099 m_useReferenceFrameA(useReferenceFrameA), 00100 m_flags(0) 00101 #ifdef _BT_USE_CENTER_LIMIT_ 00102 ,m_limit() 00103 #endif 00104 { 00105 00106 // since no frame is given, assume this to be zero angle and just pick rb transform axis 00107 // fixed axis in worldspace 00108 btVector3 rbAxisA1, rbAxisA2; 00109 btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); 00110 00111 m_rbAFrame.getOrigin() = pivotInA; 00112 m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), 00113 rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), 00114 rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); 00115 00116 btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; 00117 00118 btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); 00119 btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); 00120 btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); 00121 00122 00123 m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); 00124 m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), 00125 rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), 00126 rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); 00127 00128 #ifndef _BT_USE_CENTER_LIMIT_ 00129 //start with free 00130 m_lowerLimit = btScalar(1.0f); 00131 m_upperLimit = btScalar(-1.0f); 00132 m_biasFactor = 0.3f; 00133 m_relaxationFactor = 1.0f; 00134 m_limitSoftness = 0.9f; 00135 m_solveLimit = false; 00136 #endif 00137 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); 00138 } 00139 00140 00141 00142 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 00143 const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA) 00144 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), 00145 m_angularOnly(false), 00146 m_enableAngularMotor(false), 00147 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), 00148 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), 00149 m_useReferenceFrameA(useReferenceFrameA), 00150 m_flags(0) 00151 #ifdef _BT_USE_CENTER_LIMIT_ 00152 ,m_limit() 00153 #endif 00154 { 00155 #ifndef _BT_USE_CENTER_LIMIT_ 00156 //start with free 00157 m_lowerLimit = btScalar(1.0f); 00158 m_upperLimit = btScalar(-1.0f); 00159 m_biasFactor = 0.3f; 00160 m_relaxationFactor = 1.0f; 00161 m_limitSoftness = 0.9f; 00162 m_solveLimit = false; 00163 #endif 00164 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); 00165 } 00166 00167 00168 00169 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA) 00170 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), 00171 m_angularOnly(false), 00172 m_enableAngularMotor(false), 00173 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), 00174 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), 00175 m_useReferenceFrameA(useReferenceFrameA), 00176 m_flags(0) 00177 #ifdef _BT_USE_CENTER_LIMIT_ 00178 ,m_limit() 00179 #endif 00180 { 00182 00183 m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin()); 00184 #ifndef _BT_USE_CENTER_LIMIT_ 00185 //start with free 00186 m_lowerLimit = btScalar(1.0f); 00187 m_upperLimit = btScalar(-1.0f); 00188 m_biasFactor = 0.3f; 00189 m_relaxationFactor = 1.0f; 00190 m_limitSoftness = 0.9f; 00191 m_solveLimit = false; 00192 #endif 00193 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); 00194 } 00195 00196 00197 00198 void btHingeConstraint::buildJacobian() 00199 { 00200 if (m_useSolveConstraintObsolete) 00201 { 00202 m_appliedImpulse = btScalar(0.); 00203 m_accMotorImpulse = btScalar(0.); 00204 00205 if (!m_angularOnly) 00206 { 00207 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); 00208 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); 00209 btVector3 relPos = pivotBInW - pivotAInW; 00210 00211 btVector3 normal[3]; 00212 if (relPos.length2() > SIMD_EPSILON) 00213 { 00214 normal[0] = relPos.normalized(); 00215 } 00216 else 00217 { 00218 normal[0].setValue(btScalar(1.0),0,0); 00219 } 00220 00221 btPlaneSpace1(normal[0], normal[1], normal[2]); 00222 00223 for (int i=0;i<3;i++) 00224 { 00225 new (&m_jac[i]) btJacobianEntry( 00226 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 00227 m_rbB.getCenterOfMassTransform().getBasis().transpose(), 00228 pivotAInW - m_rbA.getCenterOfMassPosition(), 00229 pivotBInW - m_rbB.getCenterOfMassPosition(), 00230 normal[i], 00231 m_rbA.getInvInertiaDiagLocal(), 00232 m_rbA.getInvMass(), 00233 m_rbB.getInvInertiaDiagLocal(), 00234 m_rbB.getInvMass()); 00235 } 00236 } 00237 00238 //calculate two perpendicular jointAxis, orthogonal to hingeAxis 00239 //these two jointAxis require equal angular velocities for both bodies 00240 00241 //this is unused for now, it's a todo 00242 btVector3 jointAxis0local; 00243 btVector3 jointAxis1local; 00244 00245 btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); 00246 00247 btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; 00248 btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; 00249 btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); 00250 00251 new (&m_jacAng[0]) btJacobianEntry(jointAxis0, 00252 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 00253 m_rbB.getCenterOfMassTransform().getBasis().transpose(), 00254 m_rbA.getInvInertiaDiagLocal(), 00255 m_rbB.getInvInertiaDiagLocal()); 00256 00257 new (&m_jacAng[1]) btJacobianEntry(jointAxis1, 00258 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 00259 m_rbB.getCenterOfMassTransform().getBasis().transpose(), 00260 m_rbA.getInvInertiaDiagLocal(), 00261 m_rbB.getInvInertiaDiagLocal()); 00262 00263 new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, 00264 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 00265 m_rbB.getCenterOfMassTransform().getBasis().transpose(), 00266 m_rbA.getInvInertiaDiagLocal(), 00267 m_rbB.getInvInertiaDiagLocal()); 00268 00269 // clear accumulator 00270 m_accLimitImpulse = btScalar(0.); 00271 00272 // test angular limit 00273 testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); 00274 00275 //Compute K = J*W*J' for hinge axis 00276 btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); 00277 m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + 00278 getRigidBodyB().computeAngularImpulseDenominator(axisA)); 00279 00280 } 00281 } 00282 00283 00284 #endif //__SPU__ 00285 00286 00287 void btHingeConstraint::getInfo1(btConstraintInfo1* info) 00288 { 00289 if (m_useSolveConstraintObsolete) 00290 { 00291 info->m_numConstraintRows = 0; 00292 info->nub = 0; 00293 } 00294 else 00295 { 00296 info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular 00297 info->nub = 1; 00298 //always add the row, to avoid computation (data is not available yet) 00299 //prepare constraint 00300 testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); 00301 if(getSolveLimit() || getEnableAngularMotor()) 00302 { 00303 info->m_numConstraintRows++; // limit 3rd anguar as well 00304 info->nub--; 00305 } 00306 00307 } 00308 } 00309 00310 void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info) 00311 { 00312 if (m_useSolveConstraintObsolete) 00313 { 00314 info->m_numConstraintRows = 0; 00315 info->nub = 0; 00316 } 00317 else 00318 { 00319 //always add the 'limit' row, to avoid computation (data is not available yet) 00320 info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular 00321 info->nub = 0; 00322 } 00323 } 00324 00325 void btHingeConstraint::getInfo2 (btConstraintInfo2* info) 00326 { 00327 if(m_useOffsetForConstraintFrame) 00328 { 00329 getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); 00330 } 00331 else 00332 { 00333 getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); 00334 } 00335 } 00336 00337 00338 void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) 00339 { 00341 testLimit(transA,transB); 00342 00343 getInfo2Internal(info,transA,transB,angVelA,angVelB); 00344 } 00345 00346 00347 void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) 00348 { 00349 00350 btAssert(!m_useSolveConstraintObsolete); 00351 int i, skip = info->rowskip; 00352 // transforms in world space 00353 btTransform trA = transA*m_rbAFrame; 00354 btTransform trB = transB*m_rbBFrame; 00355 // pivot point 00356 btVector3 pivotAInW = trA.getOrigin(); 00357 btVector3 pivotBInW = trB.getOrigin(); 00358 #if 0 00359 if (0) 00360 { 00361 for (i=0;i<6;i++) 00362 { 00363 info->m_J1linearAxis[i*skip]=0; 00364 info->m_J1linearAxis[i*skip+1]=0; 00365 info->m_J1linearAxis[i*skip+2]=0; 00366 00367 info->m_J1angularAxis[i*skip]=0; 00368 info->m_J1angularAxis[i*skip+1]=0; 00369 info->m_J1angularAxis[i*skip+2]=0; 00370 00371 info->m_J2angularAxis[i*skip]=0; 00372 info->m_J2angularAxis[i*skip+1]=0; 00373 info->m_J2angularAxis[i*skip+2]=0; 00374 00375 info->m_constraintError[i*skip]=0.f; 00376 } 00377 } 00378 #endif //#if 0 00379 // linear (all fixed) 00380 00381 if (!m_angularOnly) 00382 { 00383 info->m_J1linearAxis[0] = 1; 00384 info->m_J1linearAxis[skip + 1] = 1; 00385 info->m_J1linearAxis[2 * skip + 2] = 1; 00386 } 00387 00388 00389 00390 00391 btVector3 a1 = pivotAInW - transA.getOrigin(); 00392 { 00393 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); 00394 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip); 00395 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip); 00396 btVector3 a1neg = -a1; 00397 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); 00398 } 00399 btVector3 a2 = pivotBInW - transB.getOrigin(); 00400 { 00401 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); 00402 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip); 00403 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip); 00404 a2.getSkewSymmetricMatrix(angular0,angular1,angular2); 00405 } 00406 // linear RHS 00407 btScalar k = info->fps * info->erp; 00408 if (!m_angularOnly) 00409 { 00410 for(i = 0; i < 3; i++) 00411 { 00412 info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]); 00413 } 00414 } 00415 // make rotations around X and Y equal 00416 // the hinge axis should be the only unconstrained 00417 // rotational axis, the angular velocity of the two bodies perpendicular to 00418 // the hinge axis should be equal. thus the constraint equations are 00419 // p*w1 - p*w2 = 0 00420 // q*w1 - q*w2 = 0 00421 // where p and q are unit vectors normal to the hinge axis, and w1 and w2 00422 // are the angular velocity vectors of the two bodies. 00423 // get hinge axis (Z) 00424 btVector3 ax1 = trA.getBasis().getColumn(2); 00425 // get 2 orthos to hinge axis (X, Y) 00426 btVector3 p = trA.getBasis().getColumn(0); 00427 btVector3 q = trA.getBasis().getColumn(1); 00428 // set the two hinge angular rows 00429 int s3 = 3 * info->rowskip; 00430 int s4 = 4 * info->rowskip; 00431 00432 info->m_J1angularAxis[s3 + 0] = p[0]; 00433 info->m_J1angularAxis[s3 + 1] = p[1]; 00434 info->m_J1angularAxis[s3 + 2] = p[2]; 00435 info->m_J1angularAxis[s4 + 0] = q[0]; 00436 info->m_J1angularAxis[s4 + 1] = q[1]; 00437 info->m_J1angularAxis[s4 + 2] = q[2]; 00438 00439 info->m_J2angularAxis[s3 + 0] = -p[0]; 00440 info->m_J2angularAxis[s3 + 1] = -p[1]; 00441 info->m_J2angularAxis[s3 + 2] = -p[2]; 00442 info->m_J2angularAxis[s4 + 0] = -q[0]; 00443 info->m_J2angularAxis[s4 + 1] = -q[1]; 00444 info->m_J2angularAxis[s4 + 2] = -q[2]; 00445 // compute the right hand side of the constraint equation. set relative 00446 // body velocities along p and q to bring the hinge back into alignment. 00447 // if ax1,ax2 are the unit length hinge axes as computed from body1 and 00448 // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). 00449 // if `theta' is the angle between ax1 and ax2, we need an angular velocity 00450 // along u to cover angle erp*theta in one step : 00451 // |angular_velocity| = angle/time = erp*theta / stepsize 00452 // = (erp*fps) * theta 00453 // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| 00454 // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) 00455 // ...as ax1 and ax2 are unit length. if theta is smallish, 00456 // theta ~= sin(theta), so 00457 // angular_velocity = (erp*fps) * (ax1 x ax2) 00458 // ax1 x ax2 is in the plane space of ax1, so we project the angular 00459 // velocity to p and q to find the right hand side. 00460 btVector3 ax2 = trB.getBasis().getColumn(2); 00461 btVector3 u = ax1.cross(ax2); 00462 info->m_constraintError[s3] = k * u.dot(p); 00463 info->m_constraintError[s4] = k * u.dot(q); 00464 // check angular limits 00465 int nrow = 4; // last filled row 00466 int srow; 00467 btScalar limit_err = btScalar(0.0); 00468 int limit = 0; 00469 if(getSolveLimit()) 00470 { 00471 #ifdef _BT_USE_CENTER_LIMIT_ 00472 limit_err = m_limit.getCorrection() * m_referenceSign; 00473 #else 00474 limit_err = m_correction * m_referenceSign; 00475 #endif 00476 limit = (limit_err > btScalar(0.0)) ? 1 : 2; 00477 00478 } 00479 // if the hinge has joint limits or motor, add in the extra row 00480 int powered = 0; 00481 if(getEnableAngularMotor()) 00482 { 00483 powered = 1; 00484 } 00485 if(limit || powered) 00486 { 00487 nrow++; 00488 srow = nrow * info->rowskip; 00489 info->m_J1angularAxis[srow+0] = ax1[0]; 00490 info->m_J1angularAxis[srow+1] = ax1[1]; 00491 info->m_J1angularAxis[srow+2] = ax1[2]; 00492 00493 info->m_J2angularAxis[srow+0] = -ax1[0]; 00494 info->m_J2angularAxis[srow+1] = -ax1[1]; 00495 info->m_J2angularAxis[srow+2] = -ax1[2]; 00496 00497 btScalar lostop = getLowerLimit(); 00498 btScalar histop = getUpperLimit(); 00499 if(limit && (lostop == histop)) 00500 { // the joint motor is ineffective 00501 powered = 0; 00502 } 00503 info->m_constraintError[srow] = btScalar(0.0f); 00504 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; 00505 if(powered) 00506 { 00507 if(m_flags & BT_HINGE_FLAGS_CFM_NORM) 00508 { 00509 info->cfm[srow] = m_normalCFM; 00510 } 00511 btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); 00512 info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; 00513 info->m_lowerLimit[srow] = - m_maxMotorImpulse; 00514 info->m_upperLimit[srow] = m_maxMotorImpulse; 00515 } 00516 if(limit) 00517 { 00518 k = info->fps * currERP; 00519 info->m_constraintError[srow] += k * limit_err; 00520 if(m_flags & BT_HINGE_FLAGS_CFM_STOP) 00521 { 00522 info->cfm[srow] = m_stopCFM; 00523 } 00524 if(lostop == histop) 00525 { 00526 // limited low and high simultaneously 00527 info->m_lowerLimit[srow] = -SIMD_INFINITY; 00528 info->m_upperLimit[srow] = SIMD_INFINITY; 00529 } 00530 else if(limit == 1) 00531 { // low limit 00532 info->m_lowerLimit[srow] = 0; 00533 info->m_upperLimit[srow] = SIMD_INFINITY; 00534 } 00535 else 00536 { // high limit 00537 info->m_lowerLimit[srow] = -SIMD_INFINITY; 00538 info->m_upperLimit[srow] = 0; 00539 } 00540 // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) 00541 #ifdef _BT_USE_CENTER_LIMIT_ 00542 btScalar bounce = m_limit.getRelaxationFactor(); 00543 #else 00544 btScalar bounce = m_relaxationFactor; 00545 #endif 00546 if(bounce > btScalar(0.0)) 00547 { 00548 btScalar vel = angVelA.dot(ax1); 00549 vel -= angVelB.dot(ax1); 00550 // only apply bounce if the velocity is incoming, and if the 00551 // resulting c[] exceeds what we already have. 00552 if(limit == 1) 00553 { // low limit 00554 if(vel < 0) 00555 { 00556 btScalar newc = -bounce * vel; 00557 if(newc > info->m_constraintError[srow]) 00558 { 00559 info->m_constraintError[srow] = newc; 00560 } 00561 } 00562 } 00563 else 00564 { // high limit - all those computations are reversed 00565 if(vel > 0) 00566 { 00567 btScalar newc = -bounce * vel; 00568 if(newc < info->m_constraintError[srow]) 00569 { 00570 info->m_constraintError[srow] = newc; 00571 } 00572 } 00573 } 00574 } 00575 #ifdef _BT_USE_CENTER_LIMIT_ 00576 info->m_constraintError[srow] *= m_limit.getBiasFactor(); 00577 #else 00578 info->m_constraintError[srow] *= m_biasFactor; 00579 #endif 00580 } // if(limit) 00581 } // if angular limit or powered 00582 } 00583 00584 00585 void btHingeConstraint::setFrames(const btTransform & frameA, const btTransform & frameB) 00586 { 00587 m_rbAFrame = frameA; 00588 m_rbBFrame = frameB; 00589 buildJacobian(); 00590 } 00591 00592 00593 void btHingeConstraint::updateRHS(btScalar timeStep) 00594 { 00595 (void)timeStep; 00596 00597 } 00598 00599 00600 btScalar btHingeConstraint::getHingeAngle() 00601 { 00602 return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); 00603 } 00604 00605 btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB) 00606 { 00607 const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); 00608 const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); 00609 const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1); 00610 // btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); 00611 btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); 00612 return m_referenceSign * angle; 00613 } 00614 00615 00616 00617 void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB) 00618 { 00619 // Compute limit information 00620 m_hingeAngle = getHingeAngle(transA,transB); 00621 #ifdef _BT_USE_CENTER_LIMIT_ 00622 m_limit.test(m_hingeAngle); 00623 #else 00624 m_correction = btScalar(0.); 00625 m_limitSign = btScalar(0.); 00626 m_solveLimit = false; 00627 if (m_lowerLimit <= m_upperLimit) 00628 { 00629 m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit); 00630 if (m_hingeAngle <= m_lowerLimit) 00631 { 00632 m_correction = (m_lowerLimit - m_hingeAngle); 00633 m_limitSign = 1.0f; 00634 m_solveLimit = true; 00635 } 00636 else if (m_hingeAngle >= m_upperLimit) 00637 { 00638 m_correction = m_upperLimit - m_hingeAngle; 00639 m_limitSign = -1.0f; 00640 m_solveLimit = true; 00641 } 00642 } 00643 #endif 00644 return; 00645 } 00646 00647 00648 static btVector3 vHinge(0, 0, btScalar(1)); 00649 00650 void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) 00651 { 00652 // convert target from body to constraint space 00653 btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation(); 00654 qConstraint.normalize(); 00655 00656 // extract "pure" hinge component 00657 btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize(); 00658 btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge); 00659 btQuaternion qHinge = qNoHinge.inverse() * qConstraint; 00660 qHinge.normalize(); 00661 00662 // compute angular target, clamped to limits 00663 btScalar targetAngle = qHinge.getAngle(); 00664 if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate. 00665 { 00666 qHinge = operator-(qHinge); 00667 targetAngle = qHinge.getAngle(); 00668 } 00669 if (qHinge.getZ() < 0) 00670 targetAngle = -targetAngle; 00671 00672 setMotorTarget(targetAngle, dt); 00673 } 00674 00675 void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt) 00676 { 00677 #ifdef _BT_USE_CENTER_LIMIT_ 00678 m_limit.fit(targetAngle); 00679 #else 00680 if (m_lowerLimit < m_upperLimit) 00681 { 00682 if (targetAngle < m_lowerLimit) 00683 targetAngle = m_lowerLimit; 00684 else if (targetAngle > m_upperLimit) 00685 targetAngle = m_upperLimit; 00686 } 00687 #endif 00688 // compute angular velocity 00689 btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); 00690 btScalar dAngle = targetAngle - curAngle; 00691 m_motorTargetVelocity = dAngle / dt; 00692 } 00693 00694 00695 00696 void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) 00697 { 00698 btAssert(!m_useSolveConstraintObsolete); 00699 int i, s = info->rowskip; 00700 // transforms in world space 00701 btTransform trA = transA*m_rbAFrame; 00702 btTransform trB = transB*m_rbBFrame; 00703 // pivot point 00704 btVector3 pivotAInW = trA.getOrigin(); 00705 btVector3 pivotBInW = trB.getOrigin(); 00706 #if 1 00707 // difference between frames in WCS 00708 btVector3 ofs = trB.getOrigin() - trA.getOrigin(); 00709 // now get weight factors depending on masses 00710 btScalar miA = getRigidBodyA().getInvMass(); 00711 btScalar miB = getRigidBodyB().getInvMass(); 00712 bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); 00713 btScalar miS = miA + miB; 00714 btScalar factA, factB; 00715 if(miS > btScalar(0.f)) 00716 { 00717 factA = miB / miS; 00718 } 00719 else 00720 { 00721 factA = btScalar(0.5f); 00722 } 00723 factB = btScalar(1.0f) - factA; 00724 // get the desired direction of hinge axis 00725 // as weighted sum of Z-orthos of frameA and frameB in WCS 00726 btVector3 ax1A = trA.getBasis().getColumn(2); 00727 btVector3 ax1B = trB.getBasis().getColumn(2); 00728 btVector3 ax1 = ax1A * factA + ax1B * factB; 00729 ax1.normalize(); 00730 // fill first 3 rows 00731 // we want: velA + wA x relA == velB + wB x relB 00732 btTransform bodyA_trans = transA; 00733 btTransform bodyB_trans = transB; 00734 int s0 = 0; 00735 int s1 = s; 00736 int s2 = s * 2; 00737 int nrow = 2; // last filled row 00738 btVector3 tmpA, tmpB, relA, relB, p, q; 00739 // get vector from bodyB to frameB in WCS 00740 relB = trB.getOrigin() - bodyB_trans.getOrigin(); 00741 // get its projection to hinge axis 00742 btVector3 projB = ax1 * relB.dot(ax1); 00743 // get vector directed from bodyB to hinge axis (and orthogonal to it) 00744 btVector3 orthoB = relB - projB; 00745 // same for bodyA 00746 relA = trA.getOrigin() - bodyA_trans.getOrigin(); 00747 btVector3 projA = ax1 * relA.dot(ax1); 00748 btVector3 orthoA = relA - projA; 00749 btVector3 totalDist = projA - projB; 00750 // get offset vectors relA and relB 00751 relA = orthoA + totalDist * factA; 00752 relB = orthoB - totalDist * factB; 00753 // now choose average ortho to hinge axis 00754 p = orthoB * factA + orthoA * factB; 00755 btScalar len2 = p.length2(); 00756 if(len2 > SIMD_EPSILON) 00757 { 00758 p /= btSqrt(len2); 00759 } 00760 else 00761 { 00762 p = trA.getBasis().getColumn(1); 00763 } 00764 // make one more ortho 00765 q = ax1.cross(p); 00766 // fill three rows 00767 tmpA = relA.cross(p); 00768 tmpB = relB.cross(p); 00769 for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i]; 00770 for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i]; 00771 tmpA = relA.cross(q); 00772 tmpB = relB.cross(q); 00773 if(hasStaticBody && getSolveLimit()) 00774 { // to make constraint between static and dynamic objects more rigid 00775 // remove wA (or wB) from equation if angular limit is hit 00776 tmpB *= factB; 00777 tmpA *= factA; 00778 } 00779 for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i]; 00780 for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i]; 00781 tmpA = relA.cross(ax1); 00782 tmpB = relB.cross(ax1); 00783 if(hasStaticBody) 00784 { // to make constraint between static and dynamic objects more rigid 00785 // remove wA (or wB) from equation 00786 tmpB *= factB; 00787 tmpA *= factA; 00788 } 00789 for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i]; 00790 for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i]; 00791 00792 btScalar k = info->fps * info->erp; 00793 00794 if (!m_angularOnly) 00795 { 00796 for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i]; 00797 for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i]; 00798 for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i]; 00799 00800 // compute three elements of right hand side 00801 00802 btScalar rhs = k * p.dot(ofs); 00803 info->m_constraintError[s0] = rhs; 00804 rhs = k * q.dot(ofs); 00805 info->m_constraintError[s1] = rhs; 00806 rhs = k * ax1.dot(ofs); 00807 info->m_constraintError[s2] = rhs; 00808 } 00809 // the hinge axis should be the only unconstrained 00810 // rotational axis, the angular velocity of the two bodies perpendicular to 00811 // the hinge axis should be equal. thus the constraint equations are 00812 // p*w1 - p*w2 = 0 00813 // q*w1 - q*w2 = 0 00814 // where p and q are unit vectors normal to the hinge axis, and w1 and w2 00815 // are the angular velocity vectors of the two bodies. 00816 int s3 = 3 * s; 00817 int s4 = 4 * s; 00818 info->m_J1angularAxis[s3 + 0] = p[0]; 00819 info->m_J1angularAxis[s3 + 1] = p[1]; 00820 info->m_J1angularAxis[s3 + 2] = p[2]; 00821 info->m_J1angularAxis[s4 + 0] = q[0]; 00822 info->m_J1angularAxis[s4 + 1] = q[1]; 00823 info->m_J1angularAxis[s4 + 2] = q[2]; 00824 00825 info->m_J2angularAxis[s3 + 0] = -p[0]; 00826 info->m_J2angularAxis[s3 + 1] = -p[1]; 00827 info->m_J2angularAxis[s3 + 2] = -p[2]; 00828 info->m_J2angularAxis[s4 + 0] = -q[0]; 00829 info->m_J2angularAxis[s4 + 1] = -q[1]; 00830 info->m_J2angularAxis[s4 + 2] = -q[2]; 00831 // compute the right hand side of the constraint equation. set relative 00832 // body velocities along p and q to bring the hinge back into alignment. 00833 // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and 00834 // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2). 00835 // if "theta" is the angle between ax1 and ax2, we need an angular velocity 00836 // along u to cover angle erp*theta in one step : 00837 // |angular_velocity| = angle/time = erp*theta / stepsize 00838 // = (erp*fps) * theta 00839 // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| 00840 // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) 00841 // ...as ax1 and ax2 are unit length. if theta is smallish, 00842 // theta ~= sin(theta), so 00843 // angular_velocity = (erp*fps) * (ax1 x ax2) 00844 // ax1 x ax2 is in the plane space of ax1, so we project the angular 00845 // velocity to p and q to find the right hand side. 00846 k = info->fps * info->erp; 00847 btVector3 u = ax1A.cross(ax1B); 00848 info->m_constraintError[s3] = k * u.dot(p); 00849 info->m_constraintError[s4] = k * u.dot(q); 00850 #endif 00851 // check angular limits 00852 nrow = 4; // last filled row 00853 int srow; 00854 btScalar limit_err = btScalar(0.0); 00855 int limit = 0; 00856 if(getSolveLimit()) 00857 { 00858 #ifdef _BT_USE_CENTER_LIMIT_ 00859 limit_err = m_limit.getCorrection() * m_referenceSign; 00860 #else 00861 limit_err = m_correction * m_referenceSign; 00862 #endif 00863 limit = (limit_err > btScalar(0.0)) ? 1 : 2; 00864 00865 } 00866 // if the hinge has joint limits or motor, add in the extra row 00867 int powered = 0; 00868 if(getEnableAngularMotor()) 00869 { 00870 powered = 1; 00871 } 00872 if(limit || powered) 00873 { 00874 nrow++; 00875 srow = nrow * info->rowskip; 00876 info->m_J1angularAxis[srow+0] = ax1[0]; 00877 info->m_J1angularAxis[srow+1] = ax1[1]; 00878 info->m_J1angularAxis[srow+2] = ax1[2]; 00879 00880 info->m_J2angularAxis[srow+0] = -ax1[0]; 00881 info->m_J2angularAxis[srow+1] = -ax1[1]; 00882 info->m_J2angularAxis[srow+2] = -ax1[2]; 00883 00884 btScalar lostop = getLowerLimit(); 00885 btScalar histop = getUpperLimit(); 00886 if(limit && (lostop == histop)) 00887 { // the joint motor is ineffective 00888 powered = 0; 00889 } 00890 info->m_constraintError[srow] = btScalar(0.0f); 00891 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; 00892 if(powered) 00893 { 00894 if(m_flags & BT_HINGE_FLAGS_CFM_NORM) 00895 { 00896 info->cfm[srow] = m_normalCFM; 00897 } 00898 btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); 00899 info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; 00900 info->m_lowerLimit[srow] = - m_maxMotorImpulse; 00901 info->m_upperLimit[srow] = m_maxMotorImpulse; 00902 } 00903 if(limit) 00904 { 00905 k = info->fps * currERP; 00906 info->m_constraintError[srow] += k * limit_err; 00907 if(m_flags & BT_HINGE_FLAGS_CFM_STOP) 00908 { 00909 info->cfm[srow] = m_stopCFM; 00910 } 00911 if(lostop == histop) 00912 { 00913 // limited low and high simultaneously 00914 info->m_lowerLimit[srow] = -SIMD_INFINITY; 00915 info->m_upperLimit[srow] = SIMD_INFINITY; 00916 } 00917 else if(limit == 1) 00918 { // low limit 00919 info->m_lowerLimit[srow] = 0; 00920 info->m_upperLimit[srow] = SIMD_INFINITY; 00921 } 00922 else 00923 { // high limit 00924 info->m_lowerLimit[srow] = -SIMD_INFINITY; 00925 info->m_upperLimit[srow] = 0; 00926 } 00927 // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) 00928 #ifdef _BT_USE_CENTER_LIMIT_ 00929 btScalar bounce = m_limit.getRelaxationFactor(); 00930 #else 00931 btScalar bounce = m_relaxationFactor; 00932 #endif 00933 if(bounce > btScalar(0.0)) 00934 { 00935 btScalar vel = angVelA.dot(ax1); 00936 vel -= angVelB.dot(ax1); 00937 // only apply bounce if the velocity is incoming, and if the 00938 // resulting c[] exceeds what we already have. 00939 if(limit == 1) 00940 { // low limit 00941 if(vel < 0) 00942 { 00943 btScalar newc = -bounce * vel; 00944 if(newc > info->m_constraintError[srow]) 00945 { 00946 info->m_constraintError[srow] = newc; 00947 } 00948 } 00949 } 00950 else 00951 { // high limit - all those computations are reversed 00952 if(vel > 0) 00953 { 00954 btScalar newc = -bounce * vel; 00955 if(newc < info->m_constraintError[srow]) 00956 { 00957 info->m_constraintError[srow] = newc; 00958 } 00959 } 00960 } 00961 } 00962 #ifdef _BT_USE_CENTER_LIMIT_ 00963 info->m_constraintError[srow] *= m_limit.getBiasFactor(); 00964 #else 00965 info->m_constraintError[srow] *= m_biasFactor; 00966 #endif 00967 } // if(limit) 00968 } // if angular limit or powered 00969 } 00970 00971 00974 void btHingeConstraint::setParam(int num, btScalar value, int axis) 00975 { 00976 if((axis == -1) || (axis == 5)) 00977 { 00978 switch(num) 00979 { 00980 case BT_CONSTRAINT_STOP_ERP : 00981 m_stopERP = value; 00982 m_flags |= BT_HINGE_FLAGS_ERP_STOP; 00983 break; 00984 case BT_CONSTRAINT_STOP_CFM : 00985 m_stopCFM = value; 00986 m_flags |= BT_HINGE_FLAGS_CFM_STOP; 00987 break; 00988 case BT_CONSTRAINT_CFM : 00989 m_normalCFM = value; 00990 m_flags |= BT_HINGE_FLAGS_CFM_NORM; 00991 break; 00992 default : 00993 btAssertConstrParams(0); 00994 } 00995 } 00996 else 00997 { 00998 btAssertConstrParams(0); 00999 } 01000 } 01001 01003 btScalar btHingeConstraint::getParam(int num, int axis) const 01004 { 01005 btScalar retVal = 0; 01006 if((axis == -1) || (axis == 5)) 01007 { 01008 switch(num) 01009 { 01010 case BT_CONSTRAINT_STOP_ERP : 01011 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP); 01012 retVal = m_stopERP; 01013 break; 01014 case BT_CONSTRAINT_STOP_CFM : 01015 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP); 01016 retVal = m_stopCFM; 01017 break; 01018 case BT_CONSTRAINT_CFM : 01019 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM); 01020 retVal = m_normalCFM; 01021 break; 01022 default : 01023 btAssertConstrParams(0); 01024 } 01025 } 01026 else 01027 { 01028 btAssertConstrParams(0); 01029 } 01030 return retVal; 01031 } 01032 01033