- Timestamp:
- Mar 31, 2009, 8:05:51 PM (15 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
r2662 r2882 20 20 #include "LinearMath/btMinMax.h" 21 21 #include <new> 22 #include "btSolverBody.h" 23 24 //----------------------------------------------------------------------------- 25 26 #define HINGE_USE_OBSOLETE_SOLVER false 27 28 //----------------------------------------------------------------------------- 22 29 23 30 24 31 btHingeConstraint::btHingeConstraint() 25 32 : btTypedConstraint (HINGE_CONSTRAINT_TYPE), 26 m_enableAngularMotor(false) 27 { 28 } 33 m_enableAngularMotor(false), 34 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), 35 m_useReferenceFrameA(false) 36 { 37 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); 38 } 39 40 //----------------------------------------------------------------------------- 29 41 30 42 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, 31 btVector3& axisInA,btVector3& axisInB )43 btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA) 32 44 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), 33 45 m_angularOnly(false), 34 m_enableAngularMotor(false) 46 m_enableAngularMotor(false), 47 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), 48 m_useReferenceFrameA(useReferenceFrameA) 35 49 { 36 50 m_rbAFrame.getOrigin() = pivotInA; … … 61 75 62 76 m_rbBFrame.getOrigin() = pivotInB; 63 m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(), -axisInB.getX(),64 rbAxisB1.getY(),rbAxisB2.getY(), -axisInB.getY(),65 rbAxisB1.getZ(),rbAxisB2.getZ(), -axisInB.getZ() );77 m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), 78 rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), 79 rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); 66 80 67 81 //start with free … … 72 86 m_limitSoftness = 0.9f; 73 87 m_solveLimit = false; 74 75 } 76 77 78 79 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA) 80 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false) 88 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); 89 } 90 91 //----------------------------------------------------------------------------- 92 93 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA) 94 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), 95 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), 96 m_useReferenceFrameA(useReferenceFrameA) 81 97 { 82 98 … … 91 107 rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); 92 108 93 btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;109 btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; 94 110 95 111 btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); … … 110 126 m_limitSoftness = 0.9f; 111 127 m_solveLimit = false; 112 } 128 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); 129 } 130 131 //----------------------------------------------------------------------------- 113 132 114 133 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 115 const btTransform& rbAFrame, const btTransform& rbBFrame )134 const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA) 116 135 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), 117 136 m_angularOnly(false), 118 m_enableAngularMotor(false) 119 { 120 // flip axis 121 m_rbBFrame.getBasis()[0][2] *= btScalar(-1.); 122 m_rbBFrame.getBasis()[1][2] *= btScalar(-1.); 123 m_rbBFrame.getBasis()[2][2] *= btScalar(-1.); 124 137 m_enableAngularMotor(false), 138 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), 139 m_useReferenceFrameA(useReferenceFrameA) 140 { 125 141 //start with free 126 142 m_lowerLimit = btScalar(1e30); … … 130 146 m_limitSoftness = 0.9f; 131 147 m_solveLimit = false; 148 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); 132 149 } 133 150 134 135 136 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame )151 //----------------------------------------------------------------------------- 152 153 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA) 137 154 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), 138 155 m_angularOnly(false), 139 m_enableAngularMotor(false) 156 m_enableAngularMotor(false), 157 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), 158 m_useReferenceFrameA(useReferenceFrameA) 140 159 { 141 160 ///not providing rigidbody B means implicitly using worldspace for body B 142 143 // flip axis144 m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);145 m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);146 m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);147 161 148 162 m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin()); … … 155 169 m_limitSoftness = 0.9f; 156 170 m_solveLimit = false; 157 } 171 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); 172 } 173 174 //----------------------------------------------------------------------------- 158 175 159 176 void btHingeConstraint::buildJacobian() 160 177 { 161 m_appliedImpulse = btScalar(0.); 162 163 if (!m_angularOnly) 164 { 165 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); 166 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); 167 btVector3 relPos = pivotBInW - pivotAInW; 168 169 btVector3 normal[3]; 170 if (relPos.length2() > SIMD_EPSILON) 171 { 172 normal[0] = relPos.normalized(); 173 } 174 else 175 { 176 normal[0].setValue(btScalar(1.0),0,0); 177 } 178 179 btPlaneSpace1(normal[0], normal[1], normal[2]); 180 181 for (int i=0;i<3;i++) 182 { 183 new (&m_jac[i]) btJacobianEntry( 178 if (m_useSolveConstraintObsolete) 179 { 180 m_appliedImpulse = btScalar(0.); 181 182 if (!m_angularOnly) 183 { 184 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); 185 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); 186 btVector3 relPos = pivotBInW - pivotAInW; 187 188 btVector3 normal[3]; 189 if (relPos.length2() > SIMD_EPSILON) 190 { 191 normal[0] = relPos.normalized(); 192 } 193 else 194 { 195 normal[0].setValue(btScalar(1.0),0,0); 196 } 197 198 btPlaneSpace1(normal[0], normal[1], normal[2]); 199 200 for (int i=0;i<3;i++) 201 { 202 new (&m_jac[i]) btJacobianEntry( 184 203 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 185 204 m_rbB.getCenterOfMassTransform().getBasis().transpose(), … … 191 210 m_rbB.getInvInertiaDiagLocal(), 192 211 m_rbB.getInvMass()); 212 } 193 213 } 194 } 195 196 //calculate two perpendicular jointAxis, orthogonal to hingeAxis 197 //these two jointAxis require equal angular velocities for both bodies 198 199 //this is unused for now, it's a todo 200 btVector3 jointAxis0local; 201 btVector3 jointAxis1local; 202 203 btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); 204 205 getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); 206 btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; 207 btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; 208 btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); 214 215 //calculate two perpendicular jointAxis, orthogonal to hingeAxis 216 //these two jointAxis require equal angular velocities for both bodies 217 218 //this is unused for now, it's a todo 219 btVector3 jointAxis0local; 220 btVector3 jointAxis1local; 209 221 210 new (&m_jacAng[0]) btJacobianEntry(jointAxis0, 211 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 212 m_rbB.getCenterOfMassTransform().getBasis().transpose(), 213 m_rbA.getInvInertiaDiagLocal(), 214 m_rbB.getInvInertiaDiagLocal()); 215 216 new (&m_jacAng[1]) btJacobianEntry(jointAxis1, 217 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 218 m_rbB.getCenterOfMassTransform().getBasis().transpose(), 219 m_rbA.getInvInertiaDiagLocal(), 220 m_rbB.getInvInertiaDiagLocal()); 221 222 new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, 223 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 224 m_rbB.getCenterOfMassTransform().getBasis().transpose(), 225 m_rbA.getInvInertiaDiagLocal(), 226 m_rbB.getInvInertiaDiagLocal()); 227 228 222 btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); 223 224 getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); 225 btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; 226 btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; 227 btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); 228 229 new (&m_jacAng[0]) btJacobianEntry(jointAxis0, 230 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 231 m_rbB.getCenterOfMassTransform().getBasis().transpose(), 232 m_rbA.getInvInertiaDiagLocal(), 233 m_rbB.getInvInertiaDiagLocal()); 234 235 new (&m_jacAng[1]) btJacobianEntry(jointAxis1, 236 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 237 m_rbB.getCenterOfMassTransform().getBasis().transpose(), 238 m_rbA.getInvInertiaDiagLocal(), 239 m_rbB.getInvInertiaDiagLocal()); 240 241 new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, 242 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 243 m_rbB.getCenterOfMassTransform().getBasis().transpose(), 244 m_rbA.getInvInertiaDiagLocal(), 245 m_rbB.getInvInertiaDiagLocal()); 246 247 // clear accumulator 248 m_accLimitImpulse = btScalar(0.); 249 250 // test angular limit 251 testLimit(); 252 253 //Compute K = J*W*J' for hinge axis 254 btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); 255 m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + 256 getRigidBodyB().computeAngularImpulseDenominator(axisA)); 257 258 } 259 } 260 261 //----------------------------------------------------------------------------- 262 263 void btHingeConstraint::getInfo1(btConstraintInfo1* info) 264 { 265 if (m_useSolveConstraintObsolete) 266 { 267 info->m_numConstraintRows = 0; 268 info->nub = 0; 269 } 270 else 271 { 272 info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular 273 info->nub = 1; 274 //prepare constraint 275 testLimit(); 276 if(getSolveLimit() || getEnableAngularMotor()) 277 { 278 info->m_numConstraintRows++; // limit 3rd anguar as well 279 info->nub--; 280 } 281 } 282 } // btHingeConstraint::getInfo1 () 283 284 //----------------------------------------------------------------------------- 285 286 void btHingeConstraint::getInfo2 (btConstraintInfo2* info) 287 { 288 btAssert(!m_useSolveConstraintObsolete); 289 int i, s = info->rowskip; 290 // transforms in world space 291 btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame; 292 btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame; 293 // pivot point 294 btVector3 pivotAInW = trA.getOrigin(); 295 btVector3 pivotBInW = trB.getOrigin(); 296 // linear (all fixed) 297 info->m_J1linearAxis[0] = 1; 298 info->m_J1linearAxis[s + 1] = 1; 299 info->m_J1linearAxis[2 * s + 2] = 1; 300 btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin(); 301 { 302 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); 303 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s); 304 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s); 305 btVector3 a1neg = -a1; 306 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); 307 } 308 btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin(); 309 { 310 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); 311 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s); 312 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s); 313 a2.getSkewSymmetricMatrix(angular0,angular1,angular2); 314 } 315 // linear RHS 316 btScalar k = info->fps * info->erp; 317 for(i = 0; i < 3; i++) 318 { 319 info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]); 320 } 321 // make rotations around X and Y equal 322 // the hinge axis should be the only unconstrained 323 // rotational axis, the angular velocity of the two bodies perpendicular to 324 // the hinge axis should be equal. thus the constraint equations are 325 // p*w1 - p*w2 = 0 326 // q*w1 - q*w2 = 0 327 // where p and q are unit vectors normal to the hinge axis, and w1 and w2 328 // are the angular velocity vectors of the two bodies. 329 // get hinge axis (Z) 330 btVector3 ax1 = trA.getBasis().getColumn(2); 331 // get 2 orthos to hinge axis (X, Y) 332 btVector3 p = trA.getBasis().getColumn(0); 333 btVector3 q = trA.getBasis().getColumn(1); 334 // set the two hinge angular rows 335 int s3 = 3 * info->rowskip; 336 int s4 = 4 * info->rowskip; 337 338 info->m_J1angularAxis[s3 + 0] = p[0]; 339 info->m_J1angularAxis[s3 + 1] = p[1]; 340 info->m_J1angularAxis[s3 + 2] = p[2]; 341 info->m_J1angularAxis[s4 + 0] = q[0]; 342 info->m_J1angularAxis[s4 + 1] = q[1]; 343 info->m_J1angularAxis[s4 + 2] = q[2]; 344 345 info->m_J2angularAxis[s3 + 0] = -p[0]; 346 info->m_J2angularAxis[s3 + 1] = -p[1]; 347 info->m_J2angularAxis[s3 + 2] = -p[2]; 348 info->m_J2angularAxis[s4 + 0] = -q[0]; 349 info->m_J2angularAxis[s4 + 1] = -q[1]; 350 info->m_J2angularAxis[s4 + 2] = -q[2]; 351 // compute the right hand side of the constraint equation. set relative 352 // body velocities along p and q to bring the hinge back into alignment. 353 // if ax1,ax2 are the unit length hinge axes as computed from body1 and 354 // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). 355 // if `theta' is the angle between ax1 and ax2, we need an angular velocity 356 // along u to cover angle erp*theta in one step : 357 // |angular_velocity| = angle/time = erp*theta / stepsize 358 // = (erp*fps) * theta 359 // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| 360 // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) 361 // ...as ax1 and ax2 are unit length. if theta is smallish, 362 // theta ~= sin(theta), so 363 // angular_velocity = (erp*fps) * (ax1 x ax2) 364 // ax1 x ax2 is in the plane space of ax1, so we project the angular 365 // velocity to p and q to find the right hand side. 366 btVector3 ax2 = trB.getBasis().getColumn(2); 367 btVector3 u = ax1.cross(ax2); 368 info->m_constraintError[s3] = k * u.dot(p); 369 info->m_constraintError[s4] = k * u.dot(q); 370 // check angular limits 371 int nrow = 4; // last filled row 372 int srow; 373 btScalar limit_err = btScalar(0.0); 374 int limit = 0; 375 if(getSolveLimit()) 376 { 377 limit_err = m_correction * m_referenceSign; 378 limit = (limit_err > btScalar(0.0)) ? 1 : 2; 379 } 380 // if the hinge has joint limits or motor, add in the extra row 381 int powered = 0; 382 if(getEnableAngularMotor()) 383 { 384 powered = 1; 385 } 386 if(limit || powered) 387 { 388 nrow++; 389 srow = nrow * info->rowskip; 390 info->m_J1angularAxis[srow+0] = ax1[0]; 391 info->m_J1angularAxis[srow+1] = ax1[1]; 392 info->m_J1angularAxis[srow+2] = ax1[2]; 393 394 info->m_J2angularAxis[srow+0] = -ax1[0]; 395 info->m_J2angularAxis[srow+1] = -ax1[1]; 396 info->m_J2angularAxis[srow+2] = -ax1[2]; 397 398 btScalar lostop = getLowerLimit(); 399 btScalar histop = getUpperLimit(); 400 if(limit && (lostop == histop)) 401 { // the joint motor is ineffective 402 powered = 0; 403 } 404 info->m_constraintError[srow] = btScalar(0.0f); 405 if(powered) 406 { 407 info->cfm[srow] = btScalar(0.0); 408 btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp); 409 info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; 410 info->m_lowerLimit[srow] = - m_maxMotorImpulse; 411 info->m_upperLimit[srow] = m_maxMotorImpulse; 412 } 413 if(limit) 414 { 415 k = info->fps * info->erp; 416 info->m_constraintError[srow] += k * limit_err; 417 info->cfm[srow] = btScalar(0.0); 418 if(lostop == histop) 419 { 420 // limited low and high simultaneously 421 info->m_lowerLimit[srow] = -SIMD_INFINITY; 422 info->m_upperLimit[srow] = SIMD_INFINITY; 423 } 424 else if(limit == 1) 425 { // low limit 426 info->m_lowerLimit[srow] = 0; 427 info->m_upperLimit[srow] = SIMD_INFINITY; 428 } 429 else 430 { // high limit 431 info->m_lowerLimit[srow] = -SIMD_INFINITY; 432 info->m_upperLimit[srow] = 0; 433 } 434 // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) 435 btScalar bounce = m_relaxationFactor; 436 if(bounce > btScalar(0.0)) 437 { 438 btScalar vel = m_rbA.getAngularVelocity().dot(ax1); 439 vel -= m_rbB.getAngularVelocity().dot(ax1); 440 // only apply bounce if the velocity is incoming, and if the 441 // resulting c[] exceeds what we already have. 442 if(limit == 1) 443 { // low limit 444 if(vel < 0) 445 { 446 btScalar newc = -bounce * vel; 447 if(newc > info->m_constraintError[srow]) 448 { 449 info->m_constraintError[srow] = newc; 450 } 451 } 452 } 453 else 454 { // high limit - all those computations are reversed 455 if(vel > 0) 456 { 457 btScalar newc = -bounce * vel; 458 if(newc < info->m_constraintError[srow]) 459 { 460 info->m_constraintError[srow] = newc; 461 } 462 } 463 } 464 } 465 info->m_constraintError[srow] *= m_biasFactor; 466 } // if(limit) 467 } // if angular limit or powered 468 } 469 470 //----------------------------------------------------------------------------- 471 472 void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) 473 { 474 475 ///for backwards compatibility during the transition to 'getInfo/getInfo2' 476 if (m_useSolveConstraintObsolete) 477 { 478 479 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); 480 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); 481 482 btScalar tau = btScalar(0.3); 483 484 //linear part 485 if (!m_angularOnly) 486 { 487 btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 488 btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); 489 490 btVector3 vel1,vel2; 491 bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); 492 bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); 493 btVector3 vel = vel1 - vel2; 494 495 for (int i=0;i<3;i++) 496 { 497 const btVector3& normal = m_jac[i].m_linearJointAxis; 498 btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); 499 500 btScalar rel_vel; 501 rel_vel = normal.dot(vel); 502 //positional error (zeroth order error) 503 btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal 504 btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; 505 m_appliedImpulse += impulse; 506 btVector3 impulse_vector = normal * impulse; 507 btVector3 ftorqueAxis1 = rel_pos1.cross(normal); 508 btVector3 ftorqueAxis2 = rel_pos2.cross(normal); 509 bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); 510 bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); 511 } 512 } 513 514 515 { 516 ///solve angular part 517 518 // get axes in world space 519 btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); 520 btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); 521 522 btVector3 angVelA; 523 bodyA.getAngularVelocity(angVelA); 524 btVector3 angVelB; 525 bodyB.getAngularVelocity(angVelB); 526 527 btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); 528 btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); 529 530 btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; 531 btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; 532 btVector3 velrelOrthog = angAorthog-angBorthog; 533 { 534 535 536 //solve orthogonal angular velocity correction 537 btScalar relaxation = btScalar(1.); 538 btScalar len = velrelOrthog.length(); 539 if (len > btScalar(0.00001)) 540 { 541 btVector3 normal = velrelOrthog.normalized(); 542 btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + 543 getRigidBodyB().computeAngularImpulseDenominator(normal); 544 // scale for mass and relaxation 545 //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; 546 547 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom)); 548 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom)); 549 550 } 551 552 //solve angular positional correction 553 btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep); 554 btScalar len2 = angularError.length(); 555 if (len2>btScalar(0.00001)) 556 { 557 btVector3 normal2 = angularError.normalized(); 558 btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + 559 getRigidBodyB().computeAngularImpulseDenominator(normal2); 560 //angularError *= (btScalar(1.)/denom2) * relaxation; 561 562 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2)); 563 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2)); 564 565 } 566 567 568 569 570 571 // solve limit 572 if (m_solveLimit) 573 { 574 btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; 575 576 btScalar impulseMag = amplitude * m_kHinge; 577 578 // Clamp the accumulated impulse 579 btScalar temp = m_accLimitImpulse; 580 m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); 581 impulseMag = m_accLimitImpulse - temp; 582 583 584 585 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign); 586 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign)); 587 588 } 589 } 590 591 //apply motor 592 if (m_enableAngularMotor) 593 { 594 //todo: add limits too 595 btVector3 angularLimit(0,0,0); 596 597 btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; 598 btScalar projRelVel = velrel.dot(axisA); 599 600 btScalar desiredMotorVel = m_motorTargetVelocity; 601 btScalar motor_relvel = desiredMotorVel - projRelVel; 602 603 btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; 604 //todo: should clip against accumulated impulse 605 btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; 606 clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; 607 btVector3 motorImp = clippedMotorImpulse * axisA; 608 609 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse); 610 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse); 611 612 } 613 } 614 } 615 616 } 617 618 //----------------------------------------------------------------------------- 619 620 void btHingeConstraint::updateRHS(btScalar timeStep) 621 { 622 (void)timeStep; 623 624 } 625 626 //----------------------------------------------------------------------------- 627 628 btScalar btHingeConstraint::getHingeAngle() 629 { 630 const btVector3 refAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0); 631 const btVector3 refAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1); 632 const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1); 633 btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); 634 return m_referenceSign * angle; 635 } 636 637 //----------------------------------------------------------------------------- 638 639 void btHingeConstraint::testLimit() 640 { 229 641 // Compute limit information 230 btScalar hingeAngle = getHingeAngle(); 231 232 //set bias, sign, clear accumulator 642 m_hingeAngle = getHingeAngle(); 233 643 m_correction = btScalar(0.); 234 644 m_limitSign = btScalar(0.); 235 645 m_solveLimit = false; 236 m_accLimitImpulse = btScalar(0.);237 238 // if (m_lowerLimit < m_upperLimit)239 646 if (m_lowerLimit <= m_upperLimit) 240 647 { 241 // if (hingeAngle <= m_lowerLimit*m_limitSoftness) 242 if (hingeAngle <= m_lowerLimit) 243 { 244 m_correction = (m_lowerLimit - hingeAngle); 648 if (m_hingeAngle <= m_lowerLimit) 649 { 650 m_correction = (m_lowerLimit - m_hingeAngle); 245 651 m_limitSign = 1.0f; 246 652 m_solveLimit = true; 247 653 } 248 // else if (hingeAngle >= m_upperLimit*m_limitSoftness) 249 else if (hingeAngle >= m_upperLimit) 250 { 251 m_correction = m_upperLimit - hingeAngle; 654 else if (m_hingeAngle >= m_upperLimit) 655 { 656 m_correction = m_upperLimit - m_hingeAngle; 252 657 m_limitSign = -1.0f; 253 658 m_solveLimit = true; 254 659 } 255 660 } 256 257 //Compute K = J*W*J' for hinge axis 258 btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); 259 m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + 260 getRigidBodyB().computeAngularImpulseDenominator(axisA)); 261 262 } 263 264 void btHingeConstraint::solveConstraint(btScalar timeStep) 265 { 266 267 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); 268 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); 269 270 btScalar tau = btScalar(0.3); 271 272 //linear part 273 if (!m_angularOnly) 274 { 275 btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 276 btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); 277 278 btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); 279 btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); 280 btVector3 vel = vel1 - vel2; 281 282 for (int i=0;i<3;i++) 283 { 284 const btVector3& normal = m_jac[i].m_linearJointAxis; 285 btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); 286 287 btScalar rel_vel; 288 rel_vel = normal.dot(vel); 289 //positional error (zeroth order error) 290 btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal 291 btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; 292 m_appliedImpulse += impulse; 293 btVector3 impulse_vector = normal * impulse; 294 m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); 295 m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); 296 } 297 } 298 299 300 { 301 ///solve angular part 302 303 // get axes in world space 304 btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); 305 btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); 306 307 const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); 308 const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); 309 310 btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); 311 btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); 312 313 btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; 314 btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; 315 btVector3 velrelOrthog = angAorthog-angBorthog; 316 { 317 //solve orthogonal angular velocity correction 318 btScalar relaxation = btScalar(1.); 319 btScalar len = velrelOrthog.length(); 320 if (len > btScalar(0.00001)) 321 { 322 btVector3 normal = velrelOrthog.normalized(); 323 btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + 324 getRigidBodyB().computeAngularImpulseDenominator(normal); 325 // scale for mass and relaxation 326 velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; 327 } 328 329 //solve angular positional correction 330 btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep); 331 btScalar len2 = angularError.length(); 332 if (len2>btScalar(0.00001)) 333 { 334 btVector3 normal2 = angularError.normalized(); 335 btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + 336 getRigidBodyB().computeAngularImpulseDenominator(normal2); 337 angularError *= (btScalar(1.)/denom2) * relaxation; 338 } 339 340 m_rbA.applyTorqueImpulse(-velrelOrthog+angularError); 341 m_rbB.applyTorqueImpulse(velrelOrthog-angularError); 342 343 // solve limit 344 if (m_solveLimit) 345 { 346 btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; 347 348 btScalar impulseMag = amplitude * m_kHinge; 349 350 // Clamp the accumulated impulse 351 btScalar temp = m_accLimitImpulse; 352 m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); 353 impulseMag = m_accLimitImpulse - temp; 354 355 356 btVector3 impulse = axisA * impulseMag * m_limitSign; 357 m_rbA.applyTorqueImpulse(impulse); 358 m_rbB.applyTorqueImpulse(-impulse); 359 } 360 } 361 362 //apply motor 363 if (m_enableAngularMotor) 364 { 365 //todo: add limits too 366 btVector3 angularLimit(0,0,0); 367 368 btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; 369 btScalar projRelVel = velrel.dot(axisA); 370 371 btScalar desiredMotorVel = m_motorTargetVelocity; 372 btScalar motor_relvel = desiredMotorVel - projRelVel; 373 374 btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; 375 //todo: should clip against accumulated impulse 376 btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; 377 clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; 378 btVector3 motorImp = clippedMotorImpulse * axisA; 379 380 m_rbA.applyTorqueImpulse(motorImp+angularLimit); 381 m_rbB.applyTorqueImpulse(-motorImp-angularLimit); 382 383 } 384 } 385 386 } 387 388 void btHingeConstraint::updateRHS(btScalar timeStep) 389 { 390 (void)timeStep; 391 392 } 393 394 btScalar btHingeConstraint::getHingeAngle() 395 { 396 const btVector3 refAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0); 397 const btVector3 refAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1); 398 const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1); 399 400 return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1) ); 401 } 402 661 return; 662 } // btHingeConstraint::testLimit() 663 664 //----------------------------------------------------------------------------- 665 //----------------------------------------------------------------------------- 666 //-----------------------------------------------------------------------------
Note: See TracChangeset
for help on using the changeset viewer.
