void FunnelPlanner::computeCrossing( float radius, const Vector2 & startPos, PortalPath * path, size_t startPortal ) {
assert( path->getPortalCount() > 0 && "Funnel planner should only be applied to PortalPaths with at least one portal" );
FunnelApex apex( startPortal - 1, startPos ); // if startPortal is zero, this should go to all 1s...i.e. -1 > all other size_t values
const WayPortal * portal = path->getPortal( startPortal );
Vector2 pLeft( portal->getLeft( radius ) );
Vector2 pRight( portal->getRight( radius ) );
Vector2 dirLeft( pLeft - apex._pos );
Vector2 dirRight( pRight - apex._pos );
#ifdef SIMPLE_FUNNEL
FunnelEdge funnelLeft( startPortal, dirLeft );
FunnelEdge funnelRight( startPortal, dirRight );
size_t currPortal = startPortal + 1;
const size_t PORTAL_COUNT = path->getPortalCount();
while ( currPortal < PORTAL_COUNT ) {
portal = path->getPortal( currPortal );
pLeft.set( portal->getLeft( radius ) );
pRight.set( portal->getRight( radius ) );
dirLeft.set( pLeft - apex._pos );
dirRight.set( pRight - apex._pos );
// test left side of the funnel
if ( funnelRight.isOnRight( dirLeft ) ) {
// the portal's funnel is on the right of the current funnel
Vector2 oldApex = apex._pos;
Vector2 newApex = funnelRight._dir + apex._pos;
//if ( apex._id != -1 && apex._id < currPortal ) {
path->setWaypoints( apex._id + 1, funnelRight._id + 1, newApex, norm( funnelRight._dir ) );
//}
apex.set( funnelRight._id, newApex );
currPortal = funnelRight._id + 1;
portal = path->getPortal( currPortal );
pLeft.set( portal->getLeft( radius ) );
pRight.set( portal->getRight( radius ) );
dirLeft.set( pLeft - apex._pos );
dirRight.set( pRight - apex._pos );
funnelLeft.set( currPortal, dirLeft );
funnelRight.set( currPortal, dirRight );
++currPortal;
continue;
} else if ( funnelLeft.isOnRight( dirLeft ) ) {
funnelLeft.set( currPortal, dirLeft );
}
// test right side of the funnel
if ( funnelLeft.isOnLeft( dirRight ) ) {
// the portal's funnel is on the left of the current funnel
Vector2 oldApex = apex._pos;
Vector2 newApex = funnelLeft._dir + apex._pos;
//if ( apex._id != -1 && apex._id < currPortal ) {
path->setWaypoints( apex._id + 1, funnelLeft._id + 1, newApex, norm( funnelLeft._dir ) );
//}
apex.set( funnelLeft._id, newApex );
currPortal = funnelLeft._id + 1;
portal = path->getPortal( currPortal );
pLeft.set( portal->getLeft( radius ) );
pRight.set( portal->getRight( radius ) );
dirLeft.set( pLeft - apex._pos );
dirRight.set( pRight - apex._pos );
funnelLeft.set( currPortal, dirLeft );
funnelRight.set( currPortal, dirRight );
++currPortal;
continue;
} else if ( funnelRight.isOnLeft( dirRight ) ) {
funnelRight.set( currPortal, dirRight );
}
++currPortal;
}
// Now handle goal
const Vector2 goalPt = path->getGoalCentroid();
Vector2 goalDir( goalPt - apex._pos );
if ( funnelLeft.isOnLeft( goalDir ) ) {
// The goal point is on the left side of the funnel
if ( apex._id != -1 && apex._id < PORTAL_COUNT ) {
path->setWaypoints( apex._id + 1, PORTAL_COUNT, apex._pos + funnelLeft._dir, norm( funnelLeft._dir ) );
}
} else if ( funnelRight.isOnRight( goalDir ) ) {
// The goal point is on the right side of the funnel
if ( apex._id != -1 && apex._id < PORTAL_COUNT ) {
path->setWaypoints( apex._id + 1, PORTAL_COUNT, apex._pos + funnelRight._dir, norm( funnelRight._dir ) );
}
}
if ( apex._id + 1 < PORTAL_COUNT ) {
path->setWaypoints( (size_t)apex._id + 1, (size_t)PORTAL_COUNT, goalPt, norm( goalPt - apex._pos ) );
}
#else
_left.push_back( FunnelEdge( startPortal - 1, startPortal, dirLeft, startPos ) );
_right.push_back( FunnelEdge( startPortal - 1, startPortal, dirRight, startPos ) );
const size_t PORTAL_COUNT = path->getPortalCount();
for ( size_t i = startPortal + 1; i < PORTAL_COUNT; ++i ) {
portal = path->getPortal( i );
// investigate the left point
pLeft.set( portal->getLeft( radius ) );
bool apexMoved = false;
while ( !_right.empty() ) {
std::list< FunnelEdge >::iterator itr = _right.begin();
Vector2 dir = pLeft - itr->_origin;
if ( itr->isOnRight( dir ) ) {
apexMoved = true;
Vector2 newApex = itr->_origin + itr->_dir;
path->setWaypoints( itr->_id + 1, itr->_endID + 1, newApex, norm( itr->_dir ) );
apex.set( itr->_endID, newApex );
_right.pop_front();
} else {
break;
}
}
if ( apexMoved ) {
_left.clear();
_left.push_back( FunnelEdge( apex._id, i, pLeft - apex._pos, apex._pos ) );
} else {
std::list< FunnelEdge >::reverse_iterator itr = _left.rbegin();
while ( ! _left.empty() ) {
Vector2 dir = pLeft - itr->_origin;
if ( itr->isOnRight( dir ) ) {
_left.pop_back();
itr = _left.rbegin();
} else {
break;
}
}
if ( _left.empty() ) {
_left.push_back( FunnelEdge( apex._id, i, pLeft - apex._pos, apex._pos ) );
} else {
Vector2 origin( itr->_origin + itr->_dir );
_left.push_back( FunnelEdge( itr->_endID, i, pLeft - origin, origin ) );
}
}
// investigate the right point
pRight.set( portal->getRight( radius ) );
apexMoved = false;
while ( !_left.empty() ) {
std::list< FunnelEdge >::iterator itr = _left.begin();
Vector2 dir = pRight - itr->_origin;
if ( itr->isOnLeft( dir ) ) {
apexMoved = true;
Vector2 newApex = itr->_origin + itr->_dir;
path->setWaypoints( itr->_id + 1, itr->_endID + 1, newApex, norm( itr->_dir ) );
apex.set( itr->_endID, newApex );
_left.pop_front();
} else {
break;
}
}
if ( apexMoved ) {
_right.clear();
_right.push_back( FunnelEdge( apex._id, i, pRight - apex._pos, apex._pos ) );
} else {
std::list< FunnelEdge >::reverse_iterator itr = _right.rbegin();
while ( ! _right.empty() ) {
Vector2 dir = pRight - itr->_origin;
if ( itr->isOnLeft( dir ) ) {
_right.pop_back();
itr = _right.rbegin();
} else {
break;
}
}
if ( _right.empty() ) {
_right.push_back( FunnelEdge( apex._id, i, pRight - apex._pos, apex._pos ) );
} else {
Vector2 origin( itr->_origin + itr->_dir );
_right.push_back( FunnelEdge( itr->_endID, i, pRight - origin, origin ) );
}
}
}
// handle the goal
const Vector2 goalPt = path->getGoalCentroid();
Vector2 goalDir;
bool apexMoved = false;
while ( !_left.empty() ) {
std::list< FunnelEdge >::iterator itr = _left.begin();
goalDir.set( goalPt - itr->_origin );
if ( itr->isOnLeft( goalDir ) ) {
apexMoved = true;
Vector2 newApex = itr->_origin + itr->_dir;
apex.set( itr->_endID, newApex );
path->setWaypoints( itr->_id + 1, itr->_endID + 1, newApex, norm( itr->_dir ) );
_left.pop_front();
} else {
break;
}
}
if ( apexMoved ) {
goalDir.set( goalPt - apex._pos );
path->setWaypoints( apex._id + 1, PORTAL_COUNT, goalPt, norm( goalDir ) );
} else {
// apexMoved is already false -- it is the only way to reach this branch
while ( !_right.empty() ) {
std::list< FunnelEdge >::iterator itr = _right.begin();
goalDir.set( goalPt - itr->_origin );
if ( itr->isOnRight( goalDir ) ) {
apexMoved = true;
Vector2 newApex = itr->_origin + itr->_dir;
apex.set( itr->_endID, newApex );
path->setWaypoints( itr->_id + 1, itr->_endID + 1, newApex, norm( itr->_dir ) );
_right.pop_front();
} else {
break;
}
}
goalDir.set( goalPt - apex._pos );
path->setWaypoints( apex._id + 1, PORTAL_COUNT, goalPt, norm( goalDir ) );
}
#endif // SIMPLE_FUNNEL
}
//----------------------------------------------------------------------------
void CHomingMissile::UpdateCruiseMissile(float frameTime)
{
IRenderer* pRenderer = gEnv->pRenderer;
IRenderAuxGeom* pGeom = pRenderer->GetIRenderAuxGeom();
float color[4] = {1,1,1,1};
const static float step = 15.f;
float y = 20.f;
bool bDebug = g_pGameCVars->i_debug_projectiles > 0;
if (m_targetId)
{
IEntity* pTarget = gEnv->pEntitySystem->GetEntity(m_targetId);
if (pTarget)
{
AABB box;
pTarget->GetWorldBounds(box);
SetDestination( box.GetCenter() );
//if (bDebug)
//pRenderer->Draw2dLabel(5.0f, y+=step, 1.5f, color, false, "Target Entity: %s", pTarget->GetName());
}
}
else
{
// update destination pos from weapon
static IItemSystem* pItemSystem = g_pGame->GetIGameFramework()->GetIItemSystem();
IItem* pItem = pItemSystem->GetItem(m_weaponId);
if (pItem && pItem->GetIWeapon())
{
const Vec3& dest = pItem->GetIWeapon()->GetDestination();
SetDestination( dest );
//if (bDebug)
//pRenderer->Draw2dLabel(5.0f, y+=step, 1.5f, color, false, "Weapon Destination: (%.1f %.1f %.1f)", dest.x, dest.y, dest.z);
}
}
pe_status_dynamics status;
if (!GetEntity()->GetPhysics()->GetStatus(&status))
return;
float currentSpeed = status.v.len();
Vec3 currentPos = GetEntity()->GetWorldPos();
Vec3 goalDir(ZERO);
if (!m_destination.IsZero())
{
if((currentPos-m_destination).len2()<(m_detonationRadius*m_detonationRadius))
{
Explode(true, true, m_destination, -status.v.normalized(), status.v, m_targetId);
return;
}
if (bDebug)
pGeom->DrawCone(m_destination, Vec3(0,0,-1), 2.5f, 7.f, ColorB(255,0,0,255));
float heightDiff = (m_cruiseAltitude-m_alignAltitude) - currentPos.z;
if (!m_isCruising && heightDiff * sgn(status.v.z) > 0.f)
{
// if heading towards align altitude (but not yet reached) accelerate to max speed
if (bDebug)
pRenderer->Draw2dLabel(5.0f, y+=step, 1.5f, color, false, "[HomingMissile] accelerating (%.1f / %.1f)", currentSpeed, m_maxSpeed);
}
else if (!m_isCruising && heightDiff * sgnnz(status.v.z) < 0.f && (status.v.z<0 || status.v.z>0.25f))
{
// align to cruise
if (currentSpeed != 0)
{
goalDir = status.v;
goalDir.z = 0;
goalDir.normalize();
}
if (bDebug)
pRenderer->Draw2dLabel(5.0f, y+=step, 1.5f, color, false, "[HomingMissile] aligning");
}
else
{
if (bDebug)
pRenderer->Draw2dLabel(5.0f, y+=step, 1.5f, color, false, "[HomingMissile] cruising...");
// cruise
m_isCruising = true;
if (!m_destination.IsZero())
{
float groundDistSq = m_destination.GetSquaredDistance2D(currentPos);
float distSq = m_destination.GetSquaredDistance(currentPos);
float descendDistSq = sqr(m_descendDistance);
if (m_isDescending || groundDistSq <= descendDistSq)
{
if (bDebug)
pRenderer->Draw2dLabel(5.0f, y+=step, 1.5f, color, false, "[HomingMissile] descending!");
if (distSq != 0)
goalDir = (m_destination - currentPos).normalized();
else
goalDir.zero();
m_isDescending = true;
}
else
{
Vec3 airPos = m_destination;
airPos.z = currentPos.z;
goalDir = airPos - currentPos;
if (goalDir.len2() != 0)
goalDir.Normalize();
}
}
}
}
float desiredSpeed = currentSpeed;
if (currentSpeed < m_maxSpeed-0.1f)
{
desiredSpeed = min(m_maxSpeed, desiredSpeed + m_accel*frameTime);
}
Vec3 currentDir = status.v.GetNormalizedSafe(FORWARD_DIRECTION);
Vec3 dir = currentDir;
if (!goalDir.IsZero())
{
float cosine = max(min(currentDir.Dot(goalDir), 0.999f), -0.999f);
float goalAngle = RAD2DEG(acos_tpl(cosine));
float maxAngle = m_turnSpeed * frameTime;
if (bDebug)
{
pGeom->DrawCone( currentPos, goalDir, 0.4f, 12.f, ColorB(255,0,0,255) );
pRenderer->Draw2dLabel(5.0f, y+=step, 1.5f, color, false, "[HomingMissile] goalAngle: %.2f", goalAngle);
}
if (goalAngle > maxAngle+0.05f)
dir = (Vec3::CreateSlerp(currentDir, goalDir, maxAngle/goalAngle)).normalize();
else //if (goalAngle < 0.005f)
dir = goalDir;
}
pe_action_set_velocity action;
action.v = dir * desiredSpeed;
GetEntity()->GetPhysics()->Action(&action);
if (bDebug)
{
pGeom->DrawCone( currentPos, dir, 0.4f, 12.f, ColorB(128,128,0,255) );
pRenderer->Draw2dLabel(5.0f, y+=step, 1.5f, color, false, "[HomingMissile] currentSpeed: %.1f (max: %.1f)", currentSpeed, m_maxSpeed);
}
}
void PortalPath::setPreferredDirection( const Agents::BaseAgent * agent, float headingCos, Agents::PrefVelocity & pVel ) {
const size_t PORTAL_COUNT = _route->getPortalCount();
Vector2 dir;
if ( _currPortal >= PORTAL_COUNT ) {
// assume that the path is clear
// TODO: See GoalVC
_goal->setDirections( agent->_pos, agent->_radius, pVel );
// speed
Vector2 goalPoint = pVel.getTarget();
Vector2 disp = goalPoint - agent->_pos;
const float distSq = absSq( disp );
float speed = agent->_prefSpeed;
if ( distSq <= 0.0001f ) {
// I've basically arrived -- speed should be zero.
speed = 0.f;
} else {
const float speedSq = speed * speed;
const float TS_SQD = SIM_TIME_STEP * SIM_TIME_STEP;
if ( distSq / speedSq < TS_SQD ) {
// The distance is less than I would travel in a single time step.
speed = sqrtf( distSq ) / SIM_TIME_STEP;
}
}
pVel.setSpeed( speed );
} else {
const WayPortal * portal = _route->getPortal( _currPortal );
Vector2 goalDir( _waypoints[ _currPortal ] - agent->_pos );
float dist = abs( goalDir );
// If the displacement to the next way point is large enough
// (i.e., not essentially zero), use it, otherwise, peek
// into the next waypoint.
//
// The goal is to always provide a goalDir to the portal
// that is well-defined and unit-length.
bool bigEnough = dist >= EPS;
if ( bigEnough ) {
goalDir /= dist;
if ( goalDir * _headings[ _currPortal ] < headingCos ) {
// Heading has deviated too far recompute crossing
FunnelPlanner planner;
planner.computeCrossing( agent->_radius, agent->_pos, this, _currPortal );
goalDir = _waypoints[ _currPortal ] - agent->_pos;
dist = abs( goalDir );
if ( bigEnough = ( dist >= EPS ) ) {
goalDir /= dist;
}
}
}
if ( ! bigEnough ) {
// simply cross the wayportal perpendicularly
//goalDir.set( portal->getCrossingDir( agent->_pos ) );
if ( _currPortal + 1 < getPortalCount() ) {
// calculate w.r.t. next waypoint
goalDir = norm( _waypoints[ _currPortal + 1 ] - agent->_pos );
} else {
// calculate w.r.t. goal
Vector2 gp;
_goal->getTargetPoint( gp, agent->_radius );
goalDir = norm( gp - agent->_pos );
}
}
assert( abs( goalDir ) > EPS && "Providing a goal direction that is too small" );
pVel.setTarget( _waypoints[ _currPortal ] );
portal->setPreferredDirection( agent->_pos, agent->_radius, goalDir, pVel );
}
}
//-----------------------------------------------------------------------------
void CHomingMissile::UpdateControlledMissile(float frameTime)
{
bool isServer = gEnv->bServer;
bool isClient = gEnv->bClient;
CActor *pClientActor=0;
if (gEnv->bClient)
pClientActor=static_cast<CActor *>(g_pGame->GetIGameFramework()->GetClientActor());
bool isOwner = ((!m_ownerId && isServer) || (isClient && pClientActor && (pClientActor->GetEntityId() == m_ownerId) && pClientActor->IsPlayer()));
IRenderer* pRenderer = gEnv->pRenderer;
IRenderAuxGeom* pGeom = pRenderer->GetIRenderAuxGeom();
float color[4] = {1,1,1,1};
const static float step = 15.f;
float y = 20.f;
bool bDebug = g_pGameCVars->i_debug_projectiles > 0;
if (isOwner || isServer)
{
//If there's a target, follow the target
if(isServer)
{
if (m_targetId)
{
if (m_lockedTimer>0.0f)
m_lockedTimer=m_lockedTimer-frameTime;
else
{
// If we are here, there's a target
IEntity* pTarget = gEnv->pEntitySystem->GetEntity(m_targetId);
if (pTarget)
{
AABB box;
pTarget->GetWorldBounds(box);
Vec3 finalDes = box.GetCenter();
SetDestination(finalDes);
//SetDestination( box.GetCenter() );
if (bDebug)
pRenderer->Draw2dLabel(5.0f, y+=step, 1.5f, color, false, "Target Entity: %s", pTarget->GetName());
}
m_lockedTimer+=0.05f;
}
}
else if(m_autoControlled)
return;
}
if (m_controlled && !m_autoControlled && isOwner && !m_targetId)
{
//Check if the weapon is still selected
CWeapon *pWeapon = GetWeapon();
if(!pWeapon || !pWeapon->IsSelected())
return;
if (m_controlledTimer>0.0f)
m_controlledTimer=m_controlledTimer-frameTime;
else if (pClientActor && pClientActor->IsPlayer()) //Follow the crosshair
{
if (IMovementController *pMC=pClientActor->GetMovementController())
{
Vec3 eyePos(ZERO);
Vec3 eyeDir(ZERO);
IVehicle* pVehicle = pClientActor->GetLinkedVehicle();
if(!pVehicle)
{
SMovementState state;
pMC->GetMovementState(state);
eyePos = state.eyePosition;
eyeDir = state.eyeDirection;
}
else
{
SViewParams viewParams;
pVehicle->UpdateView(viewParams, pClientActor->GetEntityId());
eyePos = viewParams.position;
eyeDir = viewParams.rotation * Vec3(0,1,0);
//eyeDir = (viewParams.targetPos - viewParams.position).GetNormalizedSafe();
}
int pierceability=7;
if (IPhysicalEntity *pPE=GetEntity()->GetPhysics())
{
if (pPE->GetType()==PE_PARTICLE)
{
pe_params_particle pp;
if (pPE->GetParams(&pp))
pierceability=pp.iPierceability;
}
}
static const int objTypes = ent_all;
static const int flags = (geom_colltype_ray << rwi_colltype_bit) | rwi_colltype_any | (pierceability & rwi_pierceability_mask) | (geom_colltype14 << rwi_colltype_bit);
IPhysicalWorld* pWorld = gEnv->pPhysicalWorld;
static IPhysicalEntity* pSkipEnts[10];
int numSkip = CSingle::GetSkipEntities(pWeapon, pSkipEnts, 10);
ray_hit hit;
int hits = 0;
float range=m_maxTargetDistance;
hits = pWorld->RayWorldIntersection(eyePos + 1.5f*eyeDir, eyeDir*range, objTypes, flags, &hit, 1, pSkipEnts, numSkip);
while (hits)
{
if (gEnv->p3DEngine->RefineRayHit(&hit, eyeDir*range))
break;
eyePos = hit.pt+eyeDir*0.003f;
range -= hit.dist+0.003f;
hits = pWorld->RayWorldIntersection(eyePos, eyeDir*range, objTypes, flags, &hit, 1, pSkipEnts, numSkip);
}
DestinationParams params;
if(hits)
params.pt=hit.pt;
else
params.pt=(eyePos+m_maxTargetDistance*eyeDir); //Some point in the sky...
GetGameObject()->InvokeRMI(SvRequestDestination(), params, eRMI_ToServer);
if (bDebug)
{
pRenderer->Draw2dLabel(5.0f, y+=step, 1.5f, color, false, "PlayerView eye direction: %.3f %.3f %.3f", eyeDir.x, eyeDir.y, eyeDir.z);
pRenderer->Draw2dLabel(5.0f, y+=step, 1.5f, color, false, "PlayerView Target: %.3f %.3f %.3f", hit.pt.x, hit.pt.y, hit.pt.z);
pRenderer->GetIRenderAuxGeom()->DrawCone(m_destination, Vec3(0,0,-1), 2.5f, 7.f, ColorB(255,0,0,255));
}
}
m_controlledTimer+=0.0f;
}
}
}
//This code is shared by both modes above (auto and controlled)
if(!m_destination.IsZero())
{
pe_status_dynamics status;
if (!GetEntity()->GetPhysics()->GetStatus(&status))
{
CryLogAlways("couldn't get physics status!");
return;
}
pe_status_pos pos;
if (!GetEntity()->GetPhysics()->GetStatus(&pos))
{
CryLogAlways("couldn't get physics pos!");
return;
}
float currentSpeed = status.v.len();
if (currentSpeed>0.001f)
{
Vec3 currentVel = status.v;
Vec3 currentPos = pos.pos;
Vec3 goalDir(ZERO);
assert(!_isnan(currentSpeed));
assert(!_isnan(currentVel.x) && !_isnan(currentVel.y) && !_isnan(currentVel.z));
//Just a security check
if((currentPos-m_destination).len2()<(m_detonationRadius*m_detonationRadius))
{
Explode(true, true, m_destination, -currentVel.normalized(), currentVel, m_targetId);
return;
}
goalDir = m_destination - currentPos;
goalDir.Normalize();
//Turn more slowly...
currentVel.Normalize();
if(bDebug)
{
pRenderer->Draw2dLabel(50,55,2.0f,color,false, " Destination: %.3f, %.3f, %.3f",m_destination.x,m_destination.y,m_destination.z);
pRenderer->Draw2dLabel(50,80,2.0f,color,false, " Current Dir: %.3f, %.3f, %.3f",currentVel.x,currentVel.y,currentVel.z);
pRenderer->Draw2dLabel(50,105,2.0f,color,false," Goal Dir: %.3f, %.3f, %.3f",goalDir.x,goalDir.y,goalDir.z);
}
float cosine = currentVel.Dot(goalDir);
cosine = CLAMP(cosine,-1.0f,1.0f);
float totalAngle = RAD2DEG(cry_acosf(cosine));
assert(totalAngle>=0);
if (cosine<0.99)
{
float maxAngle = m_turnSpeed*frameTime;
if (maxAngle>totalAngle)
maxAngle=totalAngle;
float t=(maxAngle/totalAngle)*m_lazyness;
assert(t>=0.0 && t<=1.0);
goalDir = Vec3::CreateSlerp(currentVel, goalDir, t);
goalDir.Normalize();
}
if(bDebug)
pRenderer->Draw2dLabel(50,180,2.0f,color,false,"Corrected Dir: %.3f, %.3f, %.3f",goalDir.x,goalDir.y,goalDir.z);
pe_action_set_velocity action;
action.v = goalDir * currentSpeed;
GetEntity()->GetPhysics()->Action(&action);
}
}
}
