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
}
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
}
}