bool CIntersectionAssistanceUnit::GetHighestScoringLastKnownGoodPosition( const QuatT& baseOrientation, QuatT& outQuat ) const
{
bool bFlippedIsBest = false;
if(!m_lastKnownGoodPositions.empty())
{
// Higher is better
float fBestScore = 0.0f;
int bestIndex = -1;
Vec3 vBaseUpDir = baseOrientation.q.GetColumn2().GetNormalized();
for(uint8 i = 0; i < m_lastKnownGoodPositions.size(); ++i)
{
const QuatT& qLastKnownGood = m_lastKnownGoodPositions[i];
if(IsPositionWithinAcceptedLimits(qLastKnownGood.t, baseOrientation.t, kDistanceTolerance))
{
// Generate [0.0f,1.0f] score for distance
const Vec3 distVec = (qLastKnownGood.t - baseOrientation.t);
const float length = max(distVec.GetLengthFast(),0.0001f);
const float distanceScore = max(1.0f - (length * kInverseDistanceTolerance) * kDistanceWeight, 0.0f);
Vec3 vUpDir = qLastKnownGood.q.GetColumn2();
const float regularOrientationScore = vBaseUpDir.Dot(vUpDir);
const float flippedOrientationScore = vBaseUpDir.Dot(-vUpDir);
float orientationScore = max(regularOrientationScore, flippedOrientationScore);
orientationScore *= kOrientationWeight;
const float fCandidateScore = distanceScore + orientationScore;
#ifndef _RELEASE
if(g_pGameCVars->pl_pickAndThrow.intersectionAssistDebugEnabled == 2)
{
CryWatch("[INDEX(%d)] : D[%.3f] O[%.3f] T[%.3f] (%s)", i, distanceScore, orientationScore, fCandidateScore, flippedOrientationScore > regularOrientationScore ? "*F*" : "R");
}
#endif //#ifndef _RELEASE
if(fCandidateScore > fBestScore)
{
bestIndex = i;
fBestScore = fCandidateScore;
bFlippedIsBest = (flippedOrientationScore > regularOrientationScore);
}
}
}
if(bestIndex >= 0)
{
outQuat = m_lastKnownGoodPositions[bestIndex];
if(bFlippedIsBest)
{
Matrix34 wMat(outQuat);
Vec3 vFlippedUpDir = -outQuat.q.GetColumn2().GetNormalized();
Vec3 vForwardDir = outQuat.q.GetColumn1().GetNormalized();
Vec3 vSideDir = -outQuat.q.GetColumn0().GetNormalized();
Matrix34 wFlippedMat;
wFlippedMat = Matrix34::CreateFromVectors(vSideDir, vForwardDir, vFlippedUpDir, wMat.GetTranslation());
outQuat = QuatT(wFlippedMat);
// Adjust pos (rotating around OOBB centre effectively)
const IEntity* pSubjectEntity = gEnv->pEntitySystem->GetEntity(m_subjectEntityId);
if(pSubjectEntity)
{
AABB entAABB;
OBB entOBB;
pSubjectEntity->GetLocalBounds(entAABB);
entOBB.SetOBBfromAABB(Quat(IDENTITY), entAABB);
Vec3 Centre = wMat.TransformPoint(entOBB.c);
Vec3 toCentre = Centre - outQuat.t;
outQuat.t += (toCentre * 2.0f);
}
}
#ifndef _RELEASE
if(g_pGameCVars->pl_pickAndThrow.intersectionAssistDebugEnabled == 2)
{
m_currentBestIndex = bestIndex;
CryWatch("[BEST INDEX] : %d", bestIndex);
}
#endif // ifndef _RELEASE
return true;
}
}
#ifndef _RELEASE
m_currentBestIndex = -1;
#endif // ifndef _RELEASE
return false;
}
void CPlayerStateUtil::UpdatePlayerPhysicsStats( CPlayer& player, SActorPhysics& actorPhysics, float frameTime )
{
const int currentFrameID = gEnv->pRenderer->GetFrameID();
if( actorPhysics.lastFrameUpdate < currentFrameID )
{
pe_status_living livStat;
if( !CPlayerStateUtil::GetPhysicsLivingStat( player, &livStat ) )
{
return;
}
SPlayerStats& stats = *player.GetActorStats();
const Vec3 newVelocity = livStat.vel-livStat.velGround;
actorPhysics.velocityDelta = newVelocity - actorPhysics.velocity;
actorPhysics.velocity = newVelocity;
actorPhysics.velocityUnconstrainedLast = actorPhysics.velocityUnconstrained;
actorPhysics.velocityUnconstrained = livStat.velUnconstrained;
actorPhysics.flags.SetFlags( SActorPhysics::EActorPhysicsFlags_WasFlying, actorPhysics.flags.AreAnyFlagsActive(SActorPhysics::EActorPhysicsFlags_Flying) );
actorPhysics.flags.SetFlags( SActorPhysics::EActorPhysicsFlags_Flying, livStat.bFlying > 0 );
actorPhysics.flags.SetFlags( SActorPhysics::EActorPhysicsFlags_Stuck, livStat.bStuck > 0 );
Vec3 flatVel(player.m_pPlayerRotation->GetBaseQuat().GetInverted()*newVelocity);
flatVel.z = 0;
stats.speedFlat = flatVel.len();
if(player.IsInAir())
{
stats.maxAirSpeed = max(stats.maxAirSpeed, newVelocity.GetLengthFast());
}
else
{
stats.maxAirSpeed = 0.f;
}
float fSpeedFlatSelector = stats.speedFlat - 0.1f;
const float groundNormalBlend = clamp(frameTime * 6.666f, 0.0f, 1.0f);
actorPhysics.groundNormal = LERP(actorPhysics.groundNormal, livStat.groundSlope, groundNormalBlend);
if (livStat.groundSurfaceIdxAux > 0)
actorPhysics.groundMaterialIdx = livStat.groundSurfaceIdxAux;
else
actorPhysics.groundMaterialIdx = livStat.groundSurfaceIdx;
actorPhysics.groundHeight = livStat.groundHeight;
EntityId newGroundColliderId = 0;
if (livStat.pGroundCollider)
{
IEntity* pEntity = gEnv->pEntitySystem->GetEntityFromPhysics(livStat.pGroundCollider);
newGroundColliderId = pEntity ? pEntity->GetId() : 0;
}
if( newGroundColliderId != actorPhysics.groundColliderId )
{
if( actorPhysics.groundColliderId )
{
if( IGameObject* pGameObject = gEnv->pGame->GetIGameFramework()->GetGameObject( actorPhysics.groundColliderId ) )
{
SGameObjectEvent event( eGFE_StoodOnChange, eGOEF_ToExtensions );
event.ptr = &player;
event.paramAsBool = false;
pGameObject->SendEvent(event);
}
}
if( newGroundColliderId )
{
if( IGameObject* pGameObject = gEnv->pGame->GetIGameFramework()->GetGameObject( newGroundColliderId ) )
{
SGameObjectEvent event( eGFE_StoodOnChange, eGOEF_ToExtensions );
event.ptr = &player;
event.paramAsBool = true;
pGameObject->SendEvent(event);
}
}
actorPhysics.groundColliderId = newGroundColliderId;
}
IPhysicalEntity *pPhysEnt = player.GetEntity()->GetPhysics();
if( pPhysEnt )
{
pe_status_dynamics dynStat;
pPhysEnt->GetStatus(&dynStat);
actorPhysics.angVelocity = dynStat.w;
actorPhysics.mass = dynStat.mass;
pe_player_dynamics simPar;
if (pPhysEnt->GetParams(&simPar) != 0)
{
actorPhysics.gravity = simPar.gravity;
}
}
actorPhysics.lastFrameUpdate = currentFrameID;
#ifdef PLAYER_MOVEMENT_DEBUG_ENABLED
player.GetMovementDebug().DebugGraph_AddValue("PhysVelo", livStat.vel.GetLength());
player.GetMovementDebug().DebugGraph_AddValue("PhysVeloX", livStat.vel.x);
player.GetMovementDebug().DebugGraph_AddValue("PhysVeloY", livStat.vel.y);
player.GetMovementDebug().DebugGraph_AddValue("PhysVeloZ", livStat.vel.z);
player.GetMovementDebug().DebugGraph_AddValue("PhysVeloUn", livStat.velUnconstrained.GetLength());
player.GetMovementDebug().DebugGraph_AddValue("PhysVeloUnX", livStat.velUnconstrained.x);
player.GetMovementDebug().DebugGraph_AddValue("PhysVeloUnY", livStat.velUnconstrained.y);
player.GetMovementDebug().DebugGraph_AddValue("PhysVeloUnZ", livStat.velUnconstrained.z);
player.GetMovementDebug().DebugGraph_AddValue("PhysVelReq", livStat.velRequested.GetLength());
player.GetMovementDebug().DebugGraph_AddValue("PhysVelReqX", livStat.velRequested.x);
player.GetMovementDebug().DebugGraph_AddValue("PhysVelReqY", livStat.velRequested.y);
player.GetMovementDebug().DebugGraph_AddValue("PhysVelReqZ", livStat.velRequested.z);
player.GetMovementDebug().LogVelocityStats(player.GetEntity(), livStat, stats.downwardsImpactVelocity, stats.fallSpeed);
#endif
}
}