void CCar::SWheel::ApplyDamage(u16 level) { inherited::ApplyDamage(level); if(!joint) return; if(level == 0 )return; float sf,df; dJointID dj=joint->GetDJoint(); switch(level) { case 1: joint->GetJointSDfactors(sf,df); sf/=20.f;df*=4.f; joint->SetJointSDfactors(sf,df); car->m_damage_particles.PlayWheel1(car,bone_id); break; case 2: dVector3 v; dJointGetHinge2Axis2(dj,v); v[0]+=0.1f;v[1]+=0.1f;v[2]+=0.1f; accurate_normalize(v); dJointSetHinge2Axis2(dj,v[0],v[1],v[2]); joint->GetJointSDfactors(sf,df); sf/=30.f;df*=8.f; joint->SetJointSDfactors(sf,df); car->m_damage_particles.PlayWheel2(car,bone_id); break; default: NODEFAULT; } }
void CPHContactBodyEffector::Apply() { const dReal* linear_velocity =dBodyGetLinearVel(m_body); dReal linear_velocity_smag =dDOT(linear_velocity,linear_velocity); dReal linear_velocity_mag =_sqrt(linear_velocity_smag); dReal effect =10000.f*m_recip_flotation*m_recip_flotation; dMass mass; dBodyGetMass(m_body,&mass); dReal l_air=linear_velocity_mag*effect;//force/velocity !!! if(l_air>mass.mass/fixed_step) l_air=mass.mass/fixed_step;//validate if(!fis_zero(l_air)) { dVector3 force={ -linear_velocity[0]*l_air, -linear_velocity[1]*l_air, -linear_velocity[2]*l_air, 0.f }; if(!m_material->Flags.is(SGameMtl::flPassable)) { dVector3& norm=m_contact.geom.normal; accurate_normalize(norm); dMass m; dBodyGetMass(m_body,&m); dReal prg=1.f*dDOT(force,norm);//+dDOT(linear_velocity,norm)*m.mass/fixed_step force[0]-=prg*norm[0]; force[1]-=prg*norm[1]; force[2]-=prg*norm[2]; } dBodyAddForce( m_body, force[0], force[1], force[2] ); } dBodySetData(m_body,NULL); }
int dcTriListCollider::dTriCyl ( const dReal* v0,const dReal* v1,const dReal* v2, Triangle* T, dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip ) { // VERIFY (skip >= (int)sizeof(dContactGeom)); VERIFY (dGeomGetClass(o1)== dCylinderClassUser); const dReal *R = dGeomGetRotation(o1); const dReal* p=dGeomGetPosition(o1); dReal radius; dReal hlz; dGeomCylinderGetParams(o1,&radius,&hlz); hlz/=2.f; // find number of contacts requested int maxc = flags & NUMC_MASK; if (maxc < 1) maxc = 1; if (maxc > 3) maxc = 3; // no more than 3 contacts per box allowed const dVector3 &triAx=T->norm; dVector3 triSideAx0={T->side0[0],T->side0[1],T->side0[2]}; //{v1[0]-v0[0],v1[1]-v0[1],v1[2]-v0[2]}; dVector3 triSideAx1={T->side1[0],T->side1[1],T->side1[2]}; //{v2[0]-v1[0],v2[1]-v1[1],v2[2]-v1[2]}; dVector3 triSideAx2={v0[0]-v2[0],v0[1]-v2[1],v0[2]-v2[2]}; //dCROSS(triAx,=,triSideAx0,triSideAx1); int code=0; dReal signum, outDepth,cos0,cos1,cos2,sin1; //////////////////////////////////////////////////////////////////////////// //sepparation along tri plane normal;/////////////////////////////////////// //////////////////////////////////////////////////////////////////////////// //accurate_normalize(triAx); //cos0=dDOT14(triAx,R+0); cos1=dFabs(dDOT14(triAx,R+1)); //cos2=dDOT14(triAx,R+2); //sin1=_sqrt(cos0*cos0+cos2*cos2); //////////////////////// //another way ////////// cos1=cos1<REAL(1.) ? cos1 : REAL(1.); //cos1 may slightly exeed 1.f sin1=_sqrt(REAL(1.)-cos1*cos1); ////////////////////////////// dReal sidePr=cos1*hlz+sin1*radius; dReal dist=-T->dist; //dDOT(triAx,v0)-dDOT(triAx,p); if(dist>0.f) RETURN0; dReal depth=sidePr-dFabs(dist); outDepth=depth; signum=dist>0.f ? 1.f : -1.f; code=0; if(depth<0.f) RETURN0; dReal depth0,depth1,depth2,dist0,dist1,dist2; bool isPdist0,isPdist1,isPdist2; bool testV0,testV1,testV2; bool sideTestV00,sideTestV01,sideTestV02; bool sideTestV10,sideTestV11,sideTestV12; bool sideTestV20,sideTestV21,sideTestV22; ////////////////////////////////////////////////////////////////////////////// //cylinder axis - one of the triangle vertexes touches cylinder's flat surface ////////////////////////////////////////////////////////////////////////////// dist0=dDOT14(v0,R+1)-dDOT14(p,R+1); dist1=dDOT14(v1,R+1)-dDOT14(p,R+1); dist2=dDOT14(v2,R+1)-dDOT14(p,R+1); isPdist0=dist0>0.f; isPdist1=dist1>0.f; isPdist2=dist2>0.f; depth0=hlz-dFabs(dist0); depth1=hlz-dFabs(dist1); depth2=hlz-dFabs(dist2); testV0=depth0>0.f; testV1=depth1>0.f; testV2=depth2>0.f; if(isPdist0==isPdist1 && isPdist1== isPdist2) //(here and lower) check the tryangle is on one side of the cylinder { if(depth0>depth1) if(depth0>depth2) if(testV0){ if(depth0<outDepth) { signum= isPdist0 ? 1.f : -1.f; outDepth=depth0; code=1; } } else RETURN0; else if(testV2){ if(depth2<outDepth) { outDepth=depth2; signum= isPdist2 ? 1.f : -1.f; code=3; } } else RETURN0; else if(depth1>depth2) if(testV1){ if(depth1<outDepth) { outDepth=depth1; signum= isPdist1 ? 1.f : -1.f; code=2; } } else RETURN0; else if(testV2){ if(depth2<outDepth) { outDepth=depth2; signum= isPdist2 ? 1.f : -1.f; code=2; } } else RETURN0; } dVector3 axis,outAx; dReal posProj; dReal pointDepth=0.f; #define TEST(vx,ox1,ox2,c) \ {\ posProj=dDOT14(v##vx,R+1)-dDOT14(p,R+1);\ \ axis[0]=v##vx[0]-p[0]-R[1]*posProj;\ axis[1]=v##vx[1]-p[1]-R[5]*posProj;\ axis[2]=v##vx[2]-p[2]-R[9]*posProj;\ \ accurate_normalize(axis);\ \ \ dist0=dDOT(v0,axis)-dDOT(p,axis);\ dist1=dDOT(v1,axis)-dDOT(p,axis);\ dist2=dDOT(v2,axis)-dDOT(p,axis);\ \ isPdist0=dist0>0.f;\ isPdist1=dist1>0.f;\ isPdist2=dist2>0.f;\ \ depth0=radius-dFabs(dist0);\ depth1=radius-dFabs(dist1);\ depth2=radius-dFabs(dist2);\ \ sideTestV##vx##0=depth0>0.f;\ sideTestV##vx##1=depth1>0.f;\ sideTestV##vx##2=depth2>0.f;\ \ if(isPdist0==isPdist1 && isPdist1== isPdist2)\ \ {\ if(sideTestV##vx##0||sideTestV##vx##1||sideTestV##vx##2){\ if(!(depth##vx<depth##ox1 || depth##vx<depth##ox2))\ {\ if(depth##vx<outDepth && depth##vx > pointDepth)\ {\ pointDepth=depth##vx;\ signum= isPdist##vx ? 1.f : -1.f;\ outAx[0]=axis[0];\ outAx[1]=axis[1];\ outAx[2]=axis[2];\ code=c;\ }\ }\ }\ else RETURN0;\ \ \ \ }\ } if(testV0) TEST(0,1,2,4) if(testV1 ) TEST(1,2,0,5) //&& sideTestV01 if(testV2 ) TEST(2,0,1,6) //&& sideTestV02 && sideTestV12 #undef TEST dVector3 tpos,pos; if(code>3) outDepth=pointDepth; //deepest vertex axis used if its depth less than outDepth //else{ //bool outV0=!(testV0&&sideTestV00&&sideTestV10&&sideTestV20); //bool outV1=!(testV1&&sideTestV01&&sideTestV11&&sideTestV21); //bool outV2=!(testV2&&sideTestV02&&sideTestV12&&sideTestV22); bool outV0=true; bool outV1=true; bool outV2=true; ///////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// ///crosses between triangle sides and cylinder axis////////////////////////// ///////////////////////////////////////////////////////////////////////////// #define TEST(ax,nx,ox,c) if(cylinderCrossesLine(p,R+1,hlz,v##ax,v##nx,triSideAx##ax,tpos)) {\ dCROSS114(axis,=,triSideAx##ax,R+1);\ accurate_normalize(axis);\ dist##ax=dDOT(v##ax,axis)-dDOT(p,axis);\ dist##ox=dDOT(v##ox,axis)-dDOT(p,axis);\ \ isPdist##ax=dist##ax>0.f;\ isPdist##ox=dist##ox>0.f;\ \ if(isPdist##ax == isPdist##ox)\ {\ depth##ax=radius-dFabs(dist##ax);\ depth##ox=radius-dFabs(dist##ox);\ \ if(depth##ax>0.f){\ if(depth##ax<=outDepth && depth##ax>=depth##ox) \ {\ outDepth=depth##ax;\ signum= isPdist##ax ? 1.f : -1.f;\ outAx[0]=axis[0];\ outAx[1]=axis[1];\ outAx[2]=axis[2];\ pos[0]=tpos[0];\ pos[1]=tpos[1];\ pos[2]=tpos[2];\ code=c;\ }\ }\ else if(depth##ox<0.f) RETURN0;\ \ }\ } accurate_normalize(triSideAx0); if(outV0&&outV1) TEST(0,1,2,7) accurate_normalize(triSideAx1); if(outV1&&outV2) TEST(1,2,0,8) accurate_normalize(triSideAx2); if(outV2&&outV0) TEST(2,0,1,9) #undef TEST //////////////////////////////////// //test cylinder rings on triangle sides//// //////////////////////////////////// dVector3 tAx,cen; dReal sign; bool cs; #define TEST(ax,nx,ox,c) \ {\ posProj=dDOT(p,triSideAx##ax)-dDOT(v##ax,triSideAx##ax);\ axis[0]=p[0]-v0[0]-triSideAx##ax[0]*posProj;\ axis[1]=p[1]-v0[1]-triSideAx##ax[1]*posProj;\ axis[2]=p[2]-v0[2]-triSideAx##ax[2]*posProj;\ \ sign=dDOT14(axis,R+1)>0.f ? 1.f :-1.f;\ cen[0]=p[0]-sign*R[1]*hlz;\ cen[1]=p[1]-sign*R[5]*hlz;\ cen[2]=p[2]-sign*R[9]*hlz;\ \ cs=circleLineIntersection(R+1,cen,radius,triSideAx##ax,v##ax,-sign,tpos);\ \ axis[0]=tpos[0]-cen[0];\ axis[1]=tpos[1]-cen[1];\ axis[2]=tpos[2]-cen[2];\ \ if(cs){ \ \ cos0=dDOT14(axis,R+0);\ cos2=dDOT14(axis,R+2);\ tAx[0]=R[2]*cos0-R[0]*cos2;\ tAx[1]=R[6]*cos0-R[4]*cos2;\ tAx[2]=R[10]*cos0-R[8]*cos2;\ \ dCROSS(axis,=,triSideAx##ax,tAx);\ \ }\ accurate_normalize(axis);\ dist##ax=dDOT(v##ax,axis)-dDOT(p,axis);\ if(dist##ax*dDOT(axis,triSideAx##nx)>0.f){\ \ cos0=dDOT14(axis,R+0);\ cos1=dFabs(dDOT14(axis,R+1));\ cos2=dDOT14(axis,R+2);\ \ \ sin1=_sqrt(cos0*cos0+cos2*cos2);\ \ sidePr=cos1*hlz+sin1*radius;\ \ \ dist##ox=dDOT(v##ox,axis)-dDOT(p,axis);\ \ isPdist##ax=dist##ax>0.f;\ isPdist##ox=dist##ox>0.f;\ \ if(isPdist##ax == isPdist##ox) \ \ {\ depth##ax=sidePr-dFabs(dist##ax);\ depth##ox=sidePr-dFabs(dist##ox);\ \ if(depth##ax>0.f){\ if(depth##ax<outDepth) \ {\ outDepth=depth##ax;\ signum= isPdist##ax ? 1.f : -1.f;\ outAx[0]=axis[0];\ outAx[1]=axis[1];\ outAx[2]=axis[2];\ pos[0]=tpos[0];\ pos[1]=tpos[1];\ pos[2]=tpos[2];\ code=c;\ }\ }\ else if(depth##ox<0.f) RETURN0;\ \ \ }\ }\ } if(7!=code) TEST(0,1,2,10) if(8!=code) TEST(1,2,0,11) if(9!=code) TEST(2,0,1,12) #undef TEST //} ////////////////////////////////////////////////////////////////////// ///if we get to this poit tri touches cylinder/////////////////////// ///////////////////////////////////////////////////////////////////// //VERIFY( g_pGameLevel ); CDB::TRI* T_array = inl_ph_world().ObjectSpace().GetStaticTris(); dVector3 norm; unsigned int ret; flags8& gl_state=gl_cl_tries_state[I-B]; if(code==0){ norm[0]=triAx[0]*signum; norm[1]=triAx[1]*signum; norm[2]=triAx[2]*signum; dReal Q1 = dDOT14(norm,R+0); dReal Q2 = dDOT14(norm,R+1); dReal Q3 = dDOT14(norm,R+2); dReal factor =_sqrt(Q1*Q1+Q3*Q3); dReal C1,C3; dReal centerDepth;//depth in the cirle centre if(factor>0.f) { C1=Q1/factor; C3=Q3/factor; } else { C1=1.f; C3=0.f; } dReal A1 = radius * C1;//cosinus dReal A2 = hlz;//Q2 dReal A3 = radius * C3;//sinus if(factor>0.f) centerDepth=outDepth-A1*Q1-A3*Q3; else centerDepth=outDepth; pos[0]=p[0]; pos[1]=p[1]; pos[2]=p[2]; pos[0]+= Q2>0 ? hlz*R[1]:-hlz*R[1]; pos[1]+= Q2>0 ? hlz*R[5]:-hlz*R[5]; pos[2]+= Q2>0 ? hlz*R[9]:-hlz*R[9]; ret=0; dVector3 cross0, cross1, cross2; dReal ds0,ds1,ds2; dCROSS(cross0,=,triAx,triSideAx0); ds0=dDOT(cross0,v0); dCROSS(cross1,=,triAx,triSideAx1); ds1=dDOT(cross1,v1); dCROSS(cross2,=,triAx,triSideAx2); ds2=dDOT(cross2,v2); contact->pos[0] = pos[0]+A1*R[0]+A3*R[2]; contact->pos[1] = pos[1]+A1*R[4]+A3*R[6]; contact->pos[2] = pos[2]+A1*R[8]+A3*R[10]; if(dDOT(cross0,contact->pos)-ds0>0.f && dDOT(cross1,contact->pos)-ds1>0.f && dDOT(cross2,contact->pos)-ds2>0.f){ contact->depth = outDepth; ret=1; } if(dFabs(Q2)>M_SQRT1_2){ A1=(-C1*M_COS_PI_3-C3*M_SIN_PI_3)*radius; A3=(-C3*M_COS_PI_3+C1*M_SIN_PI_3)*radius; CONTACT(contact,ret*skip)->pos[0]=pos[0]+A1*R[0]+A3*R[2]; CONTACT(contact,ret*skip)->pos[1]=pos[1]+A1*R[4]+A3*R[6]; CONTACT(contact,ret*skip)->pos[2]=pos[2]+A1*R[8]+A3*R[10]; CONTACT(contact,ret*skip)->depth=centerDepth+Q1*A1+Q3*A3; if(CONTACT(contact,ret*skip)->depth>0.f) if(dDOT(cross0,CONTACT(contact,ret*skip)->pos)-ds0>0.f && dDOT(cross1,CONTACT(contact,ret*skip)->pos)-ds1>0.f && dDOT(cross2,CONTACT(contact,ret*skip)->pos)-ds2>0.f) ++ret; A1=(-C1*M_COS_PI_3+C3*M_SIN_PI_3)*radius; A3=(-C3*M_COS_PI_3-C1*M_SIN_PI_3)*radius; CONTACT(contact,ret*skip)->pos[0]=pos[0]+A1*R[0]+A3*R[2]; CONTACT(contact,ret*skip)->pos[1]=pos[1]+A1*R[4]+A3*R[6]; CONTACT(contact,ret*skip)->pos[2]=pos[2]+A1*R[8]+A3*R[10]; CONTACT(contact,ret*skip)->depth=centerDepth+Q1*A1+Q3*A3; if(CONTACT(contact,ret*skip)->depth>0.f) if(dDOT(cross0,CONTACT(contact,ret*skip)->pos)-ds0>0.f && dDOT(cross1,CONTACT(contact,ret*skip)->pos)-ds1>0.f && dDOT(cross2,CONTACT(contact,ret*skip)->pos)-ds2>0.f) ++ret; } else { CONTACT(contact,ret*skip)->pos[0]=contact->pos[0]-2.f*(Q2>0 ? hlz*R[1]:-hlz*R[1]); CONTACT(contact,ret*skip)->pos[1]=contact->pos[1]-2.f*(Q2>0 ? hlz*R[5]:-hlz*R[5]); CONTACT(contact,ret*skip)->pos[2]=contact->pos[2]-2.f*(Q2>0 ? hlz*R[9]:-hlz*R[9]); CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q2*2.f*A2); if(CONTACT(contact,ret*skip)->depth>0.f) if(dDOT(cross0,CONTACT(contact,ret*skip)->pos)-ds0>0.f && dDOT(cross1,CONTACT(contact,ret*skip)->pos)-ds1>0.f && dDOT(cross2,CONTACT(contact,ret*skip)->pos)-ds2>0.f) ++ret; } }