- Timestamp:
- Mar 31, 2009, 8:05:51 PM (15 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
code/trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
r2662 r2882 30 30 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" 31 31 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" 32 #include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" 33 #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h" 34 #include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" 35 #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" 36 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" 32 37 33 38 //for debug rendering … … 47 52 48 53 49 50 //vehicle 51 #include "BulletDynamics/Vehicle/btRaycastVehicle.h" 52 #include "BulletDynamics/Vehicle/btVehicleRaycaster.h" 53 #include "BulletDynamics/Vehicle/btWheelInfo.h" 54 //character 55 #include "BulletDynamics/Character/btCharacterControllerInterface.h" 56 57 #include "LinearMath/btIDebugDraw.h" 54 #include "BulletDynamics/Dynamics/btActionInterface.h" 58 55 #include "LinearMath/btQuickprof.h" 59 56 #include "LinearMath/btMotionState.h" … … 114 111 if (body) 115 112 { 116 btTransform predictedTrans;117 113 if (body->getActivationState() != ISLAND_SLEEPING) 118 114 { … … 149 145 } 150 146 } 147 bool drawConstraints = false; 148 if (getDebugDrawer()) 149 { 150 int mode = getDebugDrawer()->getDebugMode(); 151 if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) 152 { 153 drawConstraints = true; 154 } 155 } 156 if(drawConstraints) 157 { 158 for(int i = getNumConstraints()-1; i>=0 ;i--) 159 { 160 btTypedConstraint* constraint = getConstraint(i); 161 debugDrawConstraint(constraint); 162 } 163 } 164 151 165 152 166 … … 191 205 } 192 206 193 for ( i=0;i<this->m_vehicles.size();i++) 194 { 195 for (int v=0;v<m_vehicles[i]->getNumWheels();v++) 196 { 197 btVector3 wheelColor(0,255,255); 198 if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact) 199 { 200 wheelColor.setValue(0,0,255); 201 } else 202 { 203 wheelColor.setValue(255,0,255); 204 } 205 206 btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin(); 207 208 btVector3 axle = btVector3( 209 m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()], 210 m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()], 211 m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]); 212 213 214 //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS 215 //debug wheels (cylinders) 216 m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor); 217 m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor); 218 207 if (getDebugDrawer() && getDebugDrawer()->getDebugMode()) 208 { 209 for (i=0;i<m_actions.size();i++) 210 { 211 m_actions[i]->debugDraw(m_debugDrawer); 219 212 } 220 213 } … … 288 281 } 289 282 } 290 283 /* 291 284 if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) 292 285 { … … 300 293 } 301 294 } 295 */ 296 302 297 303 298 } … … 340 335 if (getDebugDrawer()) 341 336 { 342 gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; 337 btIDebugDraw* debugDrawer = getDebugDrawer (); 338 gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; 343 339 } 344 340 if (numSimulationSubSteps) … … 404 400 405 401 ///update vehicle simulation 406 updateVehicles(timeStep); 407 408 updateCharacters(timeStep); 409 402 updateActions(timeStep); 403 410 404 updateActivationState( timeStep ); 411 405 … … 471 465 472 466 473 void btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep) 474 { 475 BT_PROFILE("updateVehicles"); 476 477 for ( int i=0;i<m_vehicles.size();i++) 478 { 479 btRaycastVehicle* vehicle = m_vehicles[i]; 480 vehicle->updateVehicle( timeStep); 481 } 482 } 483 484 void btDiscreteDynamicsWorld::updateCharacters(btScalar timeStep) 485 { 486 BT_PROFILE("updateCharacters"); 487 488 for ( int i=0;i<m_characters.size();i++) 489 { 490 btCharacterControllerInterface* character = m_characters[i]; 491 character->preStep (this); 492 character->playerStep (this,timeStep); 493 } 494 } 495 467 void btDiscreteDynamicsWorld::updateActions(btScalar timeStep) 468 { 469 BT_PROFILE("updateActions"); 470 471 for ( int i=0;i<m_actions.size();i++) 472 { 473 m_actions[i]->updateAction( this, timeStep); 474 } 475 } 496 476 497 477 … … 550 530 } 551 531 552 void btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle) 553 { 554 m_vehicles.push_back(vehicle); 555 } 556 557 void btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle) 558 { 559 m_vehicles.remove(vehicle); 560 } 561 562 void btDiscreteDynamicsWorld::addCharacter(btCharacterControllerInterface* character) 563 { 564 m_characters.push_back(character); 565 } 566 567 void btDiscreteDynamicsWorld::removeCharacter(btCharacterControllerInterface* character) 568 { 569 m_characters.remove(character); 532 void btDiscreteDynamicsWorld::addAction(btActionInterface* action) 533 { 534 m_actions.push_back(action); 535 } 536 537 void btDiscreteDynamicsWorld::removeAction(btActionInterface* action) 538 { 539 m_actions.remove(action); 540 } 541 542 543 void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle) 544 { 545 addAction(vehicle); 546 } 547 548 void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle) 549 { 550 removeAction(vehicle); 551 } 552 553 void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character) 554 { 555 addAction(character); 556 } 557 558 void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character) 559 { 560 removeAction(character); 570 561 } 571 562 … … 643 634 if (islandId<0) 644 635 { 645 ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id 646 m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); 636 if (numManifolds + m_numConstraints) 637 { 638 ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id 639 m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); 640 } 647 641 } else 648 642 { … … 690 684 } 691 685 692 // assert(0);686 // btAssert(0); 693 687 694 688 … … 754 748 btScalar m_allowedPenetration; 755 749 btOverlappingPairCache* m_pairCache; 750 btDispatcher* m_dispatcher; 756 751 757 752 758 753 public: 759 btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache ) :754 btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : 760 755 btCollisionWorld::ClosestConvexResultCallback(fromA,toA), 761 756 m_allowedPenetration(0.0f), 762 757 m_me(me), 763 m_pairCache(pairCache) 758 m_pairCache(pairCache), 759 m_dispatcher(dispatcher) 764 760 { 765 761 } … … 796 792 return false; 797 793 798 ///don't do CCD when there are already contact points (touching contact/penetration) 799 btAlignedObjectArray<btPersistentManifold*> manifoldArray; 800 btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0); 801 if (collisionPair) 802 { 803 if (collisionPair->m_algorithm) 804 { 805 manifoldArray.resize(0); 806 collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); 807 for (int j=0;j<manifoldArray.size();j++) 808 { 809 btPersistentManifold* manifold = manifoldArray[j]; 810 if (manifold->getNumContacts()>0) 811 return false; 794 btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; 795 796 //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179 797 if (m_dispatcher->needsResponse(m_me,otherObj)) 798 { 799 ///don't do CCD when there are already contact points (touching contact/penetration) 800 btAlignedObjectArray<btPersistentManifold*> manifoldArray; 801 btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0); 802 if (collisionPair) 803 { 804 if (collisionPair->m_algorithm) 805 { 806 manifoldArray.resize(0); 807 collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); 808 for (int j=0;j<manifoldArray.size();j++) 809 { 810 btPersistentManifold* manifold = manifoldArray[j]; 811 if (manifold->getNumContacts()>0) 812 return false; 813 } 812 814 } 813 815 } … … 847 849 gNumClampedCcdMotions++; 848 850 849 btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache() );851 btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); 850 852 btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); 851 853 btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); … … 1188 1190 1189 1191 1192 void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) 1193 { 1194 bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0; 1195 bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0; 1196 btScalar dbgDrawSize = constraint->getDbgDrawSize(); 1197 if(dbgDrawSize <= btScalar(0.f)) 1198 { 1199 return; 1200 } 1201 1202 switch(constraint->getConstraintType()) 1203 { 1204 case POINT2POINT_CONSTRAINT_TYPE: 1205 { 1206 btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint; 1207 btTransform tr; 1208 tr.setIdentity(); 1209 btVector3 pivot = p2pC->getPivotInA(); 1210 pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; 1211 tr.setOrigin(pivot); 1212 getDebugDrawer()->drawTransform(tr, dbgDrawSize); 1213 // that ideally should draw the same frame 1214 pivot = p2pC->getPivotInB(); 1215 pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; 1216 tr.setOrigin(pivot); 1217 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); 1218 } 1219 break; 1220 case HINGE_CONSTRAINT_TYPE: 1221 { 1222 btHingeConstraint* pHinge = (btHingeConstraint*)constraint; 1223 btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame(); 1224 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); 1225 tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame(); 1226 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); 1227 btScalar minAng = pHinge->getLowerLimit(); 1228 btScalar maxAng = pHinge->getUpperLimit(); 1229 if(minAng == maxAng) 1230 { 1231 break; 1232 } 1233 bool drawSect = true; 1234 if(minAng > maxAng) 1235 { 1236 minAng = btScalar(0.f); 1237 maxAng = SIMD_2_PI; 1238 drawSect = false; 1239 } 1240 if(drawLimits) 1241 { 1242 btVector3& center = tr.getOrigin(); 1243 btVector3 normal = tr.getBasis().getColumn(2); 1244 btVector3 axis = tr.getBasis().getColumn(0); 1245 getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect); 1246 } 1247 } 1248 break; 1249 case CONETWIST_CONSTRAINT_TYPE: 1250 { 1251 btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint; 1252 btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); 1253 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); 1254 tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); 1255 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); 1256 if(drawLimits) 1257 { 1258 //const btScalar length = btScalar(5); 1259 const btScalar length = dbgDrawSize; 1260 static int nSegments = 8*4; 1261 btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments); 1262 btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length); 1263 pPrev = tr * pPrev; 1264 for (int i=0; i<nSegments; i++) 1265 { 1266 fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments); 1267 btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length); 1268 pCur = tr * pCur; 1269 getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0)); 1270 1271 if (i%(nSegments/8) == 0) 1272 getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0)); 1273 1274 pPrev = pCur; 1275 } 1276 btScalar tws = pCT->getTwistSpan(); 1277 btScalar twa = pCT->getTwistAngle(); 1278 bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f)); 1279 if(useFrameB) 1280 { 1281 tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); 1282 } 1283 else 1284 { 1285 tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); 1286 } 1287 btVector3 pivot = tr.getOrigin(); 1288 btVector3 normal = tr.getBasis().getColumn(0); 1289 btVector3 axis1 = tr.getBasis().getColumn(1); 1290 getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true); 1291 1292 } 1293 } 1294 break; 1295 case D6_CONSTRAINT_TYPE: 1296 { 1297 btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint; 1298 btTransform tr = p6DOF->getCalculatedTransformA(); 1299 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); 1300 tr = p6DOF->getCalculatedTransformB(); 1301 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); 1302 if(drawLimits) 1303 { 1304 tr = p6DOF->getCalculatedTransformA(); 1305 const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin(); 1306 btVector3 up = tr.getBasis().getColumn(2); 1307 btVector3 axis = tr.getBasis().getColumn(0); 1308 btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit; 1309 btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit; 1310 btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; 1311 btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; 1312 getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0)); 1313 axis = tr.getBasis().getColumn(1); 1314 btScalar ay = p6DOF->getAngle(1); 1315 btScalar az = p6DOF->getAngle(2); 1316 btScalar cy = btCos(ay); 1317 btScalar sy = btSin(ay); 1318 btScalar cz = btCos(az); 1319 btScalar sz = btSin(az); 1320 btVector3 ref; 1321 ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2]; 1322 ref[1] = -sz*axis[0] + cz*axis[1]; 1323 ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2]; 1324 tr = p6DOF->getCalculatedTransformB(); 1325 btVector3 normal = -tr.getBasis().getColumn(0); 1326 btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; 1327 btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit; 1328 if(minFi > maxFi) 1329 { 1330 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false); 1331 } 1332 else if(minFi < maxFi) 1333 { 1334 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true); 1335 } 1336 tr = p6DOF->getCalculatedTransformA(); 1337 btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit; 1338 btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit; 1339 getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0)); 1340 } 1341 } 1342 break; 1343 case SLIDER_CONSTRAINT_TYPE: 1344 { 1345 btSliderConstraint* pSlider = (btSliderConstraint*)constraint; 1346 btTransform tr = pSlider->getCalculatedTransformA(); 1347 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); 1348 tr = pSlider->getCalculatedTransformB(); 1349 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); 1350 if(drawLimits) 1351 { 1352 btTransform tr = pSlider->getCalculatedTransformA(); 1353 btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f); 1354 btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f); 1355 getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0)); 1356 btVector3 normal = tr.getBasis().getColumn(0); 1357 btVector3 axis = tr.getBasis().getColumn(1); 1358 btScalar a_min = pSlider->getLowerAngLimit(); 1359 btScalar a_max = pSlider->getUpperAngLimit(); 1360 const btVector3& center = pSlider->getCalculatedTransformB().getOrigin(); 1361 getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true); 1362 } 1363 } 1364 break; 1365 default : 1366 break; 1367 } 1368 return; 1369 } // btDiscreteDynamicsWorld::debugDrawConstraint() 1370 1371 1372 1373 1374 1190 1375 void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) 1191 1376 { … … 1216 1401 return m_constraints[index]; 1217 1402 } 1403 1404
Note: See TracChangeset
for help on using the changeset viewer.
