bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep) { //don't simulate without timesubsteps if (m_numTimeSubSteps<1) return true; //printf("proceedDeltaTime\n"); #ifdef USE_QUICKPROF //toggle Profiler if ( m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_ProfileTimings) { if (!m_profileTimings) { m_profileTimings = 1; // To disable profiling, simply comment out the following line. static int counter = 0; char filename[128]; sprintf(filename,"quickprof_bullet_timings%i.csv",counter++); Profiler::init(filename, Profiler::BLOCK_CYCLE_SECONDS);//BLOCK_TOTAL_MICROSECONDS } } else { if (m_profileTimings) { m_profileTimings = 0; Profiler::destroy(); } } #endif //USE_QUICKPROF if (!SimdFuzzyZero(timeStep)) { { //do the kinematic calculation here, over the full timestep std::vector<CcdPhysicsController*>::iterator i; for (i=m_controllers.begin(); !(i==m_controllers.end()); i++) { CcdPhysicsController* ctrl = *i; SimdTransform predictedTrans; RigidBody* body = ctrl->GetRigidBody(); if (body->GetActivationState() != ISLAND_SLEEPING) { if (body->IsStatic()) { //to calculate velocities next frame body->saveKinematicState(timeStep); } } } } int i; float subTimeStep = timeStep / float(m_numTimeSubSteps); for (i=0;i<this->m_numTimeSubSteps;i++) { proceedDeltaTimeOneStep(subTimeStep); } } else { //todo: interpolate } return true; }
/// Perform an integration step of duration 'timeStep'. bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) { //printf("CcdPhysicsEnvironment::proceedDeltaTime\n"); if (SimdFuzzyZero(timeStep)) return true; if (m_debugDrawer) { gDisableDeactivation = (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_NoDeactivation); } #ifdef USE_QUICKPROF Profiler::beginBlock("SyncMotionStates"); #endif //USE_QUICKPROF //this is needed because scaling is not known in advance, and scaling has to propagate to the shape if (!m_scalingPropagated) { SyncMotionStates(timeStep); m_scalingPropagated = true; } #ifdef USE_QUICKPROF Profiler::endBlock("SyncMotionStates"); Profiler::beginBlock("predictIntegratedTransform"); #endif //USE_QUICKPROF { // std::vector<CcdPhysicsController*>::iterator i; int k; for (k=0;k<GetNumControllers();k++) { CcdPhysicsController* ctrl = m_controllers[k]; // SimdTransform predictedTrans; RigidBody* body = ctrl->GetRigidBody(); body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse(); if (body->IsActive()) { if (!body->IsStatic()) { body->applyForces( timeStep); body->integrateVelocities( timeStep); body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform); } } } } #ifdef USE_QUICKPROF Profiler::endBlock("predictIntegratedTransform"); #endif //USE_QUICKPROF OverlappingPairCache* scene = m_collisionWorld->GetPairCache(); // // collision detection (?) // #ifdef USE_QUICKPROF Profiler::beginBlock("DispatchAllCollisionPairs"); #endif //USE_QUICKPROF int numsubstep = m_numIterations; DispatcherInfo dispatchInfo; dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_stepCount = 0; dispatchInfo.m_enableSatConvex = m_enableSatCollisionDetection; dispatchInfo.m_debugDraw = this->m_debugDrawer; scene->RefreshOverlappingPairs(); GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo); #ifdef USE_QUICKPROF Profiler::endBlock("DispatchAllCollisionPairs"); #endif //USE_QUICKPROF int numRigidBodies = m_controllers.size(); m_islandManager->UpdateActivationState(GetCollisionWorld(),GetCollisionWorld()->GetDispatcher()); { int i; int numConstraints = m_constraints.size(); for (i=0;i< numConstraints ; i++ ) { TypedConstraint* constraint = m_constraints[i]; const RigidBody* colObj0 = &constraint->GetRigidBodyA(); const RigidBody* colObj1 = &constraint->GetRigidBodyB(); if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && ((colObj1) && ((colObj1)->mergesSimulationIslands()))) { if (colObj0->IsActive() || colObj1->IsActive()) { m_islandManager->GetUnionFind().unite((colObj0)->m_islandTag1, (colObj1)->m_islandTag1); } } } } m_islandManager->StoreIslandActivationState(GetCollisionWorld()); //contacts #ifdef USE_QUICKPROF Profiler::beginBlock("SolveConstraint"); #endif //USE_QUICKPROF //solve the regular constraints (point 2 point, hinge, etc) for (int g=0;g<numsubstep;g++) { // // constraint solving // int i; int numConstraints = m_constraints.size(); //point to point constraints for (i=0;i< numConstraints ; i++ ) { TypedConstraint* constraint = m_constraints[i]; constraint->BuildJacobian(); constraint->SolveConstraint( timeStep ); } } #ifdef USE_QUICKPROF Profiler::endBlock("SolveConstraint"); #endif //USE_QUICKPROF //solve the vehicles #ifdef NEW_BULLET_VEHICLE_SUPPORT //vehicles int numVehicles = m_wrapperVehicles.size(); for (int i=0;i<numVehicles;i++) { WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i]; RaycastVehicle* vehicle = wrapperVehicle->GetVehicle(); vehicle->UpdateVehicle( timeStep); } #endif //NEW_BULLET_VEHICLE_SUPPORT struct InplaceSolverIslandCallback : public SimulationIslandManager::IslandCallback { ContactSolverInfo& m_solverInfo; ConstraintSolver* m_solver; IDebugDraw* m_debugDrawer; InplaceSolverIslandCallback( ContactSolverInfo& solverInfo, ConstraintSolver* solver, IDebugDraw* debugDrawer) :m_solverInfo(solverInfo), m_solver(solver), m_debugDrawer(debugDrawer) { } virtual void ProcessIsland(PersistentManifold** manifolds,int numManifolds) { m_solver->SolveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer); } }; m_solverInfo.m_friction = 0.9f; m_solverInfo.m_numIterations = m_numIterations; m_solverInfo.m_timeStep = timeStep; m_solverInfo.m_restitution = 0.f;//m_restitution; InplaceSolverIslandCallback solverCallback( m_solverInfo, m_solver, m_debugDrawer); #ifdef USE_QUICKPROF Profiler::beginBlock("BuildAndProcessIslands"); #endif //USE_QUICKPROF /// solve all the contact points and contact friction m_islandManager->BuildAndProcessIslands(GetCollisionWorld()->GetDispatcher(),m_collisionWorld->GetCollisionObjectArray(),&solverCallback); #ifdef USE_QUICKPROF Profiler::endBlock("BuildAndProcessIslands"); Profiler::beginBlock("CallbackTriggers"); #endif //USE_QUICKPROF CallbackTriggers(); #ifdef USE_QUICKPROF Profiler::endBlock("CallbackTriggers"); Profiler::beginBlock("proceedToTransform"); #endif //USE_QUICKPROF { { UpdateAabbs(timeStep); float toi = 1.f; if (m_ccdMode == 3) { DispatcherInfo dispatchInfo; dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_stepCount = 0; dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS; //pairCache->RefreshOverlappingPairs();//?? GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo); toi = dispatchInfo.m_timeOfImpact; } // // integrating solution // { std::vector<CcdPhysicsController*>::iterator i; for (i=m_controllers.begin(); !(i==m_controllers.end()); i++) { CcdPhysicsController* ctrl = *i; SimdTransform predictedTrans; RigidBody* body = ctrl->GetRigidBody(); if (body->IsActive()) { if (!body->IsStatic()) { body->predictIntegratedTransform(timeStep* toi, predictedTrans); body->proceedToTransform( predictedTrans); } } } } // // disable sleeping physics objects // std::vector<CcdPhysicsController*> m_sleepingControllers; std::vector<CcdPhysicsController*>::iterator i; for (i=m_controllers.begin(); !(i==m_controllers.end()); i++) { CcdPhysicsController* ctrl = (*i); RigidBody* body = ctrl->GetRigidBody(); ctrl->UpdateDeactivation(timeStep); if (ctrl->wantsSleeping()) { if (body->GetActivationState() == ACTIVE_TAG) body->SetActivationState( WANTS_DEACTIVATION ); } else { if (body->GetActivationState() != DISABLE_DEACTIVATION) body->SetActivationState( ACTIVE_TAG ); } if (useIslands) { if (body->GetActivationState() == ISLAND_SLEEPING) { m_sleepingControllers.push_back(ctrl); } } else { if (ctrl->wantsSleeping()) { m_sleepingControllers.push_back(ctrl); } } } } #ifdef USE_QUICKPROF Profiler::endBlock("proceedToTransform"); Profiler::beginBlock("SyncMotionStates"); #endif //USE_QUICKPROF SyncMotionStates(timeStep); #ifdef USE_QUICKPROF Profiler::endBlock("SyncMotionStates"); Profiler::endProfilingCycle(); #endif //USE_QUICKPROF #ifdef NEW_BULLET_VEHICLE_SUPPORT //sync wheels for vehicles int numVehicles = m_wrapperVehicles.size(); for (int i=0;i<numVehicles;i++) { WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i]; wrapperVehicle->SyncWheels(); } #endif //NEW_BULLET_VEHICLE_SUPPORT } return true; }
bool BU_CollisionPair::calcTimeOfImpact( const SimdTransform& fromA, const SimdTransform& toA, const SimdTransform& fromB, const SimdTransform& toB, CastResult& result) { SimdVector3 linvelA,angvelA; SimdVector3 linvelB,angvelB; SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA); SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linvelB,angvelB); SimdVector3 linearMotionA = toA.getOrigin() - fromA.getOrigin(); SimdQuaternion angularMotionA(0,0,0,1.f); SimdVector3 linearMotionB = toB.getOrigin() - fromB.getOrigin(); SimdQuaternion angularMotionB(0,0,0,1); result.m_fraction = 1.f; SimdTransform impactTransA; SimdTransform impactTransB; int index=0; SimdScalar toiUnscaled=result.m_fraction; const SimdScalar toiUnscaledLimit = result.m_fraction; SimdTransform a2w; a2w = fromA; SimdTransform b2w = fromB; /* debugging code { const int numvertsB = m_convexB->GetNumVertices(); for (int v=0;v<numvertsB;v++) { SimdPoint3 pt; m_convexB->GetVertex(v,pt); pt = b2w * pt; char buf[1000]; if (pt.y() < 0.) { sprintf(buf,"PRE ERROR (%d) %.20E %.20E %.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } else { sprintf(buf,"PRE %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } } } */ SimdTransform b2wp = b2w; b2wp.setOrigin(b2w.getOrigin() + linearMotionB); b2wp.setRotation( b2w.getRotation() + angularMotionB); impactTransB = b2wp; SimdTransform a2wp; a2wp.setOrigin(a2w.getOrigin()+ linearMotionA); a2wp.setRotation(a2w.getRotation()+angularMotionA); impactTransA = a2wp; SimdTransform a2winv; a2winv = a2w.inverse(); SimdTransform b2wpinv; b2wpinv = b2wp.inverse(); SimdTransform b2winv; b2winv = b2w.inverse(); SimdTransform a2wpinv; a2wpinv = a2wp.inverse(); //Redon's version with concatenated transforms SimdTransform relative; relative = b2w * b2wpinv * a2wp * a2winv; //relative = a2winv * a2wp * b2wpinv * b2w; SimdQuaternion qrel; relative.getBasis().getRotation(qrel); SimdVector3 linvel = relative.getOrigin(); if (linvel.length() < SCREWEPSILON) { linvel.setValue(0.,0.,0.); } SimdVector3 angvel; angvel[0] = 2.f * SimdAsin (qrel[0]); angvel[1] = 2.f * SimdAsin (qrel[1]); angvel[2] = 2.f * SimdAsin (qrel[2]); if (angvel.length() < SCREWEPSILON) { angvel.setValue(0.f,0.f,0.f); } //Redon's version with concatenated transforms m_screwing = BU_Screwing(linvel,angvel); SimdTransform w2s; m_screwing.LocalMatrix(w2s); SimdTransform s2w; s2w = w2s.inverse(); //impactTransA = a2w; //impactTransB = b2w; bool hit = false; if (SimdFuzzyZero(m_screwing.GetS()) && SimdFuzzyZero(m_screwing.GetW())) { //W = 0 , S = 0 , no collision //toi = 0; /* { const int numvertsB = m_convexB->GetNumVertices(); for (int v=0;v<numvertsB;v++) { SimdPoint3 pt; m_convexB->GetVertex(v,pt); pt = impactTransB * pt; char buf[1000]; if (pt.y() < 0.) { sprintf(buf,"EARLY POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } else { sprintf(buf,"EARLY POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } } } */ return false;//don't continue moving within epsilon } #define EDGEEDGE #ifdef EDGEEDGE BU_EdgeEdge edgeEdge; //for all edged in A check agains all edges in B for (int ea = 0;ea < m_convexA->GetNumEdges();ea++) { SimdPoint3 pA0,pA1; m_convexA->GetEdge(ea,pA0,pA1); pA0= a2w * pA0;//in world space pA0 = w2s * pA0;//in screwing space pA1= a2w * pA1;//in world space pA1 = w2s * pA1;//in screwing space int numedgesB = m_convexB->GetNumEdges(); for (int eb = 0; eb < numedgesB;eb++) { { SimdPoint3 pB0,pB1; m_convexB->GetEdge(eb,pB0,pB1); pB0= b2w * pB0;//in world space pB0 = w2s * pB0;//in screwing space pB1= b2w * pB1;//in world space pB1 = w2s * pB1;//in screwing space SimdScalar lambda,mu; toiUnscaled = 1.; SimdVector3 edgeDirA(pA1-pA0); SimdVector3 edgeDirB(pB1-pB0); if (edgeEdge.GetTimeOfImpact(m_screwing,pA0,edgeDirA,pB0,edgeDirB,toiUnscaled,lambda,mu)) { //printf("edgeedge potential hit\n"); if (toiUnscaled>=0) { if (toiUnscaled < toiUnscaledLimit) { //inside check is already done by checking the mu and gamma ! SimdPoint3 vtx = pA0+lambda * (pA1-pA0); SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled); SimdPoint3 hitptWorld = s2w * hitpt; { if (toiUnscaled < result.m_fraction) result.m_fraction = toiUnscaled; hit = true; SimdVector3 hitNormal = edgeDirB.cross(edgeDirA); hitNormal = m_screwing.InBetweenVector(hitNormal,toiUnscaled); hitNormal.normalize(); //an approximated normal can be calculated by taking the cross product of both edges //take care of the sign ! SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ; SimdScalar dist = m_screwing.GetU().dot(hitNormalWorld); if (dist > 0) hitNormalWorld *= -1; //todo: this is the wrong point, because b2winv is still at begin of motion // not at time-of-impact location! //bhitpt = b2winv * hitptWorld; // m_manifold.SetContactPoint(BUM_FeatureEdgeEdge,index,ea,eb,hitptWorld,hitNormalWorld); } } } } } index++; } }; #endif //EDGEEDGE #define VERTEXFACE #ifdef VERTEXFACE // for all vertices in A, for each face in B,do vertex-face { const int numvertsA = m_convexA->GetNumVertices(); for (int v=0;v<numvertsA;v++) //int v=3; { SimdPoint3 vtx; m_convexA->GetVertex(v,vtx); vtx = a2w * vtx;//in world space vtx = w2s * vtx;//in screwing space const int numplanesB = m_convexB->GetNumPlanes(); for (int p = 0 ; p < numplanesB; p++) //int p=2; { { SimdVector3 planeNorm; SimdPoint3 planeSupport; m_convexB->GetPlane(planeNorm,planeSupport,p); planeSupport = b2w * planeSupport;//transform to world space SimdVector3 planeNormWorld = b2w.getBasis() * planeNorm; planeSupport = w2s * planeSupport ; //transform to screwing space planeNorm = w2s.getBasis() * planeNormWorld; planeNorm.normalize(); SimdScalar d = planeSupport.dot(planeNorm); SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d); BU_VertexPoly vtxApolyB; toiUnscaled = 1.; if ((p==2) && (v==6)) { // printf("%f toiUnscaled\n",toiUnscaled); } if (vtxApolyB.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,false)) { if (toiUnscaled >= 0. ) { //not only collect the first point, get every contactpoint, later we have to check the //manifold properly! if (toiUnscaled <= toiUnscaledLimit) { // printf("toiUnscaled %f\n",toiUnscaled ); SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled); SimdVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled); SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ; SimdPoint3 hitptWorld = s2w * hitpt; hitpt = b2winv * hitptWorld; //vertex has to be 'within' the facet's boundary if (m_convexB->IsInside(hitpt,m_tolerance)) { // m_manifold.SetContactPoint(BUM_FeatureVertexFace, index,v,p,hitptWorld,hitNormalWorld); if (toiUnscaled < result.m_fraction) result.m_fraction= toiUnscaled; hit = true; } } } } } index++; } } } // // for all vertices in B, for each face in A,do vertex-face //copy and pasted from all verts A -> all planes B so potential typos! //todo: make this into one method with a kind of 'swapped' logic // { const int numvertsB = m_convexB->GetNumVertices(); for (int v=0;v<numvertsB;v++) //int v=0; { SimdPoint3 vtx; m_convexB->GetVertex(v,vtx); vtx = b2w * vtx;//in world space /* char buf[1000]; if (vtx.y() < 0.) { sprintf(buf,"ERROR !!!!!!!!!\n",v,vtx.x(),vtx.y(),vtx.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } sprintf(buf,"vertexWorld(%d) = (%.20E,%.20E,%.20E)\n",v,vtx.x(),vtx.y(),vtx.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); */ vtx = w2s * vtx;//in screwing space const int numplanesA = m_convexA->GetNumPlanes(); for (int p = 0 ; p < numplanesA; p++) //int p=2; { { SimdVector3 planeNorm; SimdPoint3 planeSupport; m_convexA->GetPlane(planeNorm,planeSupport,p); planeSupport = a2w * planeSupport;//transform to world space SimdVector3 planeNormWorld = a2w.getBasis() * planeNorm; planeSupport = w2s * planeSupport ; //transform to screwing space planeNorm = w2s.getBasis() * planeNormWorld; planeNorm.normalize(); SimdScalar d = planeSupport.dot(planeNorm); SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d); BU_VertexPoly vtxBpolyA; toiUnscaled = 1.; if (vtxBpolyA.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,true)) { if (toiUnscaled>=0.) { if (toiUnscaled < toiUnscaledLimit) { SimdPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled); SimdVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled); //SimdScalar len = hitNormal.length()-1; //assert( SimdFuzzyZero(len) ); SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ; SimdPoint3 hitptWorld = s2w * hitpt; hitpt = a2winv * hitptWorld; //vertex has to be 'within' the facet's boundary if (m_convexA->IsInside(hitpt,m_tolerance)) { // m_manifold.SetContactPoint(BUM_FeatureFaceVertex,index,p,v,hitptWorld,hitNormalWorld); if (toiUnscaled <result.m_fraction) result.m_fraction = toiUnscaled; hit = true; } } } } } } index++; } } #endif// VERTEXFACE //the manifold now consists of all points/normals generated by feature-pairs that have a time-of-impact within this frame //in addition there are contact points from previous frames //we have to cleanup the manifold, using an additional epsilon/tolerance //as long as the distance from the contactpoint (in worldspace) to both objects is within this epsilon we keep the point //else throw it away if (hit) { //try to avoid numerical drift on close contact if (result.m_fraction < 0.00001) { // printf("toiUnscaledMin< 0.00001\n"); impactTransA = a2w; impactTransB = b2w; } else { //SimdScalar vel = linearMotionB.length(); //todo: check this margin result.m_fraction *= 0.99f; //move B to new position impactTransB.setOrigin(b2w.getOrigin()+ result.m_fraction*linearMotionB); SimdQuaternion ornB = b2w.getRotation()+angularMotionB*result.m_fraction; ornB.normalize(); impactTransB.setRotation(ornB); //now transform A SimdTransform a2s,a2b; a2s.mult( w2s , a2w); a2s= m_screwing.InBetweenTransform(a2s,result.m_fraction); a2s.multInverseLeft(w2s,a2s); a2b.multInverseLeft(b2w, a2s); //transform by motion B impactTransA.mult(impactTransB, a2b); //normalize rotation SimdQuaternion orn; impactTransA.getBasis().getRotation(orn); orn.normalize(); impactTransA.setBasis(SimdMatrix3x3(orn)); } } /* { const int numvertsB = m_convexB->GetNumVertices(); for (int v=0;v<numvertsB;v++) { SimdPoint3 pt; m_convexB->GetVertex(v,pt); pt = impactTransB * pt; char buf[1000]; if (pt.y() < 0.) { sprintf(buf,"POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } else { sprintf(buf,"POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } } } */ return hit; }