void CPHActivationShape::CutVelocity(float l_limit,float /*a_limit*/)
{
dVector3 limitedl,diffl;
if(dVectorLimit(dBodyGetLinearVel(m_body),l_limit,limitedl))
{
dVectorSub(diffl,limitedl,dBodyGetLinearVel(m_body));
dBodySetLinearVel(m_body,diffl[0],diffl[1],diffl[2]);
dBodySetAngularVel(m_body,0.f,0.f,0.f);
dxStepBody(m_body,fixed_step);
dBodySetLinearVel(m_body,limitedl[0],limitedl[1],limitedl[2]);
}
}
virtual void PhDataUpdate(dReal step)
{
int num=dBodyGetNumJoints(m_body);
for(int i=0;i<num;i++)
{
dJointID joint=dBodyGetJoint(m_body,i);
if(dJointGetType(joint)==dJointTypeContact)
{
dJointFeedback* feedback=dJointGetFeedback(joint);
R_ASSERT2(feedback,"Feedback was not set!!!");
dxJoint* b_joint=(dxJoint*) joint;
dBodyID other_body=b_joint->node[1].body;
bool b_body_second=(b_joint->node[1].body==m_body);
dReal* self_force=feedback->f1;
dReal* self_torque=feedback->t1;
dReal* othrers_force=feedback->f2;
dReal* othrers_torque=feedback->t2;
if(b_body_second)
{
other_body=b_joint->node[0].body;
self_force=feedback->f2;
self_torque=feedback->t2;
othrers_force=feedback->f1;
othrers_torque=feedback->t1;
}
save_max(m_max_force_self,_sqrt(dDOT( self_force,self_force)));
save_max(m_max_torque_self,_sqrt(dDOT( self_torque,self_torque)));
save_max(m_max_force_self_y,_abs(self_force[1]));
save_max(m_max_force_self_sd,_sqrt(self_force[0]*self_force[0]+self_force[2]*self_force[2]));
if(other_body)
{
dVector3 shoulder;dVectorSub(shoulder,dJointGetPositionContact(joint),dBodyGetPosition(other_body));
dReal shoulder_lenght=_sqrt(dDOT(shoulder,shoulder));
save_max(m_max_force_others,_sqrt(dDOT( othrers_force,othrers_force)));
if(!fis_zero(shoulder_lenght))
save_max(m_max_torque_others,_sqrt(dDOT( othrers_torque,othrers_torque))/shoulder_lenght);
}
}
}
}
