bool VelocityLimit()
{
const float *linear_velocity =dBodyGetLinearVel(m_body);
//limit velocity
bool ret=false;
if(dV_valid(linear_velocity))
{
dReal mag;
Fvector vlinear_velocity;vlinear_velocity.set(cast_fv(linear_velocity));
mag=_sqrt(linear_velocity[0]*linear_velocity[0]+linear_velocity[2]*linear_velocity[2]);//
if(mag>l_limit)
{
dReal f=mag/l_limit;
//dBodySetLinearVel(m_body,linear_velocity[0]/f,linear_velocity[1],linear_velocity[2]/f);///f
vlinear_velocity.x/=f;vlinear_velocity.z/=f;
ret=true;
}
mag=_abs(linear_velocity[1]);
if(mag>y_limit)
{
vlinear_velocity.y=linear_velocity[1]/mag*y_limit;
ret=true;
}
dBodySetLinearVel(m_body,vlinear_velocity.x,vlinear_velocity.y,vlinear_velocity.z);
return ret;
}
else
{
dBodySetLinearVel(m_body,m_safe_velocity[0],m_safe_velocity[1],m_safe_velocity[2]);
return true;
}
}
void CPHIsland::Repair()
{
if(!m_flags.is_active()) return;
dBodyID body;
for (body = firstbody; body; body = (dxBody *) body->next)
{
if(!dV_valid(dBodyGetAngularVel(body)))
dBodySetAngularVel(body,0.f,0.f,0.f);
if(!dV_valid(dBodyGetLinearVel(body)))
dBodySetLinearVel(body,0.f,0.f,0.f);
if(!dV_valid(dBodyGetPosition(body)))
dBodySetPosition(body,0.f,0.f,0.f);
if(!dQ_valid(dBodyGetQuaternion(body)))
{
dQuaternion q={1.f,0.f,0.f,0.f};//dQSetIdentity(q);
dBodySetQuaternion(body,q);
}
}
}
virtual void PhDataUpdate(dReal step)
{
const float *linear_velocity =dBodyGetLinearVel(m_body);
if(VelocityLimit())
{
dBodySetPosition(m_body,
m_safe_position[0]+linear_velocity[0]*fixed_step,
m_safe_position[1]+linear_velocity[1]*fixed_step,
m_safe_position[2]+linear_velocity[2]*fixed_step);
}
if(!dV_valid(dBodyGetPosition(m_body)))
dBodySetPosition(m_body,m_safe_position[0]-m_safe_velocity[0]*fixed_step,
m_safe_position[1]-m_safe_velocity[1]*fixed_step,
m_safe_position[2]-m_safe_velocity[2]*fixed_step);
dVectorSet(m_safe_position,dBodyGetPosition(m_body));
dVectorSet(m_safe_velocity,linear_velocity);
}
