void fixFrictionVector( dBodyID b1, dBodyID b2, dContact& contact )
{
dBodyGetPointVel(b1, contact.geom.pos[0], contact.geom.pos[1], contact.geom.pos[2], contact.fdir1);
dVector3 fdir1_b2;
if (b2)
{
dBodyGetPointVel(b2, contact.geom.pos[0], contact.geom.pos[1], contact.geom.pos[2], fdir1_b2);
contact.fdir1[0] -= fdir1_b2[0];
contact.fdir1[1] -= fdir1_b2[1];
contact.fdir1[2] -= fdir1_b2[2];
}
// at this point, contact[i].fdir1 is the relative tangent velocity of the two bodies.
dCROSS(contact.fdir1, =, contact.fdir1, contact.geom.normal);
// now, contact[i].fdir1 is perpendicular to both the normal and
// the relative tangent velocity.
double length = sqrt(contact.fdir1[0] * contact.fdir1[0]
+ contact.fdir1[1] * contact.fdir1[1]
+ contact.fdir1[2] * contact.fdir1[2]);
if (length > 1e-12)
{
// we only use our calculated direction if it has enough precision
contact.fdir1[0] /= length;
contact.fdir1[1] /= length;
contact.fdir1[2] /= length;
dNormalize3(contact.fdir1);
contact.surface.mode |= dContactFDir1;
}
}
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 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));
}
}
}
}
}
void
dxJointDBall::getInfo2( dxJoint::Info2 *info )
{
info->erp = erp;
info->cfm[0] = cfm;
dVector3 globalA1, globalA2;
dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], globalA1);
if (node[1].body)
dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], globalA2);
else
dCopyVector3(globalA2, anchor2);
dVector3 q;
dSubtractVectors3(q, globalA1, globalA2);
#ifdef dSINGLE
const dReal MIN_LENGTH = REAL(1e-7);
#else
const dReal MIN_LENGTH = REAL(1e-12);
#endif
if (dCalcVectorLength3(q) < MIN_LENGTH) {
// too small, let's choose an arbitrary direction
// heuristic: difference in velocities at anchors
dVector3 v1, v2;
dBodyGetPointVel(node[0].body, globalA1[0], globalA1[1], globalA1[2], v1);
if (node[1].body)
dBodyGetPointVel(node[1].body, globalA2[0], globalA2[1], globalA2[2], v2);
else
dSetZero(v2, 3);
dSubtractVectors3(q, v1, v2);
if (dCalcVectorLength3(q) < MIN_LENGTH) {
// this direction is as good as any
q[0] = 1;
q[1] = 0;
q[2] = 0;
}
}
dNormalize3(q);
info->J1l[0] = q[0];
info->J1l[1] = q[1];
info->J1l[2] = q[2];
dVector3 relA1;
dBodyVectorToWorld(node[0].body,
anchor1[0], anchor1[1], anchor1[2],
relA1);
dMatrix3 a1m;
dSetZero(a1m, 12);
dSetCrossMatrixMinus(a1m, relA1, 4);
dMultiply1_331(info->J1a, a1m, q);
if (node[1].body) {
info->J2l[0] = -q[0];
info->J2l[1] = -q[1];
info->J2l[2] = -q[2];
dVector3 relA2;
dBodyVectorToWorld(node[1].body,
anchor2[0], anchor2[1], anchor2[2],
relA2);
dMatrix3 a2m;
dSetZero(a2m, 12);
dSetCrossMatrixPlus(a2m, relA2, 4);
dMultiply1_331(info->J2a, a2m, q);
}
const dReal k = info->fps * info->erp;
info->c[0] = k * (targetDistance - dCalcPointsDistance3(globalA1, globalA2));
}
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())));
}
}
}
void PhysicsSpace::collisionCallback (dGeomID o1, dGeomID o2)
{
StatRealElem *NCollisionTestsStatElem = StatCollector::getGlobalElem(PhysicsHandler::statNCollisionTests);
if(NCollisionTestsStatElem) { NCollisionTestsStatElem->add(1.0f); }
if (dGeomIsSpace (o1) || dGeomIsSpace (o2))
{
// colliding a space with something
dSpaceCollide2 (o1,o2,reinterpret_cast<void *>(this),&PhysicsSpace::collisionCallback);
// collide all geoms internal to the space(s)
if (dGeomIsSpace (o1)) dSpaceCollide (dGeomGetSpace(o1),reinterpret_cast<void *>(this),&PhysicsSpace::collisionCallback);
if (dGeomIsSpace (o2)) dSpaceCollide (dGeomGetSpace(o2),reinterpret_cast<void *>(this),&PhysicsSpace::collisionCallback);
}
else
{
_DiscardCollision = false;
// colliding two non-space geoms, so generate contact
// points between o1 and o2
Int32 numContacts = dCollide(o1, o2, _ContactJoints.size(),
&(_ContactJoints[0].geom), sizeof(dContact));
StatRealElem *NCollisionsStatElem = StatCollector::getGlobalElem(PhysicsHandler::statNCollisions);
if(NCollisionsStatElem) { NCollisionsStatElem->add(static_cast<Real32>(numContacts)); }
if(numContacts>0)
{
Vec3f v1,v2,normal;
Pnt3f position;
dVector3 odeVec;
Real32 projectedNormalSpeed;
for (Int32 i=0; i < numContacts; i++)
{
normal += Vec3f(&_ContactJoints[i].geom.normal[0]);
position += Vec3f(&_ContactJoints[i].geom.pos[0]);
if(dGeomGetBody(o1))
{
dBodyGetPointVel(dGeomGetBody(o1), _ContactJoints[i].geom.pos[0], _ContactJoints[i].geom.pos[1], _ContactJoints[i].geom.pos[2],odeVec);
v1 += Vec3f(&odeVec[0]);
}
if(dGeomGetBody(o2))
{
dBodyGetPointVel(dGeomGetBody(o2), _ContactJoints[i].geom.pos[0], _ContactJoints[i].geom.pos[1], _ContactJoints[i].geom.pos[2],odeVec);
v2 += Vec3f(&odeVec[0]);
}
}
normal = normal * (1.0f/static_cast<Real32>(numContacts));
position = position * (1.0f/static_cast<Real32>(numContacts));
v1 = v1 * (1.0f/static_cast<Real32>(numContacts));
v2 = v2 * (1.0f/static_cast<Real32>(numContacts));
projectedNormalSpeed = (v1+v2).projectTo(normal);
//TODO: Add a way to get the PhysicsGeomUnrecPtr from the GeomIDs so that the PhysicsGeomUnrecPtr can be
//sent to the collision event
produceCollision(position,
normal,
NULL,
NULL,
dGeomGetCategoryBits(o1),
dGeomGetCollideBits (o1),
dGeomGetCategoryBits(o2),
dGeomGetCollideBits (o2),
v1,
v2,
osgAbs(projectedNormalSpeed));
UInt32 Index(0);
for(; Index<_CollisionListenParamsVec.size() ; ++Index)
{
if((dGeomGetCategoryBits(o1) & _CollisionListenParamsVec[Index]._Category) || (dGeomGetCategoryBits(o2) & _CollisionListenParamsVec[Index]._Category))
{
break;
}
}
if(Index < _CollisionListenParamsVec.size())
{
for(UInt32 i(0); i<_CollisionListenParamsVec.size() ; ++i)
{
if( ((dGeomGetCategoryBits(o1) & _CollisionListenParamsVec[i]._Category) || (dGeomGetCategoryBits(o2) & _CollisionListenParamsVec[i]._Category)) &&
(osgAbs(projectedNormalSpeed) >= _CollisionListenParamsVec[i]._SpeedThreshold)
)
{
//TODO: Add a way to get the PhysicsGeomUnrecPtr from the GeomIDs so that the PhysicsGeomUnrecPtr can be
//sent to the collision event
produceCollision(_CollisionListenParamsVec[i]._Listener,
position,
normal,
NULL,
NULL,
dGeomGetCategoryBits(o1),
dGeomGetCollideBits (o1),
dGeomGetCategoryBits(o2),
dGeomGetCollideBits (o2),
v1,
v2,
osgAbs(projectedNormalSpeed));
}
}
}
}
if(!_DiscardCollision)
{
// add these contact points to the simulation
for (Int32 i=0; i < numContacts; i++)
{
getCollisionContact(dGeomGetCategoryBits(o1), dGeomGetCategoryBits(o2))->updateODEContactJoint(_ContactJoints[i]);
dJointID jointId = dJointCreateContact(_CollideWorldID,
_ColJointGroupId,
&_ContactJoints[i]);
dJointAttach(jointId, dGeomGetBody(o1), dGeomGetBody(o2));
}
}
}
}
void PhysicsBody::getPointVel(const Vec3f &v, Vec3f &result)
{
dVector3 t;
dBodyGetPointVel(_BodyID, v.x(), v.y(), v.z(), t);
result.setValue(Vec3f(t[0], t[1], t[2]));
}
