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 get_box(CPhysicsShell* shell,const Fmatrix& form, Fvector& sz,Fvector& c)
{
c.set(0,0,0);
for(int i=0;3>i;++i)
{
float lo,hi;
const Fvector &ax=cast_fv(((const float*)&form+i*4));
shell->get_Extensions(ax,0,lo,hi);
sz[i]=hi-lo;c.add(Fvector().mul(ax,(lo+hi)/2));
}
}
void CPHMovementControl:: UpdateObjectBox(CPHCharacter *ach)
{
if(!m_character||!m_character->b_exist) return;
if(!ach||!ach->b_exist) return;
Fvector cbox;
PKinematics(pObject->Visual())->CalculateBones();
pObject->BoundingBox().getradius(cbox);
const Fvector &pa =cast_fv(dBodyGetPosition(ach->get_body()));
const Fvector &p =cast_fv(dBodyGetPosition(m_character->get_body()));
Fvector2 poses_dir;poses_dir.set(p.x-pa.x,p.z-pa.z);float plane_dist=poses_dir.magnitude();
if(plane_dist>2.f) return;
if(plane_dist>EPS_S)poses_dir.mul(1.f/plane_dist);
Fvector2 plane_cam;plane_cam.set(Device.vCameraDirection.x,Device.vCameraDirection.z);plane_cam.normalize_safe();
Fvector2 plane_i;plane_i.set(pObject->XFORM().i.x,pObject->XFORM().i.z);
Fvector2 plane_k;plane_k.set(pObject->XFORM().k.x,pObject->XFORM().k.z);
float R=_abs(poses_dir.dotproduct(plane_i)*cbox.x)+_abs(poses_dir.dotproduct(plane_k)*cbox.z);
R*=poses_dir.dotproduct(plane_cam); //(poses_dir.x*plane_cam.x+poses_dir.y*plane_cam.z);
Calculate(Fvector().set(0,0,0),Fvector().set(1,0,0),0,0,0,0);
m_character->SetObjectRadius(R);
ach->ChooseRestrictionType(CPHCharacter::rtStalker,1.f,m_character);
m_character->UpdateRestrictionType(ach);
}
inline dReal dcTriListCollider::FragmentonSphereTest(const dReal* center, const dReal radius,
const dReal* pt1, const dReal* pt2,dReal* norm)
{
dVector3 direction={pt2[0]-pt1[0],pt2[1]-pt1[1],pt2[2]-pt1[2]};
cast_fv(direction).normalize();
dReal center_prg=dDOT(center,direction);
dReal pt1_prg=dDOT(pt1,direction);
dReal pt2_prg=dDOT(pt2,direction);
dReal from1_dist=center_prg-pt1_prg;
if(center_prg<pt1_prg||center_prg>pt2_prg) return -1;
dVector3 line_to_center={
-pt1[0]-direction[0]*from1_dist+center[0],
-pt1[1]-direction[1]*from1_dist+center[1],
-pt1[2]-direction[2]*from1_dist+center[2]
};
float mag=dSqrt(dDOT(line_to_center,line_to_center));
//dNormalize3(norm);
dReal depth=radius-mag;
if(depth<0.f) return -1.f;
if(mag>0.f)
{
norm[0]=line_to_center[0]/mag;
norm[1]=line_to_center[1]/mag;
norm[2]=line_to_center[2]/mag;
}
else
{
norm[0]=0;
norm[1]=1;
norm[2]=0;
}
return depth;
}
void IV2XV(const IVektor &IV,Fvector &XV)
{
xm2im.transform_dir(XV),cast_fv(IV);
}
void XV2IV(const Fvector &XV,IVektor &IV)
{
xm2im.transform_dir(cast_fv(IV),XV);
}
void TContactShotMark(CDB::TRI* T,dContactGeom* c)
{
dBodyID b=dGeomGetBody(c->g1);
dxGeomUserData* data;
bool b_invert_normal=false;
if(!b)
{
b=dGeomGetBody(c->g2);
data=dGeomGetUserData(c->g2);
b_invert_normal=true;
}
else
{
data=dGeomGetUserData(c->g1);
}
if(!b) return;
dVector3 vel;
dMass m;
dBodyGetMass(b,&m);
dBodyGetPointVel(b,c->pos[0],c->pos[1],c->pos[2],vel);
dReal vel_cret=dFabs(dDOT(vel,c->normal))* _sqrt(m.mass);
Fvector to_camera;to_camera.sub(cast_fv(c->pos),Device.vCameraPosition);
float square_cam_dist=to_camera.square_magnitude();
if(data)
{
SGameMtlPair* mtl_pair = GMLib.GetMaterialPair(T->material,data->material);
if(mtl_pair)
{
//if(vel_cret>Pars.vel_cret_wallmark && !mtl_pair->CollideMarks.empty())
if(vel_cret>Pars::vel_cret_wallmark && !mtl_pair->m_pCollideMarks->empty())
{
//ref_shader pWallmarkShader = mtl_pair->CollideMarks[::Random.randI(0,mtl_pair->CollideMarks.size())];
wm_shader WallmarkShader = mtl_pair->m_pCollideMarks->GenerateWallmark();
//ref_shader pWallmarkShader = mtl_pair->CollideMarks[::Random.randI(0,mtl_pair->CollideMarks.size())];
Level().ph_commander().add_call(new CPHOnesCondition(),new CPHWallMarksCall( *((Fvector*)c->pos),T,WallmarkShader));
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
if(square_cam_dist<SQUARE_SOUND_EFFECT_DIST)
{
SGameMtl* static_mtl = GMLib.GetMaterialByIdx(T->material);
if(!static_mtl->Flags.test(SGameMtl::flPassable))
{
if(vel_cret>Pars::vel_cret_sound)
{
if(!mtl_pair->CollideSounds.empty())
{
float volume=collide_volume_min+vel_cret*(collide_volume_max-collide_volume_min)/(_sqrt(mass_limit)*default_l_limit-Pars::vel_cret_sound);
GET_RANDOM(mtl_pair->CollideSounds).play_no_feedback(0,0,0,((Fvector*)c->pos),&volume);
}
}
}
else
{
if(data->ph_ref_object&&!mtl_pair->CollideSounds.empty())
{
CPHSoundPlayer* sp=NULL;
sp=data->ph_ref_object->ph_sound_player();
if(sp) sp->Play(mtl_pair,*(Fvector*)c->pos);
}
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
if(square_cam_dist<SQUARE_PARTICLE_EFFECT_DIST)
{
if(vel_cret>Pars::vel_cret_particles && !mtl_pair->CollideParticles.empty())
{
LPCSTR ps_name = *mtl_pair->CollideParticles[::Random.randI(0,mtl_pair->CollideParticles.size())];
//отыграть партиклы столкновения материалов
Level().ph_commander().add_call(new CPHOnesCondition(),new CPHParticlesPlayCall(*c,b_invert_normal,ps_name));
}
}
}
}
}
const Fvector& CPHActivationShape:: Position ()
{
return cast_fv(dBodyGetPosition(m_body));
}
void CPHDestroyable::NotificatePart(CPHDestroyableNotificate *dn)
{
CPhysicsShell *own_shell=PPhysicsShellHolder()->PPhysicsShell() ;
CPhysicsShell *new_shell=dn->PPhysicsShellHolder()->PPhysicsShell() ;
IKinematics *own_K =smart_cast<IKinematics*>(PPhysicsShellHolder()->Visual());
IKinematics *new_K =smart_cast<IKinematics*>(dn->PPhysicsShellHolder()->Visual()) ;
VERIFY (own_K&&new_K&&own_shell&&new_shell) ;
CInifile *own_ini =own_K->LL_UserData() ;
CInifile *new_ini =new_K->LL_UserData() ;
//////////////////////////////////////////////////////////////////////////////////
Fmatrix own_transform;
own_shell ->GetGlobalTransformDynamic (&own_transform) ;
new_shell ->SetGlTransformDynamic (own_transform) ;
////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////
float random_min =1.f ;
float random_hit_imp =1.f ;
////////////////////////////////////////////////////////////////////////////////////
u16 ref_bone =own_K->LL_GetBoneRoot();
float imp_transition_factor =1.f ;
float lv_transition_factor =1.f ;
float av_transition_factor =1.f ;
////////////////////////////////////////////////////////////////////////////////////
if(own_ini&&own_ini->section_exist("impulse_transition_to_parts"))
{
random_min =own_ini->r_float("impulse_transition_to_parts","random_min");
random_hit_imp =own_ini->r_float("impulse_transition_to_parts","random_hit_imp");
////////////////////////////////////////////////////////
if(own_ini->line_exist("impulse_transition_to_parts","ref_bone"))
ref_bone =own_K->LL_BoneID(own_ini->r_string("impulse_transition_to_parts","ref_bone"));
imp_transition_factor =own_ini->r_float("impulse_transition_to_parts","imp_transition_factor");
lv_transition_factor =own_ini->r_float("impulse_transition_to_parts","lv_transition_factor");
av_transition_factor =own_ini->r_float("impulse_transition_to_parts","av_transition_factor");
if(own_ini->section_exist("collide_parts"))
{
if(own_ini->line_exist("collide_parts","small_object"))
{
new_shell->SetSmall();
}
if(own_ini->line_exist("collide_parts","ignore_small_objects"))
{
new_shell->SetIgnoreSmall();
}
}
}
if(new_ini&&new_ini->section_exist("impulse_transition_from_source_bone"))
{
//random_min =new_ini->r_float("impulse_transition_from_source_bone","random_min");
//random_hit_imp =new_ini->r_float("impulse_transition_from_source_bone","random_hit_imp");
////////////////////////////////////////////////////////
if(new_ini->line_exist("impulse_transition_from_source_bone","ref_bone"))
ref_bone =own_K->LL_BoneID(new_ini->r_string("impulse_transition_from_source_bone","ref_bone"));
imp_transition_factor =new_ini->r_float("impulse_transition_from_source_bone","imp_transition_factor");
lv_transition_factor =new_ini->r_float("impulse_transition_from_source_bone","lv_transition_factor");
av_transition_factor =new_ini->r_float("impulse_transition_from_source_bone","av_transition_factor");
}
//////////////////////////////////////////////////////////////////////////////////////////////////////
dBodyID own_body=own_shell->get_Element(ref_bone)->get_body() ;
u16 new_el_number = new_shell->get_ElementsNumber() ;
for(u16 i=0;i<new_el_number;++i)
{
CPhysicsElement* e=new_shell->get_ElementByStoreOrder(i);
float random_hit=random_min*e->getMass();
if(m_fatal_hit.is_valide() && m_fatal_hit.bone()!=BI_NONE )
{
Fvector pos;
Fmatrix m;m.set(own_K->LL_GetTransform(m_fatal_hit.bone()));
m.mulA_43 (PPhysicsShellHolder()->XFORM());
m.transform_tiny(pos,m_fatal_hit.bone_space_position());
e->applyImpulseVsGF(pos,m_fatal_hit.direction(),m_fatal_hit.phys_impulse()*imp_transition_factor);
random_hit+=random_hit_imp*m_fatal_hit.phys_impulse();
}
Fvector rnd_dir;rnd_dir.random_dir();
e->applyImpulse(rnd_dir,random_hit);
Fvector mc; mc.set(e->mass_Center());
dVector3 res_lvell;
dBodyGetPointVel(own_body,mc.x,mc.y,mc.z,res_lvell);
cast_fv(res_lvell).mul(lv_transition_factor);
e->set_LinearVel(cast_fv(res_lvell));
Fvector res_avell;res_avell.set(cast_fv(dBodyGetAngularVel(own_body)));
res_avell.mul(av_transition_factor);
e->set_AngularVel(res_avell);
}
new_shell->Enable();
new_shell->EnableCollision();
dn->PPhysicsShellHolder()->setVisible(TRUE);
dn->PPhysicsShellHolder()->setEnabled(TRUE);
if(own_shell->IsGroupObject())
new_shell->RegisterToCLGroup(own_shell->GetCLGroup());//CollideBits
CPHSkeleton* ps=dn->PPhysicsShellHolder()->PHSkeleton();
if(ps)
{
if(own_ini&&own_ini->section_exist("autoremove_parts"))
{
ps->SetAutoRemove(1000*(READ_IF_EXISTS(own_ini,r_u32,"autoremove_parts","time",ps->DefaultExitenceTime())));
}
if(new_ini&&new_ini->section_exist("autoremove"))
{
ps->SetAutoRemove(1000*(READ_IF_EXISTS(new_ini,r_u32,"autoremove","time",ps->DefaultExitenceTime())));
}
}
}
const Fvector &CPHCharacter::mass_Center () const
{
return cast_fv( dBodyGetLinearVel(m_body) );
}
bool CPHMovementControl:: ActivateBoxDynamic(DWORD id,int num_it/*=8*/,int num_steps/*5*/,float resolve_depth/*=0.01f*/)
{
bool character_exist=CharacterExist();
if(character_exist&&trying_times[id]!=u32(-1))
{
Fvector dif;dif.sub(trying_poses[id],cast_fv(dBodyGetPosition(m_character->get_body())));
if(Device.dwTimeGlobal-trying_times[id]<500&&dif.magnitude()<0.05f)
return false;
}
if(!m_character||m_character->PhysicsRefObject()->PPhysicsShell())return false;
DWORD old_id=BoxID();
bool character_disabled=character_exist && !m_character->IsEnabled();
if(character_exist&&id==old_id)return true;
if(!character_exist)
{
CreateCharacter();
}
//m_PhysicMovementControl->ActivateBox(id);
m_character->CPHObject::activate();
ph_world->Freeze();
UnFreeze();
saved_callback=ObjectContactCallback();
SetOjectContactCallback(TestDepthCallback);
SetFootCallBack(TestFootDepthCallback);
max_depth=0.f;
//////////////////////////////////pars///////////////////////////////////////////
// int num_it=8;
// int num_steps=5;
// float resolve_depth=0.01f;
if(!character_exist)
{
num_it=20;
num_steps=1;
resolve_depth=0.1f;
}
///////////////////////////////////////////////////////////////////////
float fnum_it=float(num_it);
float fnum_steps=float(num_steps);
float fnum_steps_r=1.f/fnum_steps;
Fvector vel;
Fvector pos;
GetCharacterVelocity(vel);
GetCharacterPosition(pos);
//const Fbox& box =Box();
float pass= character_exist ? _abs(Box().getradius()-boxes[id].getradius()) : boxes[id].getradius();
float max_vel=pass/2.f/fnum_it/fnum_steps/fixed_step;
float max_a_vel=M_PI/8.f/fnum_it/fnum_steps/fixed_step;
dBodySetForce(GetBody(),0.f,0.f,0.f);
dBodySetLinearVel(GetBody(),0.f,0.f,0.f);
Calculate(Fvector().set(0,0,0),Fvector().set(1,0,0),0,0,0,0);
CVelocityLimiter vl(GetBody(),max_vel,max_vel);
max_vel=1.f/fnum_it/fnum_steps/fixed_step;
bool ret=false;
m_character->SwitchOFFInitContact();
vl.Activate();
vl.l_limit*=(fnum_it*fnum_steps/5.f);
vl.y_limit=vl.l_limit;
////////////////////////////////////
for(int m=0;30>m;++m)
{
Calculate(Fvector().set(0,0,0),Fvector().set(1,0,0),0,0,0,0);
EnableCharacter();
m_character->ApplyForce(0,ph_world->Gravity()*m_character->Mass(),0);
max_depth=0.f;
ph_world->Step();
if(max_depth < resolve_depth)
{
break;
}
ph_world->CutVelocity(max_vel,max_a_vel);
}
vl.l_limit/=(fnum_it*fnum_steps/5.f);
vl.y_limit=vl.l_limit;
/////////////////////////////////////
for(int m=0;num_steps>m;++m)
{
float param =fnum_steps_r*(1+m);
InterpolateBox(id,param);
ret=false;
for(int i=0;num_it>i;++i){
max_depth=0.f;
Calculate(Fvector().set(0,0,0),Fvector().set(1,0,0),0,0,0,0);
EnableCharacter();
m_character->ApplyForce(0,ph_world->Gravity()*m_character->Mass(),0);
ph_world->Step();
ph_world->CutVelocity(max_vel,max_a_vel);
if(max_depth < resolve_depth)
{
ret=true;
break;
}
}
if(!ret) break;
}
m_character->SwitchInInitContact();
vl.Deactivate();
ph_world->UnFreeze();
if(!ret)
{
if(!character_exist)DestroyCharacter();
else if(character_disabled)m_character->Disable();
ActivateBox(old_id);
SetVelocity(vel);
dBodyID b=GetBody();
if(b)
{
dMatrix3 R;
dRSetIdentity (R);
dBodySetAngularVel(b,0.f,0.f,0.f);
dBodySetRotation(b,R);
}
SetPosition(pos);
//Msg("can not activate!");
}
else
{
ActivateBox(id);
//Msg("activate!");
}
SetOjectContactCallback(saved_callback);
SetVelocity(vel);
saved_callback=0;
if(!ret&&character_exist)
{
trying_times[id]=Device.dwTimeGlobal;
trying_poses[id].set(cast_fv(dBodyGetPosition(m_character->get_body())));
}
else
{
trying_times[id]=u32(-1);
}
return ret;
}
