void CPHWorld::SetStep(dReal s)
{
fixed_step = s;
world_cfm = CFM(SPRING_S(base_cfm,base_erp,base_fixed_step),DAMPING(base_cfm,base_erp));
world_erp = ERP(SPRING_S(base_cfm,base_erp,base_fixed_step),DAMPING(base_cfm,base_erp));
world_spring = 1.0f*SPRING (world_cfm,world_erp);
world_damping = 1.0f*DAMPING(world_cfm,world_erp);
if(ph_world&&ph_world->Exist())
{
float frame_time = Device.fTimeDelta;
u32 it_number = iFloor (frame_time /fixed_step);
frame_time -= it_number*fixed_step;
ph_world->m_previous_frame_time = frame_time;
ph_world->m_frame_time = frame_time;
}
}
void CPHWorld::Create()
{
dWorldID phWorld=0;
if (psDeviceFlags.test(mtPhysics)) Device.seqFrameMT.Add (this,REG_PRIORITY_HIGH);
else Device.seqFrame.Add (this,REG_PRIORITY_LOW);
m_commander =xr_new<CPHCommander>();
//dVector3 extensions={2048,256,2048};
/*
Fbox level_box = Level().ObjectSpace.GetBoundingVolume();
Fvector level_size,level_center;
level_box . getsize (level_size);
level_box . getcenter (level_center);
dVector3 extensions = { level_size.x ,256.f,level_size.z};
dVector3 center = {level_center.x,0.f,level_center.z};
*/
#ifdef ODE_SLOW_SOLVER
#else
dWorldSetAutoEnableDepthSF1(phWorld, 100000000);
///dWorldSetContactSurfaceLayer(phWorld,0.f);
//phWorld->contactp.min_depth =0.f;
#endif
ContactGroup = dJointGroupCreate(0);
dWorldSetGravity (phWorld, 0,-Gravity(), 0);//-2.f*9.81f
Mesh.Create (0,phWorld);
#ifdef PH_PLAIN
plane=dCreatePlane(Space,0,1,0,0.3f);
#endif
//const dReal k_p=2400000.f;//550000.f;///1000000.f;
//const dReal k_d=200000.f;
dWorldSetERP(phWorld, ERP(world_spring,world_damping) );
dWorldSetCFM(phWorld, CFM(world_spring,world_damping));
//dWorldSetERP(phWorld, 0.2f);
//dWorldSetCFM(phWorld, 0.000001f);
disable_count=0;
m_motion_ray=dCreateRayMotions(0);
phBoundaries.set(Level().ObjectSpace.GetBoundingVolume());
phBoundaries.y1-=30.f;
CPHCollideValidator::Init();
b_exist=true;
}
void CPHCapture::PullingUpdate()
{
if(!m_taget_element->isActive()||inl_ph_world().Device().dwTimeGlobal-m_time_start>m_capture_time)
{
Release();
return;
}
Fvector dir;
Fvector capture_bone_position;
//CObject* object=smart_cast<CObject*>(m_character->PhysicsRefObject());
capture_bone_position.set(m_capture_bone->mTransform.c);
m_character->PhysicsRefObject()->ObjectXFORM().transform_tiny(capture_bone_position);
m_taget_element->GetGlobalPositionDynamic(&dir);
dir.sub(capture_bone_position,dir);
float dist=dir.magnitude();
if(dist>m_pull_distance)
{
Release();
return;
}
dir.mul(1.f/dist);
if(dist<m_capture_distance)
{
m_back_force=0.f;
m_joint=dJointCreateBall(0,0);
m_island.AddJoint(m_joint);
m_ajoint=dJointCreateAMotor(0,0);
m_island.AddJoint(m_ajoint);
dJointSetAMotorMode (m_ajoint, dAMotorEuler);
dJointSetAMotorNumAxes (m_ajoint, 3);
CreateBody();
dBodySetPosition(m_body,capture_bone_position.x,capture_bone_position.y,capture_bone_position.z);
VERIFY( smart_cast<CPHElement*>(m_taget_element) );
CPHElement * e = static_cast<CPHElement*>(m_taget_element);
dJointAttach(m_joint,m_body,e->get_body());
dJointAttach(m_ajoint,m_body,e->get_body());
dJointSetFeedback (m_joint, &m_joint_feedback);
dJointSetFeedback (m_ajoint, &m_joint_feedback);
dJointSetBallAnchor(m_joint,capture_bone_position.x,capture_bone_position.y,capture_bone_position.z);
dJointSetAMotorAxis (m_ajoint, 0, 1, dir.x, dir.y, dir.z);
if(dir.x>EPS)
{
if(dir.y>EPS)
{
float mag=dir.x*dir.x+dir.y*dir.y;
dJointSetAMotorAxis (m_ajoint, 2, 2, -dir.y/mag, dir.x/mag, 0.f);
}
else if(dir.z>EPS)
{
float mag=dir.x*dir.x+dir.z*dir.z;
dJointSetAMotorAxis (m_ajoint, 2, 2, -dir.z/mag,0.f,dir.x/mag);
}
else
{
dJointSetAMotorAxis (m_ajoint, 2, 2, 1.f,0.f,0.f);
}
}
else
{
if(dir.y>EPS)
{
if(dir.z>EPS)
{
float mag=dir.y*dir.y+dir.z*dir.z;
dJointSetAMotorAxis (m_ajoint, 2, 2,0.f,-dir.z/mag,dir.y/mag);
}
else
{
dJointSetAMotorAxis (m_ajoint, 2, 2, 0.f,1.f,0.f);
}
}
else
{
dJointSetAMotorAxis (m_ajoint, 2, 2, 0.f,0.f,1.f);
}
}
//float hi=-M_PI/2.f,lo=-hi;
//dJointSetAMotorParam(m_ajoint,dParamLoStop ,lo);
//dJointSetAMotorParam(m_ajoint,dParamHiStop ,hi);
//dJointSetAMotorParam(m_ajoint,dParamLoStop2 ,lo);
//dJointSetAMotorParam(m_ajoint,dParamHiStop2 ,hi);
//dJointSetAMotorParam(m_ajoint,dParamLoStop3 ,lo);
//dJointSetAMotorParam(m_ajoint,dParamHiStop3 ,hi);
dJointSetAMotorParam(m_ajoint,dParamFMax ,m_capture_force*0.2f);
dJointSetAMotorParam(m_ajoint,dParamVel ,0.f);
dJointSetAMotorParam(m_ajoint,dParamFMax2 ,m_capture_force*0.2f);
dJointSetAMotorParam(m_ajoint,dParamVel2 ,0.f);
dJointSetAMotorParam(m_ajoint,dParamFMax3 ,m_capture_force*0.2f);
dJointSetAMotorParam(m_ajoint,dParamVel3 ,0.f);
///////////////////////////////////
float sf=0.1f,df=10.f;
float erp=ERP(world_spring*sf,world_damping*df);
float cfm=CFM(world_spring*sf,world_damping*df);
dJointSetAMotorParam(m_ajoint,dParamStopERP ,erp);
dJointSetAMotorParam(m_ajoint,dParamStopCFM ,cfm);
dJointSetAMotorParam(m_ajoint,dParamStopERP2 ,erp);
dJointSetAMotorParam(m_ajoint,dParamStopCFM2 ,cfm);
dJointSetAMotorParam(m_ajoint,dParamStopERP3 ,erp);
dJointSetAMotorParam(m_ajoint,dParamStopCFM3 ,cfm);
/////////////////////////////////////////////////////////////////////
///dJointSetAMotorParam(m_joint1,dParamFudgeFactor ,0.1f);
//dJointSetAMotorParam(m_joint1,dParamFudgeFactor2 ,0.1f);
//dJointSetAMotorParam(m_joint1,dParamFudgeFactor3 ,0.1f);
/////////////////////////////////////////////////////////////////////////////
sf=0.1f,df=10.f;
erp=ERP(world_spring*sf,world_damping*df);
cfm=CFM(world_spring*sf,world_damping*df);
dJointSetAMotorParam(m_ajoint,dParamCFM ,cfm);
dJointSetAMotorParam(m_ajoint,dParamCFM2 ,cfm);
dJointSetAMotorParam(m_ajoint,dParamCFM3 ,cfm);
///////////////////////////
//dJointSetAMotorParam(m_ajoint,dParamLoStop ,0.f);
//dJointSetAMotorParam(m_ajoint,dParamHiStop ,0.f);
m_taget_element->set_LinearVel ( Fvector().set( 0 ,0, 0 ) );
m_taget_element->set_AngularVel( Fvector().set( 0 ,0, 0 ) );
m_taget_element->set_DynamicLimits();
//m_taget_object->PPhysicsShell()->set_JointResistance()
e_state=cstCaptured;
return;
}
m_taget_element->applyForce(dir,m_pull_force);
}
IC static int CollideIntoGroup(dGeomID o1, dGeomID o2,dJointGroupID jointGroup,CPHIsland* world,const int &MAX_CONTACTS)
{
const int RS= 800+10;
const int N = RS;
static dContact contacts[RS];
int collided_contacts=0;
// get the contacts up to a maximum of N contacts
int n;
VERIFY (o1);
VERIFY (o2);
VERIFY(&contacts[0].geom);
n = dCollide(o1, o2, N, &contacts[0].geom, sizeof(dContact));
if(n>N-1)
n=N-1;
int i;
for(i = 0; i < n; ++i)
{
dContact &c =contacts[i];
dContactGeom &cgeom =c.geom;
dSurfaceParameters &surface=c.surface;
dGeomID g1 =cgeom.g1;
dGeomID g2 =cgeom.g2;
bool pushing_neg= false;
bool do_collide = true;
dxGeomUserData* usr_data_1 =NULL;
dxGeomUserData* usr_data_2 =NULL;
u16 material_idx_1 =0;
u16 material_idx_2 =0;
surface.mu =1.f;// 5000.f;
surface.soft_erp=1.f;//ERP(world_spring,world_damping);
surface.soft_cfm=1.f;//CFM(world_spring,world_damping);
surface.bounce = 0.01f;//0.1f;
surface.bounce_vel =1.5f;//0.005f;
usr_data_1 = retrieveGeomUserData(g1);
usr_data_2 = retrieveGeomUserData(g2);
///////////////////////////////////////////////////////////////////////////////////////////////////
if(usr_data_2) material_idx_2=usr_data_2->material;
if(usr_data_1) material_idx_1=usr_data_1->material;
bool is_tri_1=dTriListClass == dGeomGetClass(g1);
bool is_tri_2=dTriListClass == dGeomGetClass(g2);
if(!is_tri_2&&!is_tri_1) surface.mode=0;
if(is_tri_1) material_idx_1=(u16)surface.mode;
if(is_tri_2) material_idx_2=(u16)surface.mode;
SGameMtl* material_1=GMLib.GetMaterialByIdx(material_idx_1);
SGameMtl* material_2=GMLib.GetMaterialByIdx(material_idx_2);
////////////////params can be changed in callbacks//////////////////////////////////////////////////////////////////////////
surface.mode =dContactApprox1|dContactSoftERP|dContactSoftCFM;
float spring =material_2->fPHSpring*material_1->fPHSpring*world_spring;
float damping =material_2->fPHDamping*material_1->fPHDamping*world_damping;
surface.soft_erp=ERP(spring,damping);
surface.soft_cfm=CFM(spring,damping);
surface.mu=material_2->fPHFriction*material_1->fPHFriction;
/////////////////////////////////////////////////////////////////////////////////////////////////
Flags32 &flags_1=material_1->Flags;
Flags32 &flags_2=material_2->Flags;
if(is_tri_1)
{
#pragma warning(push)
#pragma warning(disable:4245)
if(material_1->Flags.test(SGameMtl::flSlowDown)&&!(usr_data_2->pushing_neg||usr_data_2->pushing_b_neg))
#pragma warning(pop)
{
dBodyID body=dGeomGetBody(g2);
R_ASSERT2(body,"static - static collision !!!");
if(material_1->Flags.test(SGameMtl::flLiquid))
{
add_contact_body_effector(body,c,material_1);
}
else
{
if(!usr_data_2 || !usr_data_2->ph_object || !usr_data_2->ph_object->IsRayMotion())
{
add_contact_body_effector(body,c,material_1);
}
}
}
if(material_1->Flags.test(SGameMtl::flPassable))
do_collide=false;
// if(material_2->Flags.is(SGameMtl::flClimable))
// do_collide=false;
}
if(is_tri_2)
{
#pragma warning(push)
#pragma warning(disable:4245)
if(material_2->Flags.test(SGameMtl::flSlowDown)&&!(usr_data_1->pushing_neg||usr_data_1->pushing_b_neg))
#pragma warning(pop)
{
dBodyID body=dGeomGetBody(g1);
R_ASSERT2(body,"static - static collision !!!");
if(material_2->Flags.test(SGameMtl::flLiquid))
{
add_contact_body_effector(body,c,material_2);
}
else
{
if(!usr_data_1 || !usr_data_1->ph_object || !usr_data_1->ph_object->IsRayMotion())
{
add_contact_body_effector(body,c,material_2);
}
}
}
if(material_2->Flags.test(SGameMtl::flPassable))
do_collide=false;
}
if(flags_1.test(SGameMtl::flBounceable)&&flags_2.test(SGameMtl::flBounceable))
{
surface.mode |= dContactBounce;
surface.bounce_vel = _max(material_1->fPHBounceStartVelocity,material_2->fPHBounceStartVelocity);
surface.bounce = _min(material_1->fPHBouncing,material_2->fPHBouncing);
}
/////////////////////////////////////////////////////////////////////////////////////////////////
if(usr_data_2&&usr_data_2->object_callbacks){
usr_data_2->object_callbacks->Call(do_collide,false,c,material_1,material_2);
}
if(usr_data_1&&usr_data_1->object_callbacks){
usr_data_1->object_callbacks->Call(do_collide,true,c,material_1,material_2);
}
if(usr_data_2){
usr_data_2->pushing_b_neg = usr_data_2->pushing_b_neg && !GMLib.GetMaterialByIdx(usr_data_2->b_neg_tri->material)->Flags.test(SGameMtl::flPassable);
usr_data_2->pushing_neg = usr_data_2->pushing_neg && !GMLib.GetMaterialByIdx(usr_data_2->neg_tri->material)->Flags.test(SGameMtl::flPassable);
pushing_neg=usr_data_2->pushing_b_neg||usr_data_2->pushing_neg;
if(usr_data_2->ph_object){
usr_data_2->ph_object->InitContact(&c,do_collide,material_idx_1,material_idx_2);
}
}
///////////////////////////////////////////////////////////////////////////////////////
if(usr_data_1){
usr_data_1->pushing_b_neg = usr_data_1->pushing_b_neg && !GMLib.GetMaterialByIdx(usr_data_1->b_neg_tri->material)->Flags.test(SGameMtl::flPassable);
usr_data_1->pushing_neg = usr_data_1->pushing_neg && !GMLib.GetMaterialByIdx(usr_data_1->neg_tri->material)->Flags.test(SGameMtl::flPassable);
pushing_neg=usr_data_1->pushing_b_neg||usr_data_1->pushing_neg;
if(usr_data_1->ph_object){
usr_data_1->ph_object->InitContact(&c,do_collide,material_idx_1,material_idx_2);
}
}
if (pushing_neg)
surface.mu=dInfinity;
if (do_collide && collided_contacts<MAX_CONTACTS)
{
++collided_contacts;
#ifdef DEBUG
if( ph_dbg_draw_mask.test(phDbgDrawContacts) )
DBG_DrawContact(c);
#endif
dJointID contact_joint = dJointCreateContact(0, jointGroup, &c);
world->ConnectJoint(contact_joint);
dJointAttach (contact_joint, dGeomGetBody(g1), dGeomGetBody(g2));
}
}
return collided_contacts;
}
const dReal default_l_limit = 150.f;//(3.f/fixed_step=0.02f);
const dReal default_l_scale = 1.01f;
const dReal default_w_scale = 1.01f;
const dReal default_k_l = 0.0002f;//square resistance !!
const dReal default_k_w = 0.05f;
const dReal mass_limit = 10000.f;//some conventional value used as evaluative param (there is no code restriction on mass)
extern const u16 max_joint_allowed_for_exeact_integration = 30;
//base params
const dReal base_fixed_step = 0.02f ;
const dReal base_erp = 0.54545456f ;
const dReal base_cfm = 1.1363636e-006f ;
//base params
dReal fixed_step = 0.01f;
dReal world_cfm = CFM(SPRING_S(base_cfm,base_erp,base_fixed_step),DAMPING(base_cfm,base_erp));
dReal world_erp = ERP(SPRING_S(base_cfm,base_erp,base_fixed_step),DAMPING(base_cfm,base_erp));
dReal world_spring = 1.0f*SPRING (world_cfm,world_erp);
dReal world_damping = 1.0f*DAMPING(world_cfm,world_erp);
const dReal default_world_gravity = 2*9.81f;
/////////////////////////////////////////////////////
int phIterations = 18;
float phTimefactor = 1.f;
float phBreakCommonFactor = 0.01f;
float phRigidBreakWeaponFactor = 1.f;
Fbox phBoundaries = {1000.f,1000.f,-1000.f,-1000.f};