Code:
int CLivingEntity::Step(float time_interval)
{
if (time_interval<=0)
return 1;
float dt = m_timeStepFull-m_timeStepPerformed;
time_interval = m_pWorld->m_bWorldStep==2 ? min(time_interval, dt) : dt;
time_interval = max(time_interval, 0.001f);
const int iCaller = get_iCaller_int();
int i,j,jmin,ipartMin,nents,ncont,bFlying,bWasFlying,bUnprojected,idmat,iPrim, bHasExtraParts=0,
bHasFastPhys,icnt,nUnproj,bStaticUnproj,bDynUnproj,bMoving=0,nPrecompEnts=0,nPrecompParts=0, nUsedPartsCount=0,
&nNoResponseAllocLE=m_pWorld->m_threadData[iCaller].nNoResponseAllocLE,
&nPrecompEntsAlloc=m_pWorld->m_threadData[iCaller].nPrecompEntsAllocLE,&nPrecompPartsAlloc=m_pWorld->m_threadData[iCaller].nPrecompPartsAllocLE;
const Quat oldQRot = m_qrot;
Vec3 pos,vel,pos0,vel0,newpos,move(ZERO),nslope,ncontactHist[4],ncontact,ptcontact,ncontactSum,BBoxInner[2],velGround,axis,sz,heightAdj=Vec3(0.f);
float movelen,tmin,vrel,movesum,kInertia;
le_precomp_entity *&pPrecompEnts=m_pWorld->m_threadData[iCaller].pTmpPrecompEntsLE;
le_precomp_part *&pPrecompParts=m_pWorld->m_threadData[iCaller].pTmpPrecompPartsLE;
le_tmp_contact *&pNoResponseContactLE=m_pWorld->m_threadData[iCaller].pTmpNoResponseContactLE, *pNRC;
le_contact unproj[8];
CCylinderGeom CylinderGeomOuter,*pCyl[2];
geom_world_data gwd[3];
intersection_params ip;
CPhysicalEntity **pentlist, *pentmin=NULL;
geom_contact *pcontacts;
pe_action_impulse ai;
pe_status_dynamics sd;
WriteLockCond lockMain(m_lockStep,m_bStateReading^1);
ip.bNoAreaContacts = true;
IF (nNoResponseAllocLE==0, 0) pNoResponseContactLE=new le_tmp_contact[nNoResponseAllocLE=16];
if (m_timeForceInertia>0.0001f)
kInertia = 6.0f;
else if (m_kInertiaAccel && m_velRequested.len2()>0.1f)
kInertia = m_kInertiaAccel;
else
kInertia = m_kInertia;
pos = m_pWorld->m_bWorldStep==2 ? m_pos : SyncWithGroundCollider(time_interval);
vel0=vel = m_vel;
bWasFlying = bFlying = m_bFlying;
if (!m_bStateReading) {
m_timeUseLowCap -= time_interval;
m_timeSinceStanceChange += time_interval;
m_timeSinceImpulseContact += time_interval;
m_timeForceInertia = max(0.0f,m_timeForceInertia-time_interval);
m_timeStepPerformed += time_interval;
}
if (m_bActive &&
(!(vel.len2()==0 && m_velRequested.len2()==0 && (!bFlying || m_gravity.len2()==0) && m_dhSpeed==0 && m_dhAcc==0) ||
m_bActiveEnvironment || m_nslope.z<m_slopeSlide || m_velGround.len2()>0))
{
FUNCTION_PROFILER( GetISystem(),PROFILE_PHYSICS );
PHYS_ENTITY_PROFILER
m_bActiveEnvironment = 0;
//m_nslope.Set(0,0,1);
if (kInertia==0 && !bFlying && !m_bJumpRequested || m_pWorld->m_vars.bFlyMode) {
vel = m_velRequested;
vel -= m_nslope*(m_nslope*vel);
}
if (bFlying && !m_pWorld->m_vars.bFlyMode && m_kAirControl>0) {
if (kInertia>0) {
Vec3 velDelta = m_velRequested * (m_kInertia*time_interval*m_kAirControl);
Vec3 velDiff = m_velRequested - vel;
const float kAirControlSelect = m_kAirControl-1.0f;
velDelta.x = (float)__fsel(-(velDiff.x * velDelta.x), 0.0f, velDelta.x);
velDelta.y = (float)__fsel(-(velDiff.y * velDelta.y), 0.0f, velDelta.y);
velDelta.z = (float)__fsel(-(velDiff.z * velDelta.z), 0.0f, velDelta.z);
velDelta.x = (float)__fsel(velDiff.x, min(velDiff.x, velDelta.x), max(velDiff.x, velDelta.x));
velDelta.y = (float)__fsel(velDiff.y, min(velDiff.y, velDelta.y), max(velDiff.y, velDelta.y));
velDelta.z = (float)__fsel(velDiff.z, min(velDiff.z, velDelta.z), max(velDiff.z, velDelta.z));
velDelta.y = (float)__fsel(kAirControlSelect, velDiff.y, velDelta.y);
velDelta.x = (float)__fsel(kAirControlSelect, velDiff.x, velDelta.x);
vel += velDelta;
} else if (m_gravity.len2()>0)
vel = m_gravity*(vel*m_gravity-m_velRequested*m_gravity)/m_gravity.len2()+m_velRequested;
else
vel = m_velRequested;
}
//filippo:m_forceFly is to let the game set a velocity no matter what is the status of the entity.
if (m_forceFly)
vel = m_velRequested;
else if (bFlying && !m_bSwimming && !m_pWorld->m_vars.bFlyMode)
move += m_gravity*sqr(time_interval)*0.5f;
if (vel.len2() > sqr(m_pWorld->m_vars.maxVelPlayers))
vel.normalize() *= m_pWorld->m_vars.maxVelPlayers;
move += vel*time_interval;
m_forceFly = false;
bUnprojected = 0;
axis = m_qrot*Vec3(0,0,1);
if (_isnan(move.len2())) //necessary? Is there any way that this can occur? - Rich S
return 1;
--m_bSquashed; m_bSquashed-=m_bSquashed>>31;
if (m_pWorld->m_vars.iCollisionMode!=0 && m_pWorld->m_vars.bFlyMode) { //Rich S - is bFlyMode a cheat? Can we disable in release?
pos+=move; bFlying=1; m_hLatest=0; ReleaseGroundCollider();
} else {
const float fSizeX = m_size.x;
const float fSizeZ = m_size.z;
movelen = move.len();
const float fGap = (movelen+m_pWorld->m_vars.maxContactGapPlayer)*1.5f;
const Vec3 posDiff = pos - m_pos;
BBoxInner[0] = m_BBox[0]+(posDiff)-Vec3(fGap,fGap,fGap);
BBoxInner[1] = m_BBox[1]+(posDiff)+Vec3(fGap,fGap,fGap);
const float fGap2 = max(10.0f*time_interval,fGap); // adds a safety margin of m_size.x width
const Vec3 BBoxOuter0 = m_BBox[0]+(posDiff)-Vec3(fGap2,fGap2,fGap2);
const Vec3 BBoxOuter1 = m_BBox[1]+(posDiff)+Vec3(fGap2,fGap2,fGap2);
nents = m_pWorld->GetEntitiesAround(BBoxOuter0,BBoxOuter1,
pentlist, m_collTypes|ent_independent|ent_triggers|ent_sort_by_mass, this, 0,iCaller);
if (m_vel.len2()) for(i=0;i<m_nColliders;i++) if (m_pColliders[i]->HasConstraintContactsWith(this,constraint_inactive))
m_pColliders[i]->Awake();
const float fMassInv = m_massinv;
for(i=j=bHasFastPhys=0,vrel=0; i<nents; i++) if (!m_pForeignData || pentlist[i]->m_pForeignData!=m_pForeignData){
Vec3 size = pentlist[i]->m_BBox[1]-pentlist[i]->m_BBox[0];
int bFastPhys = 0;
if (IgnoreCollision(m_collisionClass, pentlist[i]->m_collisionClass))
continue;
if (pentlist[i]->m_iSimClass==2) {
if (pentlist[i]->m_flags & ref_small_and_fast)
continue;
else if (pentlist[i]->GetMassInv()*0.4f<fMassInv) {
pentlist[i]->GetStatus(&sd);
vrel = max(vrel,sd.v.len()+sd.w.len()*max(max(size.x,size.y),size.z));;
bHasFastPhys |= (bFastPhys = isneg(fSizeX*0.2f-vrel*time_interval));
}
}
if (!bFastPhys && !AABB_overlap(pentlist[i]->m_BBox,BBoxInner) && size.len2()>0)
continue;
idmat = pentlist[i]->GetType();
if (idmat==PE_SOFT || idmat==PE_ROPE)
pentlist[i]->Awake();
else if (pentlist[i]->m_iSimClass<4 &&
(idmat!=PE_LIVING && !pentlist[i]->IgnoreCollisionsWith(this,1) ||
idmat==PE_LIVING && pentlist[i]->m_parts[0].flags&collider_flags &&
pentlist[i]!=this && m_pLivingEntToIgnore!=pentlist[i] && ((CLivingEntity*)pentlist[i])->m_pLivingEntToIgnore!=this))
{
if (pentlist[i]->m_iSimClass==1 && m_timeSinceImpulseContact<0.2f && pentlist[i]->GetMassInv()>0) {
int ipart; unsigned int flags;
for(ipart=0,flags=0; ipart<pentlist[i]->m_nParts; ipart++)
flags |= pentlist[i]->m_parts[ipart].flags;
if (flags & collider_flags)
pentlist[i]->Awake();
}
pentlist[j++] = pentlist[i];
}
}
nents = j;
pos0 = pos;
bStaticUnproj = bDynUnproj = 0;
newpos = pos+move;
IF (nents>nPrecompEntsAlloc, 0) delete[] pPrecompEnts, pPrecompEnts=new le_precomp_entity[nPrecompEntsAlloc=nents];
for(i=0; i<nents; i++) {
le_precomp_entity &ent = pPrecompEnts[nPrecompEnts++];
ent.BBox[0]=pentlist[i]->m_BBox[0];
ent.BBox[1]=pentlist[i]->m_BBox[1];
ent.sz=(sz=pentlist[i]->m_BBox[1]-pentlist[i]->m_BBox[0]);
ent.bCheckBBox=sz.len2()>0;
ent.massinv=pentlist[i]->GetMassInv();
ent.entType=pentlist[i]->GetType();
ent.iSimClass=pentlist[i]->m_iSimClass;
ent.pent=pentlist[i];
ent.nParts=pentlist[i]->m_nParts;
ent.ignoreCollisionsWith=pentlist[i]->IgnoreCollisionsWith(this,1);
ent.iPartsBegin=nPrecompParts;
ent.iPartsEnd=nPrecompParts+(nUsedPartsCount=pentlist[i]->GetUsedPartsCount(iCaller));
IF (ent.iPartsEnd>nPrecompPartsAlloc, 0) {
ReallocateList(pPrecompParts, nPrecompParts, ent.iPartsEnd+128);
nPrecompPartsAlloc=ent.iPartsEnd+128;
}
for(int j1=0; j1<nUsedPartsCount; j1++) {
le_precomp_part &part = pPrecompParts[nPrecompParts++];
part.BBox[0]=pentlist[i]->m_parts[part.ipart=j=pentlist[i]->GetUsedPart(iCaller,j1)].BBox[0];
part.BBox[1]=pentlist[i]->m_parts[j].BBox[1];
part.partrot=pentlist[i]->m_qrot*pentlist[i]->m_parts[j].q;
part.partoff=pentlist[i]->m_pos + pentlist[i]->m_qrot*pentlist[i]->m_parts[j].pos;
part.partscale=pentlist[i]->m_parts[j].scale;
part.partflags=pentlist[i]->m_parts[j].flags;
part.pgeom=pentlist[i]->m_parts[j].pPhysGeomProxy->pGeom;
part.surface_idx=pentlist[i]->m_parts[j].surface_idx;
ent.iLastPart=nPrecompParts-1;
}
}
// first, check if we need unprojection in the initial position
if (sqr(m_qrot.v.x)+sqr(m_qrot.v.y)<sqr(0.001f))
gwd[0].R.SetIdentity();
else
gwd[0].R = Matrix33(m_qrot);
gwd[0].centerOfMass = gwd[0].offset = pos + gwd[0].R*m_parts[0].pos;
gwd[0].v.zero(); gwd[0].w.zero(); // since we check a static character against potentially moving environment here
ip.vrel_min = fSizeX;
ip.time_interval = time_interval*2;
ip.maxUnproj = fSizeX*2.5f;
ip.ptOutsidePivot[0] = gwd[0].offset;
pCyl[0] = m_pCylinderGeom;
if (bHasFastPhys) {
cylinder cylOuter;
cylOuter.r = fSizeX+min(fSizeX*1.5f,vrel*time_interval);
cylOuter.hh = fSizeZ+min(fSizeZ,vrel*time_interval);
cylOuter.center.zero();
cylOuter.axis.Set(0,0,1);
CylinderGeomOuter.CreateCylinder(&cylOuter);
pCyl[1] = &CylinderGeomOuter;
}
retry_without_ground_sync:
if (m_parts[0].flagsCollider) {
for(i=nUnproj=0,ncontactSum.zero();i<nents;++i)
if (pPrecompEnts[i].entType!=PE_LIVING) {
const float minv = pPrecompEnts[i].massinv;
const int bHeavy = isneg(minv*0.4f-fMassInv);
const int iSimClass = pPrecompEnts[i].iSimClass;
CPhysicalEntity *const pent = pPrecompEnts[i].pent;
int bFastPhys = 0;
if (bHasFastPhys && bHeavy && iSimClass==2 && pent!=m_pLastGroundCollider) {
pent->GetStatus(&sd);
const Vec3 &sz0 = pPrecompEnts[i].sz;
vrel = max(vrel,sd.v.len()+sd.w.len()*max(max(sz0.x,sz0.y),sz0.z));
bFastPhys = isneg(fSizeX*0.2f-vrel*time_interval);
gwd[1].v = sd.v; gwd[1].w = sd.w;
gwd[1].centerOfMass = sd.centerOfMass;
} else {
gwd[1].v.zero(); gwd[1].w.zero();
}
for(int iCyl=0; iCyl<bFastPhys+m_nParts; iCyl++) {
int igwd,flagsCollider; IGeometry *pCurCyl;
if (iCyl<=bFastPhys) {
pCurCyl=pCyl[iCyl]; igwd=0; flagsCollider=collider_flags;
} else {
pCurCyl = m_parts[iCyl-bFastPhys].pPhysGeom->pGeom;
gwd[igwd=2].R = Matrix33(m_qrot*m_parts[iCyl-bFastPhys].q);
gwd[2].centerOfMass = gwd[2].offset = pos + m_qrot*m_parts[iCyl-bFastPhys].pos;
gwd[2].scale = m_parts[iCyl-bFastPhys].scale;
if (!((flagsCollider = m_parts[iCyl-bFastPhys].flagsCollider) & geom_colltype_solid))
continue;
bHasExtraParts = 1;
}
for(int j1=pPrecompEnts[i].iPartsBegin, bCheckBBox=pPrecompEnts[i].bCheckBBox; j1<pPrecompEnts[i].iPartsEnd; ++j1)
if (pPrecompParts[j1].partflags & flagsCollider && (!bCheckBBox || AABB_overlap(BBoxInner,pPrecompParts[j1].BBox))) {
//(pent->m_qrot*pentlist[i]->m_parts[j].q).getmatrix(gwd[1].R); //Q2M_IVO
gwd[1].R = Matrix33(pPrecompParts[j1].partrot);
gwd[1].offset = pPrecompParts[j1].partoff;
gwd[1].scale = pPrecompParts[j1].partscale;
j = pPrecompParts[j1].ipart;
//if (m_pWorld->m_pRenderer) m_pWorld->m_pRenderer->DrawGeometry(pent->m_parts[j].pPhysGeomProxy->pGeom, &gwd[1], 4);
if (icnt=pCurCyl->Intersect(pPrecompParts[j1].pgeom, gwd+igwd,gwd+1, &ip, pcontacts)) {
const uint32 uFlags = m_flags;
for(ncont=0; ncont<icnt-1 && pcontacts[ncont].dir*(pcontacts[ncont].pt-gwd[0].offset)>0; ncont++);
if ((pos0-m_pos).len2()>sqr(0.001f)) {//m_pos-posncont==icnt) {
gwd[0].offset+=m_pos-pos; move-=m_pos-pos; pos=m_pos;
pos0=m_pos; goto retry_without_ground_sync; // if we made SyncWithGroundCollider and it caused intersections, roll it back
//continue;
}
if (iCyl==0 && bHeavy && !(pPrecompParts[j1].partflags & geom_no_coll_response)) {
const int bVeryHeavyFlags = (~(-isneg(minv*2-fMassInv))) | pent->m_flags;
if (m_pWorld->m_bWorldStep==2) { // this means step induced by rigid bodies moving around
// if the entity is rigid, store the contact
int bPushOther;
if (bPushOther = iSimClass>0 && minv>0) {// && pent->m_iGroup==m_pWorld->m_iCurGroup) {
nUnproj = min(nUnproj+1,(int)(CRY_ARRAY_COUNT(unproj)));
unproj[nUnproj-1].pent = pent;
unproj[nUnproj-1].ipart = j;
unproj[nUnproj-1].pt = pcontacts[ncont].pt;
unproj[nUnproj-1].n = pcontacts[ncont].n;
unproj[nUnproj-1].center = gwd[0].offset;
unproj[nUnproj-1].penetration = pcontacts[ncont].t;
} else if (!(bVeryHeavyFlags & pef_cannot_squash_players)) {
m_bSquashed = min(5, m_bSquashed+5*isneg(ncontactSum*pcontacts[ncont].n+0.99f));
if (iSimClass>0)
ncontactSum = pcontacts[ncont].n;
}
// check previous contacts from this frame, register in entity if there are conflicts
for(icnt=0; icnt<nUnproj-bPushOther; icnt++) {
const float fUnprojDotContact = unproj[icnt].n*pcontacts[ncont].n;
if (fUnprojDotContact<0) {
RegisterUnprojContact(unproj[icnt]);
if (bPushOther)
RegisterUnprojContact(unproj[nUnproj-1]);
if (!((bVeryHeavyFlags|unproj[icnt].pent->m_flags) & pef_cannot_squash_players))
m_bSquashed = min(5, m_bSquashed+5*isneg(max(5*minv-fMassInv, fUnprojDotContact+0.99f)));
}
}
} else if (!(bVeryHeavyFlags & pef_cannot_squash_players)) {
m_bSquashed = min(5, m_bSquashed+5*isneg(ncontactSum*pcontacts[ncont].n+0.99f));
if (iSimClass>0)
ncontactSum = pcontacts[ncont].n;
}
if (uFlags & pef_pushable_by_players) {
(minv==0 ? bStaticUnproj:bDynUnproj)++;
Vec3 offs = pcontacts[ncont].dir*(pcontacts[ncont].t+m_pWorld->m_vars.maxContactGapPlayer);
pos += offs; gwd[0].offset += offs;
bUnprojected = 1;
if (pcontacts[ncont].t>m_size.x)
ip.ptOutsidePivot[0].Set(1E11f,1E11f,1E11f);
}
}
if (iSimClass==2) {
Matrix33 K;
K.SetZero();
if (uFlags & pef_pushable_by_players)
GetContactMatrix(pcontacts[ncont].center, 0, K);
int bPushOther = (uFlags & lef_push_objects) && (pent->m_flags & pef_pushable_by_players);
bPushOther |= iszero(iSimClass-2);
bPushOther &= iszero((int)(INT_PTR)pent-(int)(INT_PTR)m_pLastGroundCollider)^1;
if (bPushOther)
pent->GetContactMatrix(pcontacts[ncont].center, j, K);
else if (!(uFlags & pef_pushable_by_players))
continue;
pcontacts[ncont].center -= pcontacts[ncont].dir*pcontacts[ncont].t;
ncontact = -pcontacts[ncont].n;//(gwd[0].offset-pcontacts[ncont].center).normalized();
if (fabs_tpl(ncontact.z)<0.5f) {
ncontact.z=0; ncontact.normalize();
}
RigidBody *pbody = pent->GetRigidBody(j);
vrel = ncontact*(pbody->v+(pbody->w^pcontacts[ncont].center-pbody->pos)-vel-m_velGround);
if (iCyl==0 || fabs_tpl(vrel)*time_interval>m_size.x*0.2f) {
vrel = max(0.0f,vrel-ncontact*(vel+m_velGround));
float imp=vrel/max(1e-6f,ncontact*K*ncontact);
ai.impulse = ncontact*imp;
ai.point = pcontacts[ncont].center; ai.ipart = 0;
if (ai.impulse.len2()*sqr(fMassInv) > max(1.0f,sqr(vrel)))
ai.impulse.normalize() *= fabs_tpl(vrel)*m_mass*0.5f;
if ((uFlags & (pef_pushable_by_players|geom_no_coll_response)) == pef_pushable_by_players)
vel += ai.impulse*fMassInv;
/*if (vel.z>-5) {
vel.z = max(0.0f, vel.z); vel.z += ai.impulse.len()*fMassInv*0.1f;
}*/
bFlying = 1; m_timeFlying = 0;
if (m_kInertia==0)
m_timeForceInertia = m_timeImpulseRecover;
if (bPushOther) {
if (fabs_tpl(pcontacts[ncont].dir*axis)<0.7f)
ai.impulse -= axis*(axis*ai.impulse); // ignore vertical component - might be semi-random depending on cylinder intersection
ai.impulse.Flip(); ai.ipart = j;
//if (pent->GetType()<PE_LIVING)
// ai.impulse *= 0.2f;
pent->Action(&ai,1);
m_timeSinceImpulseContact = 0;
}
idmat = pPrecompParts[j1].surface_idx&pcontacts[ncont].id[1]>>31 | max(pcontacts[ncont].id[1],0);
RegisterContact(pos,pcontacts[ncont].pt,ncontact,pent,j,idmat,imp,0,pcontacts[ncont].iPrim[1]);
}
}
//break;
}
}
}
}
}
if (bStaticUnproj && bDynUnproj && m_bSquashed) {
pos = pos0; // disable unprojection if we are being squashed
bStaticUnproj = bDynUnproj = 0;
}
else if (bStaticUnproj+bDynUnproj>0) {
newpos = pos+move;
}
float h = ShootRayDown(pPrecompEnts,nents,pPrecompParts,newpos,nslope);
float hcur = newpos*axis-m_hPivot;
const float fAxisDotSlope = axis*nslope;
if (fAxisDotSlope>m_slopeFall &&
(hcur<h && hcur>h-(m_hCyl-m_size.z)*1.01f ||
hcur>h && sqr_signed(hcur-h)<vel.len2()*sqr(time_interval) && !bFlying && !m_bJumpRequested && !m_bSwimming))
{
if (h>hcur && m_nslope*axis<m_slopeSlide && m_nslope*nslope<m_slopeSlide &&
fAxisDotSlope<m_slopeSlide && m_velRequested.len2()==0)
{
newpos = pos; vel.zero();
} else
newpos += (heightAdj = axis*(h+m_hPivot-newpos*axis));
move = newpos-pos; movelen = move.len();
}
pos0 = pos;
if (m_bJumpRequested)
AddLegsImpulse(-vel,m_nslope,true);
m_bJumpRequested = 0;
m_bStuck = 0; ncontactSum.zero();
if (movelen>m_size.x*1E-4f && m_parts[0].flagsCollider!=0) {
ip.bSweepTest = true;
gwd[0].v = move/movelen;
int iter = 0;
float move0 = movelen; movesum = 0;
pe_player_dimensions pd;
do {
float tlim = 0.0f;
gwd[0].offset = pos + gwd[0].R*m_parts[0].pos;
ip.time_interval = movelen+m_pWorld->m_vars.maxContactGapPlayer; tmin = ip.time_interval*2; iPrim = -1;
pNRC = pNoResponseContactLE; pNRC->pent = NULL; pNRC->tmin = tmin;
for(i=0; i<nents; ++i) {
if (pPrecompEnts[i].entType!=PE_LIVING ||
pPrecompEnts[i].nParts - iszero((int)(pPrecompParts[pPrecompEnts[i].iLastPart].partflags & collider_flags)) > 1-bHasExtraParts ||
max(sqr(m_qrot.v.x)+sqr(m_qrot.v.y),sqr(pPrecompEnts[i].pent->m_qrot.v.x)+sqr(pPrecompEnts[i].pent->m_qrot.v.y))>0.001f)
{
CPhysicalEntity *const pent=pPrecompEnts[i].pent;
int bCheckBBox=pPrecompEnts[i].bCheckBBox;
for(int j1=pPrecompEnts[i].iPartsBegin; j1<pPrecompEnts[i].iPartsEnd; ++j1) {
const uint32 uPartFlags = pPrecompParts[j1].partflags;
if (uPartFlags & (collider_flags|geom_log_interactions) &&
(!bCheckBBox || AABB_overlap(BBoxInner,pPrecompParts[j1].BBox)))
{
if (uPartFlags & geom_log_interactions) {
EventPhysBBoxOverlap event;
event.pEntity[0]=this; event.pForeignData[0]=m_pForeignData; event.iForeignData[0]=m_iForeignData;
event.pEntity[1]=pent; event.pForeignData[1]=pent->m_pForeignData; event.iForeignData[1]=pent->m_iForeignData;
m_pWorld->OnEvent(m_flags, &event);
if (!(uPartFlags & collider_flags))
continue;
}
gwd[1].R = Matrix33(pPrecompParts[j1].partrot);
gwd[1].offset = pPrecompParts[j1].partoff;
gwd[1].scale = pPrecompParts[j1].partscale;
int ipart = 0;
if((ncont = m_pCylinderGeom->Intersect(pPrecompParts[j1].pgeom, gwd,gwd+1, &ip, pcontacts)))
got_contact:
if (pcontacts[ncont-1].t<tmin && pcontacts[ncont-1].n*gwd[0].v>0) {
if (!((pPrecompParts[j1].partflags | m_parts[ipart].flags)&geom_no_coll_response)) {
// Solid contact
tmin = pcontacts[ncont-1].t; tlim=0.0f; ncontact = -pcontacts[ncont-1].n; ptcontact = pcontacts[ncont-1].pt; iPrim = pcontacts[ncont-1].iPrim[1];
pentmin=pent; jmin=pPrecompParts[j1].ipart; idmat=pPrecompParts[j1].surface_idx&pcontacts[ncont-1].id[1]>>31 | max(pcontacts[ncont-1].id[1],0); ipartMin=ipart;
} else if (pcontacts[ncont-1].t<pNRC->tmin) {
// Non-response contact
pNRC->pent = pent; pNRC->tmin = pcontacts[ncont-1].t;
pNRC->ptcontact = pcontacts[ncont-1].pt; pNRC->ncontact = -pcontacts[ncont-1].n;
pNRC->ipart = pPrecompParts[j1].ipart; pNRC->iPrim = pcontacts[ncont-1].iPrim[1];
pNRC->idmat = pPrecompParts[j1].surface_idx&pcontacts[ncont-1].id[1]>>31 | max(pcontacts[ncont-1].id[1],0);
}
}
for(ipart++; ipart<(m_nParts&-bHasExtraParts); ipart++) if (m_parts[ipart].flagsCollider & uPartFlags) {
gwd[2].R = Matrix33(m_qrot*m_parts[ipart].q);
gwd[2].centerOfMass = gwd[2].offset = pos + m_qrot*m_parts[ipart].pos;
gwd[2].scale = m_parts[ipart].scale; gwd[2].v = gwd[0].v;
if((ncont = m_parts[ipart].pPhysGeomProxy->pGeom->Intersect(pPrecompParts[j1].pgeom, gwd+2,gwd+1, &ip, pcontacts)))
goto got_contact;
}
}
}
} else {
CPhysicalEntity *const pent=pPrecompEnts[i].pent;
pent->GetParams(&pd);
Vec2 dorigin,ncoll,move2d=(Vec2)gwd[0].v;
dorigin = Vec2(pos-pent->m_pos);
float kb=dorigin*move2d, kc=len2(dorigin)-sqr(m_size.x+pd.sizeCollider.x), kd=kb*kb-kc, zup0,zdown0,zup1,zdown1;
if (kd>=0) {
zup0 = (zdown0 = pos.z-m_hPivot)+m_hCyl+m_size.z+m_size.x*m_bUseCapsule;
zdown0 = max(zdown0, zup0-(m_size.x*m_bUseCapsule+m_size.z)*2);
zup1 = (zdown1 = pent->m_pos.z-pd.heightPivot)+pd.heightCollider+pd.sizeCollider.z+pd.sizeCollider.x*pd.bUseCapsule;
zdown1 = max(zdown1, zup1-(pd.sizeCollider.x*pd.bUseCapsule+pd.sizeCollider.z)*2);
kd=sqrt_tpl(kd);
float tfirst=-kb+kd; ncoll = Vec2(pos+gwd[0].v*tfirst-pent->m_pos);
bool bSideContact = min(zup0+gwd[0].v.z*tfirst,zup1)>max(zdown0+gwd[0].v.z*tfirst,zdown1);
if (tfirst>-m_size.x && tfirst<tmin && ncoll*move2d>=0 && bSideContact)
continue; // if entities separate during this timestep, ignore other collisions
tfirst=-kb-kd; ncoll = Vec2(pos+gwd[0].v*tfirst-pent->m_pos);
/*if (tfirst<-m_size.x || min(zup0+gwd[0].v.z*tfirst,zup1)<max(zdown0+gwd[0].v.z*tfirst,zdown1) || ncoll*move2d>=0) {
tfirst=-kb+kd; ncoll = Vec2(pos+gwd[0].v*tfirst-pent->m_pos);
}*/
if (tfirst>-m_size.x && tfirst<tmin && ncoll*move2d<0 && bSideContact) {
tlim = (tmin = tfirst)*iszero(iter); ncontact.Set(ncoll.x,ncoll.y,0).normalize(); ptcontact = pos+gwd[0].v*tfirst-ncontact*m_size.x;
pentmin=pent; jmin=0; idmat=m_surface_idx; iPrim=-1; ipartMin=0;
}
// also check for cap-cap contact
if (fabs_tpl(gwd[0].v.z)>m_size.z*1E-5f) {
int nSignZ = sgnnz(gwd[0].v.z); //Could be float?
zup0 = pos.z-m_hPivot+m_hCyl+(m_size.z+m_size.x*m_bUseCapsule)*nSignZ;
zdown1 = pent->m_pos.z-pd.heightPivot+pd.heightCollider-(pd.sizeCollider.z+pd.sizeCollider.x*pd.bUseCapsule)*nSignZ;
tfirst = zdown1-zup0;
if (inrange(tfirst,-m_size.x*gwd[0].v.z,tmin*gwd[0].v.z) &&
len2(dorigin*gwd[0].v.z+move2d*tfirst)<sqr((m_size.x+pd.sizeCollider.x)*gwd[0].v.z))
{
tmin = tfirst/gwd[0].v.z; ncontact.Set(0,0,-nSignZ); (ptcontact = pos+gwd[0].v*tfirst).z += m_size.z*nSignZ; //?
pentmin=pent; jmin=-1; idmat=m_surface_idx; iPrim=-1; ipartMin=0;
}
}
}
}
if (pNRC->pent) {
IF (pNRC>=(pNoResponseContactLE+nNoResponseAllocLE-1), 0) {
delete [] pNoResponseContactLE; pNoResponseContactLE=new le_tmp_contact[nNoResponseAllocLE+16];
pNRC=pNoResponseContactLE+nNoResponseAllocLE-1; nNoResponseAllocLE+=16;
}
++pNRC; pNRC->pent=NULL; pNRC->tmin=ip.time_interval*2;
}
}
if (tmin<=ip.time_interval) {
tmin = max(tlim,tmin-m_pWorld->m_vars.maxContactGapPlayer);
pos += gwd[0].v*tmin;
static const float g_cosSlide=cos_tpl(0.3f), g_sinSlide=sin_tpl(0.3f);
/*if (bFlying) {
if ((ncontact*axis)*(1-m_bSwimming)>g_cosSlide)
ncontact = axis*g_cosSlide + (pos-ptcontact-axis*(axis*(pos-ptcontact))).normalized()*g_sinSlide;
} else */
if (!bFlying && inrange(ncontact*axis, 0.85f,-0.1f) && (unsigned int)pentmin->m_iSimClass-1u<2u &&
pentmin->GetMassInv()>m_massinv*0.25f)
ncontact.z=0, ncontact.normalize();
int bPush = pentmin->m_iSimClass>0 || isneg(min(m_slopeClimb-ncontact*axis, ncontact*axis+m_slopeFall)) | bFlying;
int bUnmovable = isneg(-pentmin->m_iSimClass>>31 & ~(-((int)m_flags & pef_pushable_by_players)>>31));
bPush &= bUnmovable^1;
{
Matrix33 K;
K.SetZero();
int bPushOther = (m_flags & (pentmin->m_iSimClass==3 ? lef_push_players : lef_push_objects)) &&
(pentmin->m_flags & pef_pushable_by_players) &&
(pentmin->m_iSimClass | m_pWorld->m_vars.bPlayersCanBreak | pentmin->m_flags & pef_players_can_break);
bPushOther &= iszero((int)(INT_PTR)pentmin-(int)(INT_PTR)m_pLastGroundCollider)^1;
bPushOther |= bUnmovable;
if (bPushOther)
pentmin->GetContactMatrix(ptcontact, jmin, K);
if (!bPushOther || /*pentmin->m_iSimClass-3 | */m_flags & pef_pushable_by_players)
GetContactMatrix(ptcontact, -1, K);
else
bPush = 0;
vrel = ncontact*(vel+m_velGround); //(ncontact*gwd[0].v)*vel.len();
ai.impulse = ncontact;
if (pentmin->m_iSimClass==3) {
// make the player slide off when standing on other players
vrel -= ncontact*((CLivingEntity*)pentmin)->m_vel;
if (ncontact*axis > 0.95f) {
ai.impulse += (pos-pentmin->m_pos-axis*(axis*(pos-pentmin->m_pos))).normalized();
if (inrange(vrel,-1.0f,1.0f))
vrel = -1.0f;
}
} else {
RigidBody *pbody = pentmin->GetRigidBody(jmin);
vrel -= ncontact*(pbody->v+(pbody->w^ptcontact-pbody->pos));
}
vrel = min(0.0f, vrel);
float imp = -vrel*1.01f/max(1e-6f,ncontact*K*ncontact);
jmin -= jmin>>31;
if (bPush || m_flags & lef_report_sliding_contacts || pentmin->m_parts[jmin].flags & geom_manually_breakable)
RegisterContact(pos,ptcontact,ncontact,pentmin,jmin,idmat,imp*bPush,0,iPrim,ipartMin);
ai.impulse *= imp;
ai.point = ptcontact; ai.ipart = 0;
if (ai.impulse.len2()*sqr(m_massinv) > max(1.0f,sqr(vrel)))
ai.impulse.normalize() *= fabs_tpl(vrel)*m_mass*0.5f;
if (bPush) {
vel += ai.impulse*m_massinv;
if (m_kInertia==0 && (pentmin->m_iSimClass-3 | m_flags & pef_pushable_by_players))
m_timeForceInertia = m_timeImpulseRecover;
}
if (bPushOther) {
ai.impulse.Flip(); ai.ipart = jmin;
if (fabs_tpl(ncontact*axis)<0.7f)
ai.impulse -= axis*(axis*ai.impulse); // ignore vertical component - might be semi-random depending on cylinder intersection
ai.iApplyTime = isneg(pentmin->m_iSimClass-3)<<1;
if (pentmin->GetType()<PE_LIVING)
ai.impulse *= 0.2f;
pentmin->Action(&ai);
if (pentmin->m_iSimClass<3)
m_timeSinceImpulseContact = 0;
}
}
movelen -= tmin; movesum += tmin;
for(i=0;i<iter && ncontactHist[i]*ncontact<0.95f;i++);
if (i==iter)
ncontactSum += ncontact;
ncontactHist[iter] = ncontact;
if (iter==1 && movesum==0.0f) {
ncontact = (ncontactHist[0]^ncontactHist[1]).normalized();
gwd[0].v = ncontact*(gwd[0].v*ncontact);
} else {
gwd[0].v -= ncontact*(gwd[0].v*ncontact);
gwd[0].v = gwd[0].v*0.9998f+ncontact*(gwd[0].v.len()*0.02f);
}
tmin = gwd[0].v.len(); movelen*=tmin;
if (tmin>0) gwd[0].v/=tmin;
movelen = (float)__fsel(gwd[0].v*move, movelen, 0.0f);
} else {
pos += gwd[0].v*movelen; movesum += movelen; /* exit do-loop */ iter=1000;
}
for (le_tmp_contact* c=pNoResponseContactLE; c<=pNRC && c->pent; c++)
if (c->tmin<tmin)
RegisterContact(pos,c->ptcontact,c->ncontact,c->pent,c->ipart,c->idmat,0.f,0,c->iPrim);
} while(movelen>m_pWorld->m_vars.maxContactGapPlayer*0.1f && ++iter<3);
const float fContactSumLenSq = ncontactSum.len2();
if(movesum<move0*0.001f && (sqr_signed(ncontactSum.z)>sqr(0.4f)*fContactSumLenSq || fContactSumLenSq<0.6f))
m_bStuck = 1;
if (m_parts[0].flagsCollider!=0 && (bUnprojected || !(m_flags & lef_loosen_stuck_checks))) {
ip.bSweepTest = false;
gwd[0].offset = pos + gwd[0].R*m_parts[0].pos;
gwd[0].v = -axis;
ip.bStopAtFirstTri = true; ip.bNoBorder = true; ip.time_interval = m_size.z*10;
for(i=0; i<nents; ++i)
if (pPrecompEnts[i].iSimClass==0) {
CPhysicalEntity *const pent = pPrecompEnts[i].pent;
for(int j1=pPrecompEnts[i].iPartsBegin; j1<pPrecompEnts[i].iPartsEnd; ++j1)
if (pPrecompParts[j1].partflags & collider_flags && !(pPrecompParts[j1].partflags & geom_no_coll_response)) {
gwd[1].R = Matrix33(pPrecompParts[j1].partrot);
gwd[1].offset = pPrecompParts[j1].partoff;
gwd[1].scale = pPrecompParts[j1].partscale;
if(m_pCylinderGeom->Intersect(pPrecompParts[j1].pgeom, gwd,gwd+1, &ip, pcontacts)) {
if (pcontacts->t>m_pWorld->m_vars.maxContactGapPlayer)
vel.zero(),m_bStuck=1;
pos = pos0; m_timeUseLowCap=1.0f;
goto nomove;
}
}
} nomove:;
}
} else
pos += move;
if (!m_pLastGroundCollider)// || m_pLastGroundCollider->GetMassInv()>m_massinv)
velGround.zero();
else
velGround = m_velGround;
m_hLatest = h = ShootRayDown(pPrecompEnts, nents, pPrecompParts, pos,nslope, time_interval,false,true);
if (nslope*axis<0.087f)
nslope = m_nslope;
else {
WriteLockCond lock(m_lockLiving,m_bStateReading^1);
m_nslope = nslope;
}
if (m_pLastGroundCollider) {
RegisterContact(newpos,newpos,m_qrot*Vec3(0,0,1),m_pLastGroundCollider,m_iLastGroundColliderPart,m_lastGroundSurfaceIdx,0,1,m_lastGroundPrim);
if (m_pLastGroundCollider->m_iSimClass==0)
ReleaseGroundCollider();
}
if (bFlying)
m_timeFlying += time_interval;
int bGroundContact = isneg(max(pos*axis-m_hPivot-(h+m_groundContactEps), m_slopeFall-nslope*axis));
if (!bGroundContact)
ReleaseGroundCollider();
bFlying = m_pWorld->m_vars.bFlyMode || m_gravity*axis>0 || m_bSwimming || ((bGroundContact|m_bStuck)^1);
m_bActiveEnvironment = m_bStuck;
if (bFlying)
Step_HandleFlying(vel, velGround, bWasFlying, heightAdj, kInertia, time_interval);
else {
if (bWasFlying)
Step_HandleWasFlying(vel, bFlying, axis, bGroundContact);
Vec3 velReq = m_velRequested,g;
if (!m_bSwimming) velReq -= m_nslope*(velReq*m_nslope);
if (kInertia * time_interval > 1.0f) kInertia = 1.0f/time_interval;
Vec3 last_force = (velReq-vel)*kInertia;
const float axisSlope = m_nslope*axis;
if (axisSlope<m_slopeSlide && !m_bSwimming) {
g = m_gravity;
last_force += g-m_nslope*(g*m_nslope);
}
const Vec3 velIncLastForce = vel + (last_force*time_interval);
if (velIncLastForce*vel<0 && velIncLastForce*m_velRequested<=0)
vel.zero();
else
vel = velIncLastForce;
if (axisSlope<m_slopeClimb) {
const float axisVel = vel*axis;
if (axisVel>0 && last_force*axis>0)
vel -= axis*(axisVel);
if ((pos-pos0)*axis > m_size.z*0.001f)
vel -= axis*(axis*vel);
}
if (axisSlope<m_slopeFall && !m_bStuck) {
bFlying=1; vel += m_nslope-axis*(axisSlope);
}
if (m_velRequested.len2()==0 && vel.len2()<0.001f || vel.len2()<0.0001f)
vel.zero();
}
if (!bFlying)
m_timeFlying = 0;
if (m_flags & lef_snap_velocities)
vel = DecodeVec6b(EncodeVec6b(vel));
if (!m_bStateReading) {
float dh;
if (!bFlying && (dh=(pos-pos0)*axis)>m_size.z*0.01f) {
m_dhSpeed = max(m_dhSpeed, dh/m_stablehTime);
m_dh += dh;
m_stablehTime = 0;
} else
dh = 0;
m_stablehTime = min(m_stablehTime+time_interval,0.5f);
m_dhSpeed += m_dhAcc*time_interval;
if (m_dhAcc==0 && m_dh*m_dhSpeed<0 || m_dh*m_dhAcc<0 || m_dh*(m_dh-m_dhSpeed*time_interval)<0)
m_dh = m_dhSpeed = m_dhAcc = 0;
else
m_dh -= m_dhSpeed*time_interval;
}
if (m_pHeadGeom) {
ip.bSweepTest = true;
gwd[0].offset = pos + gwd[0].R*m_parts[0].pos;
gwd[0].v = axis;
tmin = ip.time_interval = m_hHead-m_hCyl-min(m_dh,0.0f);
for(i=0;i<nents;i++) if (pentlist[i]->m_iSimClass==0) {//pentlist[i]->GetType()!=PE_LIVING && pentlist[i]->GetMassInv()*0.4f<m_massinv) {
for(int j1=0;j1<pentlist[i]->GetUsedPartsCount(iCaller);j1++)
if (pentlist[i]->m_parts[j=pentlist[i]->GetUsedPart(iCaller,j1)].flags & collider_flags) {
Matrix33 rotMat = Matrix33(pentlist[i]->m_qrot*pentlist[i]->m_parts[j].q);
gwd[1].R = rotMat;
gwd[1].offset = pentlist[i]->m_pos + rotMat*pentlist[i]->m_parts[j].pos;
gwd[1].scale = pentlist[i]->m_parts[j].scale;
if(m_pHeadGeom->Intersect(pentlist[i]->m_parts[j].pPhysGeomProxy->pGeom, gwd,gwd+1, &ip, pcontacts))
tmin = min(tmin,(float)pcontacts[0].t);
}
}
if (m_dh<ip.time_interval+min(m_dh,0.0f)-tmin || fabs_tpl(m_dhSpeed)+fabs_tpl(m_dhAcc)==0)
m_dh = ip.time_interval+min(m_dh,0.0f)-tmin;
}
}
coord_block_BBox partCoord;
ComputeBBoxLE(pos,BBoxInner,&partCoord);
UpdatePosition(pos,BBoxInner, m_pWorld->RepositionEntity(this,1,BBoxInner));
bMoving = 1;
} else if (!m_bActive) {
if (m_velRequested.len2()>0) {
m_pos += m_velRequested*time_interval;
m_BBox[0] += m_velRequested*time_interval; m_BBox[1] += m_velRequested*time_interval;
JobAtomicAdd(&m_pWorld->m_lockGrid,-m_pWorld->RepositionEntity(this,1));
bMoving = 1;
}
if (m_bReleaseGroundColliderWhenNotActive!=0)
ReleaseGroundCollider();
}
{ WriteLockCond lock(m_lockLiving,m_bStateReading^1);
//if (m_pWorld->m_vars.bMultiplayer)
// m_pos = CompressPos(m_pos);
m_deltaV = (m_vel - vel0);
m_vel = vel+m_vel-vel0;
m_bFlying = bFlying;
m_deltaQRot = m_qrot * !oldQRot;
m_timeSmooth = (float)__fsel(-time_interval, m_timeSmooth, time_interval);
if (m_pWorld->m_bUpdateOnlyFlagged) {
m_deltaPos = m_posLocal-m_pos;
if (m_deltaPos.len2()<sqr(0.01f) || m_deltaPos.len2()>sqr(2.0f))
m_deltaPos.zero();
}
if (m_pBody)
if (!m_nColliders) {
delete m_pBody; m_pBody=0;
} else {
m_pBody->pos=m_pos+m_qrot*Vec3(0,0,m_hCyl); m_pBody->q=m_qrot;
m_pBody->P=(m_pBody->v=m_vel)*(m_pBody->M=m_mass);
m_pBody->Minv=m_massinv;
if (m_pWorld->m_timePhysics > m_timeRotChanged+0.05f)
m_pBody->w.zero(), m_pBody->L.zero();
/*quaternionf dq = m_history[m_iHist].q*!m_history[m_iHist-3&m_szHistory-1].q;
float dt=0; for(i=0; i<4; i++)
dt += m_history[m_iHist-i&m_szHistory-1].dt;
if (inrange(dt, 0.0f,1.0f)) {
if (dq.v.len2()<sqr(0.05f))
m_pBody->w = dq.v*(2/dt);
else
m_pBody->w = dq.v.normalized()*(acos_tpl(dq.w)*2/dt);
}*/
}
}
if (!m_bStateReading) {
if( bMoving ) {
Vec3 gravity; MARK_UNUSED gravity;
pe_params_buoyancy pb;
m_pWorld->CheckAreas(this,gravity,&pb,0);
if (!is_unused(gravity))
m_gravity = gravity;
if (m_pWorld->m_pWaterMan)
m_pWorld->m_pWaterMan->OnWaterInteraction(this);
}
if (m_flags & (pef_monitor_poststep | pef_log_poststep)) {
EventPhysPostStep epps;
epps.pEntity=this; epps.pForeignData=m_pForeignData; epps.iForeignData=m_iForeignData;
epps.dt=time_interval; epps.pos=m_pos; epps.q=m_qrot; epps.idStep=m_pWorld->m_idStep;
epps.pos -= m_qrot*Vec3(0,0,m_dh);
m_pWorld->OnEvent(m_flags,&epps);
}
/*if (m_pWorld->m_iLastLogPump > m_timeLogged)
m_posLogged = pos0;
m_timeLogged = m_pWorld->m_iLastLogPump;*/
}
return 1;
}
