virtual void	stepSimulation(float deltaTime)
    {
#ifndef BT_NO_PROFILE
		CProfileManager::Reset();
#endif
		
		
		
		void* myUserPtr = 0;
                    
		gTotalPoints = 0;
		numNearCallbacks = 0;
		{
			BT_PROFILE("plWorldCollide");
			if (m_collisionSdkHandle && m_collisionWorldHandle)
			{
				plWorldCollide(m_collisionSdkHandle,m_collisionWorldHandle,myNearCallback, myUserPtr);
			}
		}

#if 0
		switch (m_tutorialIndex)
		{
			case TUT_SPHERE_SPHERE:
			{
				if (m_timeSeriesCanvas0)
				{
					float xPos = 0.f;
					float xVel = 1.f;
					m_timeSeriesCanvas0->insertDataAtCurrentTime(xPos,0,true);
					m_timeSeriesCanvas0->insertDataAtCurrentTime(xVel,1,true);
				}
				break;
			}
			

			default:
			{
			}
			
		};
#endif
		
		if (m_timeSeriesCanvas0)
			m_timeSeriesCanvas0->nextTick();
		
		
			
		//	m_app->m_renderer->writeSingleInstanceTransformToCPU(m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation, m_bodies[i]->m_graphicsIndex);
	
	
		 m_app->m_renderer->writeTransforms();
#ifndef BT_NO_PROFILE
		 CProfileManager::Increment_Frame_Counter();
#endif
    }
示例#2
0
    virtual void	stepSimulation(float deltaTime)
    {
		switch (m_tutorialIndex)
		{
			case TUT_VELOCITY:
			{
				tutorial1Update(deltaTime);
				float xPos = m_bodies[0]->m_worldPose.m_position.x;
				float xVel = m_bodies[0]->m_linearVelocity.x;
				m_timeSeriesCanvas0->insertDataAtCurrentTime(xPos,0,true);
				m_timeSeriesCanvas0->insertDataAtCurrentTime(xVel,1,true);
				break;
			}
			case TUT_ACCELERATION:
			{
				tutorial2Update(deltaTime);
				float yPos = m_bodies[0]->m_worldPose.m_position.y;
				float yVel = m_bodies[0]->m_linearVelocity.y;
				m_timeSeriesCanvas1->insertDataAtCurrentTime(yPos,0,true);
				m_timeSeriesCanvas1->insertDataAtCurrentTime(yVel,1,true);
				
				break;
			}
			case TUT_COLLISION:
			{
				m_contactPoints.clear();
				LWContactPoint contactPoint;
				tutorialCollisionUpdate(deltaTime, contactPoint);
				m_contactPoints.push_back(contactPoint);
				m_timeSeriesCanvas1->insertDataAtCurrentTime(contactPoint.m_distance,0,true);
				
				break;
			}
			case TUT_SOLVE_CONTACT_CONSTRAINT:
			{
				m_contactPoints.clear();
				LWContactPoint contactPoint;
				tutorialSolveContactConstraintUpdate(deltaTime, contactPoint);
				m_contactPoints.push_back(contactPoint);
				if (contactPoint.m_distance<0)
				{
					m_bodies[0]->computeInvInertiaTensorWorld();
					m_bodies[1]->computeInvInertiaTensorWorld();
					
					b3Scalar appliedImpulse = resolveCollision(*m_bodies[0],
									 *m_bodies[1],
									 contactPoint
									 );
				
					m_timeSeriesCanvas1->insertDataAtCurrentTime(appliedImpulse,1,true);
					
				} else
				{
					m_timeSeriesCanvas1->insertDataAtCurrentTime(0.,1,true);
				}
				m_timeSeriesCanvas1->insertDataAtCurrentTime(contactPoint.m_distance,0,true);
				
				break;
			}

			default:
			{
			}
			
		};
		
		
		if (m_timeSeriesCanvas0)
			m_timeSeriesCanvas0->nextTick();
		
		if (m_timeSeriesCanvas1)
			m_timeSeriesCanvas1->nextTick();

		
		for (int i=0;i<m_bodies.size();i++)
		{
			
			m_bodies[i]->integrateAcceleration(deltaTime);
			m_bodies[i]->integrateVelocity(deltaTime);
			
			m_app->m_renderer->writeSingleInstanceTransformToCPU(m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation, m_bodies[i]->m_graphicsIndex);
		}

		
		 m_app->m_renderer->writeTransforms();
    }
