void CIKLimb::Calculate(CKinematics* K,const Fmatrix &obj)
{
SCalculateData cd(this,K,obj);
m_prev_state_anim=true;
Collide(&cd);
if(cd.m_tri)
{
Matrix m ;
GoalMatrix (m,&cd) ;
#ifdef DEBUG
if(m_limb.SetGoal(m,ph_dbg_draw_mask.test(phDbgIKLimits)))
#else
if(m_limb.SetGoal(m,TRUE))
#endif
{
Fmatrix hip;
CBoneData& BD=K->LL_GetData(m_bones[0]);
hip.mul_43(K->LL_GetTransform(BD.GetParentID()),BD.bind_transform);
hip.invert();
float x[7];
Fvector pos;
pos.set(K->LL_GetTransform(m_bones[1]).c);
hip.transform_tiny(pos);
xm2im.transform_tiny(pos);
if(m_limb.SolveByPos(cast_fp(pos),x))
{
cd.m_angles=x;
CalculateBones(&cd);
m_prev_state_anim=false;
}
}
}
}
void CCylinderGeom::get_extensions_bt(const Fvector& axis,float center_prg,float& lo_ext, float& hi_ext)
{
VERIFY (m_geom_transform) ;
const dReal *rot =NULL ;
const dReal *pos =NULL ;
dVector3 p ;
dMatrix3 r ;
dGeomID g =geometry_bt() ;
get_final_tx_bt(pos,rot,p,r) ;
GetCylinderExtensions(g,cast_fp(axis),pos,rot,center_prg,&lo_ext,&hi_ext);
}
void CCharacterPhysicsSupport:: RemoveActiveWeaponCollision ()
{
VERIFY( m_pPhysicsShell );
VERIFY( m_weapon_attach_bone );
VERIFY( !m_weapon_geoms.empty() );
xr_vector<CODEGeom*>::iterator ii =m_weapon_geoms.begin(), ee = m_weapon_geoms.end();
Fmatrix m0;
(*ii)->get_xform( m0 );
CPhysicsElement* root = m_active_item_obj->PPhysicsShell()->get_ElementByStoreOrder( 0 );
CODEGeom *rg = root->geometry( 0 );
VERIFY( rg );
Fmatrix m1;
rg->get_xform( m1 );
Fmatrix me;
root->GetGlobalTransformDynamic( &me );
Fmatrix m1_to_e = Fmatrix().mul_43( Fmatrix().invert( m1 ), me );
Fmatrix m0e = Fmatrix().mul_43( m0, m1_to_e );
root->SetTransform( m0e );
for( ;ii!=ee; ++ii )
{
CODEGeom *g =(*ii);
//g->dbg_draw( 0.01f, D3DCOLOR_XRGB( 0, 0, 255 ), Flags32() );
m_weapon_attach_bone->remove_geom( g );
g->destroy();
xr_delete(g);
}
//m_active_item_obj->PPhysicsShell()->dbg_draw_geometry( 0.2f, D3DCOLOR_XRGB( 255, 0, 100 ) );
Fvector a_vel, l_vel;
const Fvector& mc = root->mass_Center();
dBodyGetPointVel( m_weapon_attach_bone->get_body(),mc.x, mc.y, mc.z, cast_fp(l_vel) );
m_weapon_attach_bone->get_AngularVel( a_vel );
root->set_AngularVel( a_vel );
root->set_LinearVel( l_vel );
m_weapon_geoms.clear();
m_weapon_attach_bone = 0;
m_active_item_obj = 0;
bone_fix_clear();
}
void CPHActivationShape:: Size (Fvector &size)
{
dGeomBoxGetLengths(m_geom,cast_fp(size));
}
bool CPHActivationShape:: Activate (const Fvector need_size,u16 steps,float max_displacement,float max_rotation,bool un_freeze_later/* =false*/)
{
#ifdef DEBUG
if(debug_output().ph_dbg_draw_mask().test(phDbgDrawDeathActivationBox))
{
debug_output().DBG_OpenCashedDraw();
Fmatrix M;
PHDynamicData::DMXPStoFMX(dBodyGetRotation(m_body),dBodyGetPosition(m_body),M);
Fvector v;dGeomBoxGetLengths(m_geom,cast_fp(v));v.mul(0.5f);
debug_output().DBG_DrawOBB(M,v,D3DCOLOR_XRGB(0,255,0));
}
#endif
VERIFY(m_geom&&m_body);
CPHObject::activate();
ph_world->Freeze();
UnFreeze();
max_depth=0.f;
dGeomUserDataSetObjectContactCallback(m_geom,GetMaxDepthCallback) ;
//ph_world->Step();
ph_world->StepTouch();
u16 num_it =15;
float fnum_it=float(num_it);
float fnum_steps=float(steps);
float fnum_steps_r=1.f/fnum_steps;
float resolve_depth=0.01f;
float max_vel=max_depth/fnum_it*fnum_steps_r/fixed_step;
float limit_l_vel=_max(_max(need_size.x,need_size.y),need_size.z)/fnum_it*fnum_steps_r/fixed_step;
if(limit_l_vel>default_l_limit)
limit_l_vel=default_l_limit;
if(max_vel>limit_l_vel)
max_vel=limit_l_vel;
float max_a_vel=max_rotation/fnum_it*fnum_steps_r/fixed_step;
if(max_a_vel>default_w_limit)
max_a_vel=default_w_limit;
//ph_world->CutVelocity(0.f,0.f);
dGeomUserDataSetCallbackData(m_geom,this);
dGeomUserDataSetObjectContactCallback( m_geom, ActivateTestDepthCallback );
if( m_flags.test( flStaticEnvironment ) )
dGeomUserDataAddObjectContactCallback(m_geom,StaticEnvironment);
max_depth=0.f;
Fvector from_size;
Fvector step_size,size;
dGeomBoxGetLengths(m_geom,cast_fp(from_size));
step_size.sub(need_size,from_size);
step_size.mul(fnum_steps_r);
size.set(from_size);
bool ret=false;
V_PH_WORLD_STATE temp_state;
ph_world->GetState(temp_state);
for(int m=0;steps>m;++m)
{
//float param =fnum_steps_r*(1+m);
//InterpolateBox(id,param);
size.add(step_size);
dGeomBoxSetLengths(m_geom,size.x,size.y,size.z);
u16 attempts=10;
do{
ret=false;
for(int i=0;num_it>i;++i)
{
max_depth=0.f;
ph_world->Step();
CHECK_POS(Position(),"pos after ph_world->Step()",false);
ph_world->CutVelocity(max_vel,max_a_vel);
CHECK_POS(Position(),"pos after CutVelocity",true);
//if(m==0&&i==0)ph_world->GetState(temp_state);
if(max_depth < resolve_depth)
{
ret=true;
break;
}
}
attempts--;
}while(!ret&&attempts>0);
#ifdef DEBUG
// Msg("correction attempts %d",10-attempts);
#endif
}
RestoreVelocityState(temp_state);
CHECK_POS(Position(),"pos after RestoreVelocityState(temp_state);",true);
if(!un_freeze_later)ph_world->UnFreeze();
#ifdef DEBUG
if(debug_output().ph_dbg_draw_mask().test(phDbgDrawDeathActivationBox))
{
debug_output().DBG_OpenCashedDraw();
Fmatrix M;
PHDynamicData::DMXPStoFMX(dBodyGetRotation(m_body),dBodyGetPosition(m_body),M);
Fvector v;v.set(need_size);v.mul(0.5f);
debug_output().DBG_DrawOBB(M,v,D3DCOLOR_XRGB(0,255,255));
debug_output().DBG_ClosedCashedDraw(30000);
}
#endif
return ret;
}