static CBaseEntity *FindPhysicsBlocker( IPhysicsObject *pPhysics )
{
IPhysicsFrictionSnapshot *pSnapshot = pPhysics->CreateFrictionSnapshot();
CBaseEntity *pBlocker = NULL;
float maxVel = 10.0f;
while ( pSnapshot->IsValid() )
{
IPhysicsObject *pOther = pSnapshot->GetObject(1);
if ( pOther->IsMoveable() )
{
CBaseEntity *pOtherEntity = static_cast<CBaseEntity *>(pOther->GetGameData());
// dot with this if you have a direction
//Vector normal;
//pSnapshot->GetSurfaceNormal(normal);
float force = pSnapshot->GetNormalForce();
float vel = force * pOther->GetInvMass();
if ( vel > maxVel )
{
pBlocker = pOtherEntity;
maxVel = vel;
}
}
pSnapshot->NextFrictionData();
}
pPhysics->DestroyFrictionSnapshot( pSnapshot );
return pBlocker;
}
static bool InContactWithHeavyObject( IPhysicsObject *pObject, float heavyMass )
{
bool contact = false;
IPhysicsFrictionSnapshot *pSnapshot = pObject->CreateFrictionSnapshot();
while ( pSnapshot->IsValid() )
{
IPhysicsObject *pOther = pSnapshot->GetObject( 1 );
if ( !pOther->IsMoveable() || pOther->GetMass() > heavyMass )
{
contact = true;
break;
}
pSnapshot->NextFrictionData();
}
pObject->DestroyFrictionSnapshot( pSnapshot );
return contact;
}
void PhysForceClearVelocity( IPhysicsObject *pPhys )
{
IPhysicsFrictionSnapshot *pSnapshot = pPhys->CreateFrictionSnapshot();
// clear the velocity of the rigid body
Vector vel;
AngularImpulse angVel;
vel.Init();
angVel.Init();
pPhys->SetVelocity( &vel, &angVel );
// now clear the "strain" stored in the contact points
while ( pSnapshot->IsValid() )
{
pSnapshot->ClearFrictionForce();
pSnapshot->RecomputeFriction();
pSnapshot->NextFrictionData();
}
pPhys->DestroyFrictionSnapshot( pSnapshot );
}
//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
bool CTriggerPortal::IsTouchingPortal( CBaseEntity *pEntity )
{
// First, check the touchlinks. This will find non-vphysics entities touching us
touchlink_t *root = ( touchlink_t * )GetDataObject( TOUCHLINK );
if ( root )
{
for ( touchlink_t *link = root->nextLink; link != root; link = link->nextLink )
{
CBaseEntity *pTouch = link->entityTouched;
if ( pTouch == pEntity )
return true;
}
}
// Then check the friction snapshot. This will find vphysics objects touching us.
IPhysicsObject *pPhysics = VPhysicsGetObject();
if ( !pPhysics )
return false;
IPhysicsFrictionSnapshot *pSnapshot = pPhysics->CreateFrictionSnapshot();
bool bFound = false;
while ( pSnapshot->IsValid() )
{
IPhysicsObject *pOther = pSnapshot->GetObject( 1 );
if ( ((CBaseEntity *)pOther->GetGameData()) == pEntity )
{
bFound = true;
break;
}
pSnapshot->NextFrictionData();
}
pPhysics->DestroyFrictionSnapshot( pSnapshot );
return bFound;
}
bool PhysHasContactWithOtherInDirection( IPhysicsObject *pPhysics, const Vector &dir )
{
bool hit = false;
void *pGameData = pPhysics->GetGameData();
IPhysicsFrictionSnapshot *pSnapshot = pPhysics->CreateFrictionSnapshot();
while ( pSnapshot->IsValid() )
{
IPhysicsObject *pOther = pSnapshot->GetObject( 1 );
if ( pOther->GetGameData() != pGameData )
{
Vector normal;
pSnapshot->GetSurfaceNormal( normal );
if ( DotProduct(normal,dir) > 0 )
{
hit = true;
break;
}
}
pSnapshot->NextFrictionData();
}
pPhysics->DestroyFrictionSnapshot( pSnapshot );
return hit;
}
bool CPhysicsNPCSolver::CheckTouching()
{
CAI_BaseNPC *pNPC = m_hNPC.Get();
if ( !pNPC )
return false;
CBaseEntity *pPhysicsEnt = m_hEntity.Get();
if ( !pPhysicsEnt )
return false;
IPhysicsObject *pPhysics = pPhysicsEnt->VPhysicsGetObject();
IPhysicsObject *pNPCPhysics = pNPC->VPhysicsGetObject();
if ( !pNPCPhysics || !pPhysics )
return false;
IPhysicsFrictionSnapshot *pSnapshot = pPhysics->CreateFrictionSnapshot();
bool found = false;
bool penetrate = false;
while ( pSnapshot->IsValid() )
{
IPhysicsObject *pOther = pSnapshot->GetObject(1);
if ( pOther == pNPCPhysics )
{
found = true;
if ( IsContactOnNPCHead(pSnapshot, pPhysics, pNPC ) )
{
penetrate = true;
pSnapshot->MarkContactForDelete();
}
break;
}
pSnapshot->NextFrictionData();
}
pSnapshot->DeleteAllMarkedContacts( true );
pPhysics->DestroyFrictionSnapshot( pSnapshot );
// if the object is penetrating something, check to see if it's intersecting this NPC
// if so, go ahead and switch over to penetration solver mode
if ( !penetrate && (pPhysics->GetGameFlags() & FVPHYSICS_PENETRATING) )
{
penetrate = IsIntersecting();
}
if ( penetrate )
{
pPhysicsEnt->ClearNavIgnore();
BecomePenetrationSolver();
}
return found;
}
float CalculateObjectStress( IPhysicsObject *pObject, CBaseEntity *pInputOwnerEntity, vphysics_objectstress_t *pOutput )
{
CUtlVector< CBaseEntity * > pObjectList;
CUtlVector< Vector > objectForce;
bool hasLargeObject = false;
// add a slot for static objects
pObjectList.AddToTail( NULL );
objectForce.AddToTail( vec3_origin );
// add a slot for friendly objects
pObjectList.AddToTail( NULL );
objectForce.AddToTail( vec3_origin );
CBaseCombatCharacter *pBCC = pInputOwnerEntity->MyCombatCharacterPointer();
IPhysicsFrictionSnapshot *pSnapshot = pObject->CreateFrictionSnapshot();
float objMass = pObject->GetMass();
while ( pSnapshot->IsValid() )
{
float force = pSnapshot->GetNormalForce();
if ( force > 0.0f )
{
IPhysicsObject *pOther = pSnapshot->GetObject(1);
CBaseEntity *pOtherEntity = static_cast<CBaseEntity *>(pOther->GetGameData());
if ( !pOtherEntity )
{
// object was just deleted, but we still have a contact point this frame...
// just assume it came from the world.
pOtherEntity = GetWorldEntity();
}
CBaseEntity *pOtherOwner = pOtherEntity;
if ( pOtherEntity->GetOwnerEntity() )
{
pOtherOwner = pOtherEntity->GetOwnerEntity();
}
int outIndex = 0;
if ( !pOther->IsMoveable() )
{
outIndex = 0;
}
// NavIgnored objects are often being pushed by a friendly
else if ( pBCC && (pBCC->IRelationType( pOtherOwner ) == D_LI || pOtherEntity->IsNavIgnored()) )
{
outIndex = 1;
}
// player held objects do no stress
else if ( pOther->GetGameFlags() & FVPHYSICS_PLAYER_HELD )
{
outIndex = 1;
}
else
{
if ( pOther->GetMass() >= VPHYSICS_LARGE_OBJECT_MASS )
{
if ( pInputOwnerEntity->GetGroundEntity() != pOtherEntity)
{
hasLargeObject = true;
}
}
// moveable, non-friendly
// aggregate contacts over each object to avoid greater stress in multiple contact cases
// NOTE: Contacts should be in order, so this shouldn't ever search, but just in case
outIndex = pObjectList.Count();
for ( int i = pObjectList.Count()-1; i >= 2; --i )
{
if ( pObjectList[i] == pOtherOwner )
{
outIndex = i;
break;
}
}
if ( outIndex == pObjectList.Count() )
{
pObjectList.AddToTail( pOtherOwner );
objectForce.AddToTail( vec3_origin );
}
}
if ( outIndex != 0 && pInputOwnerEntity->GetMoveType() != MOVETYPE_VPHYSICS && !IsPhysicallyControlled(pOtherEntity, pOther) )
{
// UNDONE: Test this! This is to remove any shadow/shadow stress. The game should handle this with blocked/damage
force = 0.0f;
}
Vector normal;
pSnapshot->GetSurfaceNormal( normal );
objectForce[outIndex] += normal * force;
}
pSnapshot->NextFrictionData();
}
pObject->DestroyFrictionSnapshot( pSnapshot );
pSnapshot = NULL;
// clear out all friendly force
objectForce[1].Init();
float sum = 0;
Vector negativeForce = vec3_origin;
Vector positiveForce = vec3_origin;
Assert( pObjectList.Count() == objectForce.Count() );
for ( int objectIndex = pObjectList.Count()-1; objectIndex >= 0; --objectIndex )
{
sum += objectForce[objectIndex].Length();
for ( int i = 0; i < 3; i++ )
{
if ( objectForce[objectIndex][i] < 0 )
{
negativeForce[i] -= objectForce[objectIndex][i];
}
else
{
positiveForce[i] += objectForce[objectIndex][i];
}
}
}
// "external" stress is two way (something pushes on the object and something else pushes back)
// so the set of minimum values per component are the projections of the two-way force
// "internal" stress is one way (the object is pushing against something OR something pushing back)
// the momentum must have come from inside the object (gravity, controller, etc)
Vector internalForce = vec3_origin;
Vector externalForce = vec3_origin;
for ( int i = 0; i < 3; i++ )
{
if ( negativeForce[i] < positiveForce[i] )
{
internalForce[i] = positiveForce[i] - negativeForce[i];
externalForce[i] = negativeForce[i];
}
else
{
internalForce[i] = negativeForce[i] - positiveForce[i];
externalForce[i] = positiveForce[i];
}
}
// sum is kg in / s
Vector gravVector;
physenv->GetGravity( &gravVector );
float gravity = gravVector.Length();
if ( pInputOwnerEntity->GetMoveType() != MOVETYPE_VPHYSICS && pObject->IsMoveable() )
{
Vector lastVel;
lastVel.Init();
if ( pObject->GetShadowController() )
{
pObject->GetShadowController()->GetLastImpulse( &lastVel );
}
else
{
if ( ( pObject->GetCallbackFlags() & CALLBACK_IS_PLAYER_CONTROLLER ) )
{
CBasePlayer *pPlayer = ToBasePlayer( pInputOwnerEntity );
IPhysicsPlayerController *pController = pPlayer ? pPlayer->GetPhysicsController() : NULL;
if ( pController )
{
pController->GetLastImpulse( &lastVel );
}
}
}
// Work in progress...
// Peek into the controller for this object. Look at the input velocity and make sure it's all
// accounted for in the computed stress. If not, redistribute external to internal as it's
// probably being reflected in a way we can't measure here.
float inputLen = lastVel.Length() * (1.0f / physenv->GetSimulationTimestep()) * objMass;
if ( inputLen > 0.0f )
{
float internalLen = internalForce.Length();
if ( internalLen < inputLen )
{
float ratio = internalLen / inputLen;
Vector delta = internalForce * (1.0f - ratio);
internalForce += delta;
float deltaLen = delta.Length();
sum -= deltaLen;
float extLen = VectorNormalize(externalForce) - deltaLen;
if ( extLen < 0 )
{
extLen = 0;
}
externalForce *= extLen;
}
}
}
float invGravity = gravity;
if ( invGravity <= 0 )
{
invGravity = 1.0f;
}
else
{
invGravity = 1.0f / invGravity;
}
sum *= invGravity;
internalForce *= invGravity;
externalForce *= invGravity;
if ( !pObject->IsMoveable() )
{
// the above algorithm will see almost all force as internal if the object is not moveable
// (it doesn't push on anything else, so nothing is reciprocated)
// exceptions for friction of a single other object with multiple contact points on this object
// But the game wants to see it all as external because obviously the object can't move, so it can't have
// internal stress
externalForce = internalForce;
internalForce.Init();
if ( !pObject->IsStatic() )
{
sum += objMass;
}
}
else
{
// assume object is at rest
if ( sum > objMass )
{
sum = objMass + (sum-objMass) * 0.5;
}
}
if ( pOutput )
{
pOutput->exertedStress = internalForce.Length();
pOutput->receivedStress = externalForce.Length();
pOutput->hasNonStaticStress = pObjectList.Count() > 2 ? true : false;
pOutput->hasLargeObjectContact = hasLargeObject;
}
// sum is now kg
return sum;
}
void PhysComputeSlideDirection( IPhysicsObject *pPhysics, const Vector &inputVelocity, const AngularImpulse &inputAngularVelocity,
Vector *pOutputVelocity, Vector *pOutputAngularVelocity, float minMass )
{
Vector velocity = inputVelocity;
AngularImpulse angVel = inputAngularVelocity;
Vector pos;
IPhysicsFrictionSnapshot *pSnapshot = pPhysics->CreateFrictionSnapshot();
while ( pSnapshot->IsValid() )
{
IPhysicsObject *pOther = pSnapshot->GetObject( 1 );
if ( !pOther->IsMoveable() || pOther->GetMass() > minMass )
{
Vector normal;
pSnapshot->GetSurfaceNormal( normal );
// BUGBUG: Figure out the correct rotation clipping equation
if ( pOutputAngularVelocity )
{
angVel = normal * DotProduct( angVel, normal );
#if 0
pSnapshot->GetContactPoint( point );
Vector point, dummy;
AngularImpulse angularClip, clip2;
pPhysics->CalculateVelocityOffset( normal, point, dummy, angularClip );
VectorNormalize( angularClip );
float proj = DotProduct( angVel, angularClip );
if ( proj > 0 )
{
angVel -= angularClip * proj;
}
CrossProduct( angularClip, normal, clip2 );
proj = DotProduct( angVel, clip2 );
if ( proj > 0 )
{
angVel -= clip2 * proj;
}
//NDebugOverlay::Line( point, point - normal * 20, 255, 0, 0, true, 0.1 );
#endif
}
// Determine how far along plane to slide based on incoming direction.
// NOTE: Normal points away from this object
float proj = DotProduct( velocity, normal );
if ( proj > 0.0f )
{
velocity -= normal * proj;
}
}
pSnapshot->NextFrictionData();
}
pPhysics->DestroyFrictionSnapshot( pSnapshot );
//NDebugOverlay::Line( pos, pos + unitVel * 20, 0, 0, 255, true, 0.1 );
if ( pOutputVelocity )
{
*pOutputVelocity = velocity;
}
if ( pOutputAngularVelocity )
{
*pOutputAngularVelocity = angVel;
}
}