void InverseDynamicsExample::stepSimulation(float deltaTime)
{
    if(m_multiBody) {
        const int num_dofs=m_multiBody->getNumDofs();
        btInverseDynamics::vecx nu(num_dofs), qdot(num_dofs), q(num_dofs),joint_force(num_dofs);
        btInverseDynamics::vecx pd_control(num_dofs);

        // compute joint forces from one of two control laws:
        // 1) "computed torque" control, which gives perfect, decoupled,
        //    linear second order error dynamics per dof in case of a
        //    perfect model and (and negligible time discretization effects)
        // 2) decoupled PD control per joint, without a model
        for(int dof=0;dof<num_dofs;dof++) {
            q(dof) = m_multiBody->getJointPos(dof);
            qdot(dof)= m_multiBody->getJointVel(dof);

            const btScalar qd_dot=0;
            const btScalar qd_ddot=0;
			if (m_timeSeriesCanvas)
	            m_timeSeriesCanvas->insertDataAtCurrentTime(q[dof],dof,true);
            
            // pd_control is either desired joint torque for pd control,
            // or the feedback contribution to nu
            pd_control(dof) = kd*(qd_dot-qdot(dof)) + kp*(qd[dof]-q(dof));
            // nu is the desired joint acceleration for computed torque control
            nu(dof) = qd_ddot + pd_control(dof);

        }
         if(useInverseModel)
         {
             // calculate joint forces corresponding to desired accelerations nu
            if (m_multiBody->hasFixedBase())
             {
                 if(-1 != m_inverseModel->calculateInverseDynamics(q,qdot,nu,&joint_force))
                 {
                     //joint_force(dof) += damping*dot_q(dof);
                     // use inverse model: apply joint force corresponding to
                     // desired acceleration nu
                     
                     for(int dof=0;dof<num_dofs;dof++)
                     {
                         m_multiBody->addJointTorque(dof,joint_force(dof));
                     }
                 }
                 
             } else
             {
                 //the inverse dynamics model represents the 6 DOFs of the base, unlike btMultiBody.
                 //append some dummy values to represent the 6 DOFs of the base
                 btInverseDynamics::vecx nu6(num_dofs+6), qdot6(num_dofs+6), q6(num_dofs+6),joint_force6(num_dofs+6);
                 for (int i=0;i<num_dofs;i++)
                 {
                     nu6[6+i] = nu[i];
                     qdot6[6+i] = qdot[i];
                     q6[6+i] = q[i];
                     joint_force6[6+i] = joint_force[i];
                 }
                 if(-1 != m_inverseModel->calculateInverseDynamics(q6,qdot6,nu6,&joint_force6))
                 {
                     //joint_force(dof) += damping*dot_q(dof);
                     // use inverse model: apply joint force corresponding to
                     // desired acceleration nu
                     
                     for(int dof=0;dof<num_dofs;dof++)
                     {
                         m_multiBody->addJointTorque(dof,joint_force6(dof+6));
                     }
                 }

             }
            
             
         } else
         {
             for(int dof=0;dof<num_dofs;dof++)
             {
                 // no model: just apply PD control law
                 m_multiBody->addJointTorque(dof,pd_control(dof));
             }
        }
    }

    if (m_timeSeriesCanvas)
        m_timeSeriesCanvas->nextTick();

    //todo: joint damping for btMultiBody, tune parameters
    
    // step the simulation
    if (m_dynamicsWorld)
    {
        // todo(thomas) check that this is correct:
        // want to advance by 10ms, with 1ms timesteps
        m_dynamicsWorld->stepSimulation(1e-3,0);//,1e-3);
		btAlignedObjectArray<btQuaternion> scratch_q;
		btAlignedObjectArray<btVector3> scratch_m;
		m_multiBody->forwardKinematics(scratch_q, scratch_m);
#if 0
		for (int i = 0; i < m_multiBody->getNumLinks(); i++)
		{
			//btVector3 pos = m_multiBody->getLink(i).m_cachedWorldTransform.getOrigin();
			btTransform tr = m_multiBody->getLink(i).m_cachedWorldTransform;
			btVector3 pos = tr.getOrigin() - quatRotate(tr.getRotation(), m_multiBody->getLink(i).m_dVector);
			btVector3 localAxis = m_multiBody->getLink(i).m_axes[0].m_topVec;
			//printf("link %d: %f,%f,%f, local axis:%f,%f,%f\n", i, pos.x(), pos.y(), pos.z(), localAxis.x(), localAxis.y(), localAxis.z());

			


		}
#endif
    }
}