CPhysicsShell* P_build_SimpleShell(CGameObject* obj,float mass,bool not_active_state)
{
CPhysicsShell* pPhysicsShell = P_create_Shell();
#ifdef DEBUG
pPhysicsShell->dbg_obj=smart_cast<CPhysicsShellHolder*>(obj);
#endif
Fobb obb; obj->Visual()->vis.box.get_CD(obb.m_translate,obb.m_halfsize); obb.m_rotate.identity();
CPhysicsElement* E = P_create_Element(); R_ASSERT(E); E->add_Box(obb);
pPhysicsShell->add_Element(E);
pPhysicsShell->setMass(mass);
pPhysicsShell->set_PhysicsRefObject(smart_cast<CPhysicsShellHolder*>(obj));
if(!obj->H_Parent())
pPhysicsShell->Activate(obj->XFORM(),0,obj->XFORM(),not_active_state);
return pPhysicsShell;
}
void reset_root_bone_start_pose( CPhysicsShell& shell )
{
VERIFY( &shell );
CPhysicsElement * physics_root_element = shell.get_ElementByStoreOrder( 0 );
VERIFY( physics_root_element );
IKinematics * K = shell.PKinematics();
VERIFY( K );
u16 animation_root_bone_id = K->LL_GetBoneRoot();
CODEGeom *physics_root_bone_geom = physics_root_element->geometry( 0 );
VERIFY( physics_root_bone_geom );
u16 physics_root_bone_id = physics_root_bone_geom->bone_id();
VERIFY( physics_root_bone_id != BI_NONE );
if( animation_root_bone_id == physics_root_bone_id )
return ;
//u16 anim_bones_number = K->LL_BoneCount();
//buffer_vector<u32> anim_bones_bind_positions( _alloca(anim_bones_number*sizeof(u32)),
// anim_bones_number
// );
#pragma todo("LL_GetBindTransform shoud use buffer_vector")
xr_vector<Fmatrix> anim_bones_bind_positions;
K->LL_GetBindTransform( anim_bones_bind_positions );
const Fmatrix physics_root_to_anim_root_bind_transformation
= Fmatrix().mul_43( Fmatrix().invert( anim_bones_bind_positions[ physics_root_bone_id ] ),
anim_bones_bind_positions[ animation_root_bone_id ] );
const Fmatrix &physics_root_bone_anim_transform = K->LL_GetTransform( physics_root_bone_id );
const Fmatrix physics_root_bone_corrected_pos = Fmatrix().mul_43( physics_root_bone_anim_transform ,
physics_root_to_anim_root_bind_transformation
);
physics_root_element->SetTransform( Fmatrix().mul_43( shell.mXFORM, physics_root_bone_corrected_pos ) );
//physics_root_element->TransformPosition( Fmatrix().mul_43( Fmatrix().invert( K->LL_GetTransform( animation_root_bone_id ) ), physics_root_bone_corrected_pos ) );
}
CPhysicsShell* P_build_Shell (CGameObject* obj,bool not_active_state,BONE_P_MAP* bone_map)
{
CKinematics* pKinematics=smart_cast<CKinematics*>(obj->Visual());
CPhysicsShell* pPhysicsShell = P_create_Shell();
#ifdef DEBUG
pPhysicsShell->dbg_obj=smart_cast<CPhysicsShellHolder*>(obj);
#endif
pPhysicsShell->build_FromKinematics(pKinematics,bone_map);
pPhysicsShell->set_PhysicsRefObject(smart_cast<CPhysicsShellHolder*>(obj));
pPhysicsShell->mXFORM.set(obj->XFORM());
pPhysicsShell->Activate(not_active_state);//,
//m_pPhysicsShell->SmoothElementsInertia(0.3f);
pPhysicsShell->SetAirResistance();//0.0014f,1.5f
return pPhysicsShell;
}
CPhysicsShell* P_build_Shell (CGameObject* obj,bool not_active_state,U16Vec& fixed_bones)
{
bone_map.clear ();
CPhysicsShell* pPhysicsShell;
if(!fixed_bones.empty())
for (U16It it=fixed_bones.begin(); it!=fixed_bones.end(); it++)
bone_map.insert(mk_pair(*it,physicsBone()));
pPhysicsShell=P_build_Shell(obj,not_active_state,&bone_map);
// fix bones
BONE_P_PAIR_IT i=bone_map.begin(),e=bone_map.end();
if(i!=e) pPhysicsShell->SetPrefereExactIntegration();
for(;i!=e;i++){
CPhysicsElement* fixed_element=i->second.element;
//R_ASSERT2(fixed_element,"fixed bone has no physics");
if(!fixed_element) continue;
fixed_element->Fix();
}
return pPhysicsShell;
}
CPhysicsShell* P_build_Shell (CGameObject* obj,bool not_active_state,BONE_P_MAP* p_bone_map,LPCSTR fixed_bones)
{
CPhysicsShell* pPhysicsShell;
CKinematics* pKinematics=smart_cast<CKinematics*>(obj->Visual());
if(fixed_bones)
{
int count = _GetItemCount(fixed_bones);
for (int i=0 ;i<count; ++i)
{
string64 fixed_bone ;
_GetItem (fixed_bones,i,fixed_bone) ;
u16 fixed_bone_id=pKinematics->LL_BoneID(fixed_bone) ;
R_ASSERT2(BI_NONE!=fixed_bone_id,"wrong fixed bone") ;
p_bone_map->insert(mk_pair(fixed_bone_id,physicsBone())) ;
}
pPhysicsShell=P_build_Shell(obj,not_active_state,p_bone_map);
//m_pPhysicsShell->add_Joint(P_create_Joint(CPhysicsJoint::enumType::full_control,0,fixed_element));
}
else
pPhysicsShell=P_build_Shell(obj,not_active_state);
BONE_P_PAIR_IT i=p_bone_map->begin(),e=p_bone_map->end();
if(i!=e) pPhysicsShell->SetPrefereExactIntegration();
for(;i!=e;i++)
{
CPhysicsElement* fixed_element=i->second.element;
R_ASSERT2(fixed_element,"fixed bone has no physics");
//if(!fixed_element) continue;
fixed_element->Fix();
}
return pPhysicsShell;
}
void CTeleWhirlwindObject:: keep ()
{
CPhysicsShell* p = get_object() ->PPhysicsShell();
if(!p||!p->isActive())
return;
else
{
p->SetAirResistance(0.f,0.f);
p->set_ApplyByGravity(FALSE);
}
u16 element_number = p ->get_ElementsNumber();
Fvector center = m_telekinesis ->Center();
CPhysicsElement* maxE=p->get_ElementByStoreOrder(0);
for(u16 element=0;element<element_number;++element)
{
CPhysicsElement* E= p->get_ElementByStoreOrder(element);
if(maxE->getMass()<E->getMass())maxE=E;
Fvector dir;dir.sub(center,E->mass_Center());
dir.normalize_safe();
Fvector vel;
E->get_LinearVel(vel);
float force=dir.dotproduct(vel)*E->getMass()/2.f;
if(force<0.f)
{
dir.mul(force);
}
}
maxE->setTorque(Fvector().set(0,500.f,0));
Fvector dist;dist.sub(center,maxE->mass_Center());
if(dist.magnitude()>m_telekinesis->keep_radius()*1.5f)
{
p->setTorque(Fvector().set(0,0,0));
p->setForce(Fvector().set(0,0,0));
p->set_LinearVel(Fvector().set(0,0,0));
p->set_AngularVel(Fvector().set(0,0,0));
p->set_ApplyByGravity(TRUE);
switch_state(TS_Raise);
}
}
void CTeleWhirlwindObject:: raise (float step)
{
CPhysicsShell* p = get_object() ->PPhysicsShell();
if(!p||!p->isActive())
return;
else
{
p->SetAirResistance(0.f,0.f);
p->set_ApplyByGravity(TRUE);
}
u16 element_number = p ->get_ElementsNumber();
Fvector center = m_telekinesis ->Center();
CPhysicsElement* maxE=p->get_ElementByStoreOrder(0);
for(u16 element=0;element<element_number;++element)
{
float k=strength;//600.f;
float predict_v_eps=0.1f;
float mag_eps =.01f;
CPhysicsElement* E= p->get_ElementByStoreOrder(element);
if(maxE->getMass()<E->getMass()) maxE=E;
if (!E->isActive()) continue;
Fvector pos=E->mass_Center();
Fvector diff;
diff.sub(center,pos);
float mag=_sqrt(diff.x*diff.x+diff.z*diff.z);
Fvector lc;lc.set(center);
if(mag>1.f)
{
lc.y/=mag;
}
diff.sub(lc,pos);
mag=diff.magnitude();
float accel=k/mag/mag/mag;//*E->getMass()
Fvector dir;
if(mag<mag_eps)
{
accel=0.f;
//Fvector zer;zer.set(0,0,0);
//E->set_LinearVel(zer);
dir.random_dir();
}
else
{
dir.set(diff);dir.mul(1.f/mag);
}
Fvector vel;
E->get_LinearVel(vel);
float delta_v=accel*fixed_step;
Fvector delta_vel; delta_vel.set(dir);delta_vel.mul(delta_v);
Fvector predict_vel;predict_vel.add(vel,delta_vel);
Fvector delta_pos;delta_pos.set(predict_vel);delta_pos.mul(fixed_step);
Fvector predict_pos;predict_pos.add(pos,delta_pos);
Fvector predict_diff;predict_diff.sub(lc,predict_pos);
float predict_mag=predict_diff.magnitude();
float predict_v=predict_vel.magnitude();
Fvector force;force.set(dir);
if(predict_mag>mag && predict_vel.dotproduct(dir)>0.f && predict_v>predict_v_eps)
{
Fvector motion_dir;motion_dir.set(predict_vel);motion_dir.mul(1.f/predict_v);
float needed_d=diff.dotproduct(motion_dir);
Fvector needed_diff;needed_diff.set(motion_dir);needed_diff.mul(needed_d);
Fvector nearest_p;nearest_p.add(pos,needed_diff);//
Fvector needed_vel;needed_vel.set(needed_diff);needed_vel.mul(1.f/fixed_step);
force.sub(needed_vel,vel);
force.mul(E->getMass()/fixed_step);
}
else
{
force.mul(accel*E->getMass());
}
E->applyForce(force.x,force.y+get_object()->EffectiveGravity()*E->getMass(),force.z);
}
Fvector dist;dist.sub(center,maxE->mass_Center());
if(dist.magnitude()<m_telekinesis->keep_radius()&&b_destroyable)
{
p->setTorque(Fvector().set(0,0,0));
p->setForce(Fvector().set(0,0,0));
p->set_LinearVel(Fvector().set(0,0,0));
p->set_AngularVel(Fvector().set(0,0,0));
switch_state(TS_Keep);
}
}
