Logo Search packages:      
Sourcecode: blender version File versions

void btSequentialImpulseConstraintSolver::prepareConstraints ( btPersistentManifold *  manifoldPtr,
const btContactSolverInfo &  info,
btIDebugDraw *  debugDrawer 
) [protected]

btScalar(info.m_numIterations);

Definition at line 870 of file btSequentialImpulseConstraintSolver.cpp.

References btRigidBody::applyImpulse(), btRigidBody::computeImpulseDenominator(), btVector3::cross(), btVector3::dot(), btTransform::getBasis(), btRigidBody::getCenterOfMassPosition(), btRigidBody::getCenterOfMassTransform(), btManifoldPoint::getDistance(), btRigidBody::getInvInertiaDiagLocal(), btRigidBody::getInvInertiaTensorWorld(), btRigidBody::getInvMass(), btManifoldPoint::getLifeTime(), btManifoldPoint::getPositionWorldOnA(), btManifoldPoint::getPositionWorldOnB(), btRigidBody::getVelocityInLocalPoint(), btConstraintPersistentData::m_accumulatedTangentImpulse0, btConstraintPersistentData::m_accumulatedTangentImpulse1, btConstraintPersistentData::m_angularComponentA, btConstraintPersistentData::m_angularComponentB, btConstraintPersistentData::m_appliedImpulse, btManifoldPoint::m_combinedFriction, btManifoldPoint::m_combinedRestitution, btConstraintPersistentData::m_contactSolverFunc, btRigidBody::m_contactSolverType, btConstraintPersistentData::m_friction, btConstraintPersistentData::m_frictionAngularComponent0A, btConstraintPersistentData::m_frictionAngularComponent0B, btConstraintPersistentData::m_frictionAngularComponent1A, btConstraintPersistentData::m_frictionAngularComponent1B, btConstraintPersistentData::m_frictionSolverFunc, btRigidBody::m_frictionSolverType, btConstraintPersistentData::m_frictionWorldTangential0, btConstraintPersistentData::m_frictionWorldTangential1, btConstraintPersistentData::m_jacDiagABInv, btConstraintPersistentData::m_jacDiagABInvTangent0, btConstraintPersistentData::m_jacDiagABInvTangent1, btManifoldPoint::m_normalWorldOnB, btConstraintPersistentData::m_penetration, btConstraintPersistentData::m_persistentLifeTime, btConstraintPersistentData::m_prevAppliedImpulse, btConstraintPersistentData::m_restitution, and btManifoldPoint::m_userPersistentData.

Referenced by solveGroup().

{

      (void)debugDrawer;

      btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
      btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();


      //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
      {
            manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
            
            int numpoints = manifoldPtr->getNumContacts();

            gTotalContactPoints += numpoints;

            btVector3 color(0,1,0);
            for (int i=0;i<numpoints ;i++)
            {
                  btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
                  if (cp.getDistance() <= btScalar(0.))
                  {
                        const btVector3& pos1 = cp.getPositionWorldOnA();
                        const btVector3& pos2 = cp.getPositionWorldOnB();

                        btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); 
                        btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
                        

                        //this jacobian entry is re-used for all iterations
                        btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
                              body1->getCenterOfMassTransform().getBasis().transpose(),
                              rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
                              body1->getInvInertiaDiagLocal(),body1->getInvMass());

                        
                        btScalar jacDiagAB = jac.getDiagonal();

                        btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
                        if (cpd)
                        {
                              //might be invalid
                              cpd->m_persistentLifeTime++;
                              if (cpd->m_persistentLifeTime != cp.getLifeTime())
                              {
                                    //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
                                    new (cpd) btConstraintPersistentData;
                                    cpd->m_persistentLifeTime = cp.getLifeTime();

                              } else
                              {
                                    //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
                                    
                              }
                        } else
                        {
                                    
                              cpd = new btConstraintPersistentData;
                              assert(cpd);

                              totalCpd ++;
                              //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
                              cp.m_userPersistentData = cpd;
                              cpd->m_persistentLifeTime = cp.getLifeTime();
                              //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
                              
                        }
                        assert(cpd);

                        cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB;

                        //Dependent on Rigidbody A and B types, fetch the contact/friction response func
                        //perhaps do a similar thing for friction/restutution combiner funcs...
                        
                        cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
                        cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
                        
                        btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
                        btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
                        btVector3 vel = vel1 - vel2;
                        btScalar rel_vel;
                        rel_vel = cp.m_normalWorldOnB.dot(vel);
                        
                        btScalar combinedRestitution = cp.m_combinedRestitution;
                        
                        cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations);
                        cpd->m_friction = cp.m_combinedFriction;
                        cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
                        if (cpd->m_restitution <= btScalar(0.))
                        {
                              cpd->m_restitution = btScalar(0.0);

                        };
                        
                        //restitution and penetration work in same direction so
                        //rel_vel 
                        
                        btScalar penVel = -cpd->m_penetration/info.m_timeStep;

                        if (cpd->m_restitution > penVel)
                        {
                              cpd->m_penetration = btScalar(0.);
                        }                       
                        
                        

                        btScalar relaxation = info.m_damping;
                        if (m_solverMode & SOLVER_USE_WARMSTARTING)
                        {
                              cpd->m_appliedImpulse *= relaxation;
                        } else
                        {
                              cpd->m_appliedImpulse =btScalar(0.);
                        }
      
                        //for friction
                        cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
                        
                        //re-calculate friction direction every frame, todo: check if this is really needed
                        btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);


#define NO_FRICTION_WARMSTART 1

      #ifdef NO_FRICTION_WARMSTART
                        cpd->m_accumulatedTangentImpulse0 = btScalar(0.);
                        cpd->m_accumulatedTangentImpulse1 = btScalar(0.);
      #endif //NO_FRICTION_WARMSTART
                        btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
                        btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
                        btScalar denom = relaxation/(denom0+denom1);
                        cpd->m_jacDiagABInvTangent0 = denom;


                        denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
                        denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
                        denom = relaxation/(denom0+denom1);
                        cpd->m_jacDiagABInvTangent1 = denom;

                        btVector3 totalImpulse = 
      #ifndef NO_FRICTION_WARMSTART
                              cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
                              cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
      #endif //NO_FRICTION_WARMSTART
                              cp.m_normalWorldOnB*cpd->m_appliedImpulse;



                        ///
                        {
                        btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
                        cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
                        btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);            
                        cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
                        }
                        {
                              btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
                              cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
                        }
                        {
                              btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
                              cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
                        }
                        {
                              btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
                              cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
                        }
                        {
                              btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
                              cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
                        }
                        
                        ///



                        //apply previous frames impulse on both bodies
                        body0->applyImpulse(totalImpulse, rel_pos1);
                        body1->applyImpulse(-totalImpulse, rel_pos2);
                  }
                  
            }
      }
}


Generated by  Doxygen 1.6.0   Back to index