//========= Copyright © 1996-2005, Valve Corporation, All rights reserved. ============// // // Purpose: // //=============================================================================// #include "cbase.h" #include "ai_link.h" #include "ai_navtype.h" #include "ai_waypoint.h" #include "ai_pathfinder.h" #include "ai_navgoaltype.h" #include "ai_routedist.h" #include "ai_route.h" // memdbgon must be the last include file in a .cpp file!!! #include "tier0/memdbgon.h" BEGIN_SIMPLE_DATADESC(CAI_Path) // m_Waypoints (reconsititute on load) DEFINE_FIELD( m_goalTolerance, FIELD_FLOAT ), DEFINE_CUSTOM_FIELD( m_activity, ActivityDataOps() ), DEFINE_FIELD( m_target, FIELD_EHANDLE ), DEFINE_FIELD( m_sequence, FIELD_INTEGER ), DEFINE_FIELD( m_vecTargetOffset, FIELD_VECTOR ), DEFINE_FIELD( m_waypointTolerance, FIELD_FLOAT ), DEFINE_CUSTOM_FIELD( m_arrivalActivity, ActivityDataOps() ), DEFINE_FIELD( m_arrivalSequence, FIELD_INTEGER ), // m_iLastNodeReached DEFINE_FIELD( m_bGoalPosSet, FIELD_BOOLEAN ), DEFINE_FIELD( m_goalPos, FIELD_POSITION_VECTOR), DEFINE_FIELD( m_bGoalTypeSet, FIELD_BOOLEAN ), DEFINE_FIELD( m_goalType, FIELD_INTEGER ), DEFINE_FIELD( m_goalFlags, FIELD_INTEGER ), DEFINE_FIELD( m_routeStartTime, FIELD_TIME ), DEFINE_FIELD( m_goalDirection, FIELD_VECTOR ), DEFINE_FIELD( m_goalDirectionTarget, FIELD_EHANDLE ), DEFINE_FIELD( m_goalSpeed, FIELD_FLOAT ), DEFINE_FIELD( m_goalSpeedTarget, FIELD_EHANDLE ), DEFINE_FIELD( m_goalStoppingDistance, FIELD_FLOAT ), END_DATADESC() //----------------------------------------------------------------------------- AI_Waypoint_t CAI_Path::gm_InvalidWaypoint( Vector(0,0,0), 0, NAV_NONE, 0, 0 ); //----------------------------------------------------------------------------- void CAI_Path::SetWaypoints(AI_Waypoint_t* route, bool fSetGoalFromLast) { m_Waypoints.Set(route); AI_Waypoint_t *pLast = m_Waypoints.GetLast(); if ( pLast ) { pLast->flPathDistGoal = -1; if ( fSetGoalFromLast ) { if ( pLast ) { m_bGoalPosSet = false; pLast->ModifyFlags( bits_WP_TO_GOAL, true ); SetGoalPosition(pLast->GetPos()); } } } AssertRouteValid( m_Waypoints.GetFirst() ); } //----------------------------------------------------------------------------- void CAI_Path::PrependWaypoints( AI_Waypoint_t *pWaypoints ) { m_Waypoints.PrependWaypoints( pWaypoints ); AI_Waypoint_t *pLast = m_Waypoints.GetLast(); pLast->flPathDistGoal = -1; AssertRouteValid( m_Waypoints.GetFirst() ); } //----------------------------------------------------------------------------- void CAI_Path::PrependWaypoint( const Vector &newPoint, Navigation_t navType, unsigned waypointFlags ) { m_Waypoints.PrependWaypoint( newPoint, navType, waypointFlags ); AI_Waypoint_t *pLast = m_Waypoints.GetLast(); pLast->flPathDistGoal = -1; AssertRouteValid( m_Waypoints.GetFirst() ); } //----------------------------------------------------------------------------- float CAI_Path::GetPathLength() { AI_Waypoint_t *pLast = m_Waypoints.GetLast(); if ( pLast && pLast->flPathDistGoal == -1 ) { ComputeRouteGoalDistances( pLast ); } AI_Waypoint_t *pCurrent = GetCurWaypoint(); return ( ( pCurrent ) ? pCurrent->flPathDistGoal : 0 ); } //----------------------------------------------------------------------------- float CAI_Path::GetPathDistanceToGoal( const Vector &startPos ) { AI_Waypoint_t *pCurrent = GetCurWaypoint(); if ( pCurrent ) { return ( GetPathLength() + ComputePathDistance(pCurrent->NavType(), startPos, pCurrent->GetPos()) ); } return 0; } //----------------------------------------------------------------------------- Activity CAI_Path::SetMovementActivity(Activity activity) { Assert( activity != ACT_RESET && activity != ACT_INVALID ); //Msg("Set movement to %s\n", ActivityList_NameForIndex(activity) ); m_sequence = ACT_INVALID; return (m_activity = activity); } //----------------------------------------------------------------------------- Activity CAI_Path::GetArrivalActivity( ) const { if ( !m_Waypoints.IsEmpty() ) { return m_arrivalActivity; } return ACT_INVALID; } //----------------------------------------------------------------------------- void CAI_Path::SetArrivalActivity(Activity activity) { m_arrivalActivity = activity; m_arrivalSequence = ACT_INVALID; } //----------------------------------------------------------------------------- int CAI_Path::GetArrivalSequence( ) const { if ( !m_Waypoints.IsEmpty() ) { return m_arrivalSequence; } return ACT_INVALID; } //----------------------------------------------------------------------------- void CAI_Path::SetArrivalSequence( int sequence ) { m_arrivalSequence = sequence; } //----------------------------------------------------------------------------- void CAI_Path::SetGoalDirection( const Vector &goalDirection ) { m_goalDirectionTarget = NULL; m_goalDirection = goalDirection; VectorNormalize( m_goalDirection ); /* AI_Waypoint_t *pLast = m_Waypoints.GetLast(); if ( pLast ) { NDebugOverlay::Box( pLast->vecLocation, Vector( -2, -2, -2 ), Vector( 2, 2, 2 ), 0,0,255, 0, 2.0 ); NDebugOverlay::Line( pLast->vecLocation, pLast->vecLocation + m_goalDirection * 32, 0,0,255, true, 2.0 ); } */ } //----------------------------------------------------------------------------- void CAI_Path::SetGoalDirection( CBaseEntity *pTarget ) { m_goalDirectionTarget = pTarget; if (pTarget) { AI_Waypoint_t *pLast = m_Waypoints.GetLast(); if ( pLast ) { m_goalDirection = pTarget->GetAbsOrigin() - pLast->vecLocation; VectorNormalize( m_goalDirection ); /* NDebugOverlay::Box( pLast->vecLocation, Vector( -2, -2, -2 ), Vector( 2, 2, 2 ), 0,0,255, 0, 2.0 ); NDebugOverlay::Line( pLast->vecLocation, pLast->vecLocation + m_goalDirection * 32, 0,0,255, true, 2.0 ); */ } } } //----------------------------------------------------------------------------- Vector CAI_Path::GetGoalDirection( const Vector &startPos ) { if (m_goalDirectionTarget) { AI_Waypoint_t *pLast = m_Waypoints.GetLast(); if ( pLast ) { AI_Waypoint_t *pPrev = pLast->GetPrev(); if (pPrev) { Vector goalDirection = m_goalDirectionTarget->GetAbsOrigin() - pPrev->vecLocation; VectorNormalize( goalDirection ); return goalDirection; } else { Vector goalDirection = m_goalDirectionTarget->GetAbsOrigin() - startPos; VectorNormalize( goalDirection ); return goalDirection; } } } else if (m_goalDirection == vec3_origin) { // Assert(0); // comment out the default directions in SetGoal() to find test cases for missing initialization AI_Waypoint_t *pLast = m_Waypoints.GetLast(); if ( pLast ) { AI_Waypoint_t *pPrev = pLast->GetPrev(); if (pPrev) { Vector goalDirection = pLast->vecLocation - pPrev->vecLocation; VectorNormalize( goalDirection ); return goalDirection; } else { Vector goalDirection =pLast->vecLocation - startPos; VectorNormalize( goalDirection ); return goalDirection; } } } return m_goalDirection; } //----------------------------------------------------------------------------- void CAI_Path::SetGoalSpeed( float flSpeed ) { m_goalSpeed = flSpeed; } //----------------------------------------------------------------------------- void CAI_Path::SetGoalSpeed( CBaseEntity *pTarget ) { m_goalSpeedTarget = pTarget; } //----------------------------------------------------------------------------- float CAI_Path::GetGoalSpeed( const Vector &startPos ) { if (m_goalSpeedTarget) { Vector goalDirection = GetGoalDirection( startPos ); Vector targetVelocity = m_goalSpeedTarget->GetSmoothedVelocity(); float dot = DotProduct( goalDirection, targetVelocity ); dot = MAX( 0.0f, dot ); // return a relative impact speed of m_goalSpeed if (m_goalSpeed > 0.0) { return dot + m_goalSpeed; } return dot; } return m_goalSpeed; } //----------------------------------------------------------------------------- void CAI_Path::SetGoalStoppingDistance( float flDistance ) { m_goalStoppingDistance = flDistance; } //----------------------------------------------------------------------------- float CAI_Path::GetGoalStoppingDistance( ) const { return m_goalStoppingDistance; } //----------------------------------------------------------------------------- const Vector &CAI_Path::CurWaypointPos() const { if ( GetCurWaypoint() ) return GetCurWaypoint()->GetPos(); AssertMsg(0, "Invalid call to CurWaypointPos()"); return gm_InvalidWaypoint.GetPos(); } //----------------------------------------------------------------------------- const Vector &CAI_Path::NextWaypointPos() const { if ( GetCurWaypoint() && GetCurWaypoint()->GetNext()) return GetCurWaypoint()->GetNext()->GetPos(); static Vector invalid( 0, 0, 0 ); AssertMsg(0, "Invalid call to NextWaypointPos()"); return gm_InvalidWaypoint.GetPos(); } //----------------------------------------------------------------------------- float CAI_Path::CurWaypointYaw() const { return GetCurWaypoint()->flYaw; } //----------------------------------------------------------------------------- // Purpose: // Input : // Output : //----------------------------------------------------------------------------- void CAI_Path::SetGoalPosition(const Vector &goalPos) { #ifdef _DEBUG // Make sure goal position isn't set more than once if (m_bGoalPosSet == true) { DevMsg( "GetCurWaypoint Goal Position Set Twice!\n"); } #endif m_bGoalPosSet = true; VectorAdd( goalPos, m_vecTargetOffset, m_goalPos ); } //----------------------------------------------------------------------------- // Purpose: Sets last node as goal and goal position // Input : // Output : //----------------------------------------------------------------------------- void CAI_Path::SetLastNodeAsGoal(bool bReset) { #ifdef _DEBUG // Make sure goal position isn't set more than once if (!bReset && m_bGoalPosSet == true) { DevMsg( "GetCurWaypoint Goal Position Set Twice!\n"); } #endif // Find the last node if (GetCurWaypoint()) { AI_Waypoint_t* waypoint = GetCurWaypoint(); while (waypoint) { if (!waypoint->GetNext()) { m_goalPos = waypoint->GetPos(); m_bGoalPosSet = true; waypoint->ModifyFlags( bits_WP_TO_GOAL, true ); return; } waypoint = waypoint->GetNext(); } } } //----------------------------------------------------------------------------- // Purpose: Explicitly change the goal position w/o check // Input : // Output : //----------------------------------------------------------------------------- void CAI_Path::ResetGoalPosition(const Vector &goalPos) { m_bGoalPosSet = true; VectorAdd( goalPos, m_vecTargetOffset, m_goalPos ); } //----------------------------------------------------------------------------- // Returns the *base* goal position (without the offset applied) //----------------------------------------------------------------------------- const Vector& CAI_Path::BaseGoalPosition() const { #ifdef _DEBUG // Make sure goal position was set if (m_bGoalPosSet == false) { DevMsg( "GetCurWaypoint Goal Position Never Set!\n"); } #endif // FIXME: A little risky; store the base if this becomes a problem static Vector vecResult; VectorSubtract( m_goalPos, m_vecTargetOffset, vecResult ); return vecResult; } //----------------------------------------------------------------------------- // Returns the *actual* goal position (with the offset applied) //----------------------------------------------------------------------------- const Vector & CAI_Path::ActualGoalPosition(void) const { #ifdef _DEBUG // Make sure goal position was set if (m_bGoalPosSet == false) { DevMsg( "GetCurWaypoint Goal Position Never Set!\n"); } #endif return m_goalPos; } //----------------------------------------------------------------------------- // Purpose: // Input : // Output : //----------------------------------------------------------------------------- void CAI_Path::SetGoalType(GoalType_t goalType) { #ifdef _DEBUG // Make sure goal position isn't set more than once if (m_goalType != GOALTYPE_NONE && goalType != GOALTYPE_NONE ) { DevMsg( "GetCurWaypoint Goal Type Set Twice!\n"); } #endif if (m_goalType != GOALTYPE_NONE) { m_routeStartTime = gpGlobals->curtime; m_bGoalTypeSet = true; } else m_bGoalTypeSet = false; m_goalType = goalType; } //----------------------------------------------------------------------------- // Purpose: // Input : // Output : //----------------------------------------------------------------------------- GoalType_t CAI_Path::GoalType(void) const { return m_goalType; } //----------------------------------------------------------------------------- void CAI_Path::Advance( void ) { if ( CurWaypointIsGoal() ) return; // ------------------------------------------------------- // If I have another waypoint advance my path // ------------------------------------------------------- if (GetCurWaypoint()->GetNext()) { AI_Waypoint_t *pNext = GetCurWaypoint()->GetNext(); // If waypoint was a node take note of it if (GetCurWaypoint()->Flags() & bits_WP_TO_NODE) { m_iLastNodeReached = GetCurWaypoint()->iNodeID; } delete GetCurWaypoint(); SetWaypoints(pNext); return; } // ------------------------------------------------- // This is an error catch that should *not* happen // It means a route was created with no goal // ------------------------------------------------- else { DevMsg( "!!ERROR!! Force end of route with no goal!\n"); GetCurWaypoint()->ModifyFlags( bits_WP_TO_GOAL, true ); } AssertRouteValid( m_Waypoints.GetFirst() ); } //----------------------------------------------------------------------------- // Purpose: Clears the route and resets all its fields to default values // Input : // Output : //----------------------------------------------------------------------------- void CAI_Path::Clear( void ) { m_Waypoints.RemoveAll(); m_goalType = GOALTYPE_NONE; // Type of goal m_goalPos = vec3_origin; // Our ultimate goal position m_bGoalPosSet = false; // Was goal position set m_bGoalTypeSet = false; // Was goal position set m_goalFlags = false; m_vecTargetOffset = vec3_origin; m_routeStartTime = FLT_MAX; m_goalTolerance = 0.0; // How close do we need to get to the goal // FIXME: split m_goalTolerance into m_buildTolerance and m_moveTolerance, let them be seperatly controllable. m_activity = ACT_INVALID; m_sequence = ACT_INVALID; m_target = NULL; m_arrivalActivity = ACT_INVALID; m_arrivalSequence = ACT_INVALID; m_goalDirectionTarget = NULL; m_goalDirection = vec3_origin; m_goalSpeedTarget = NULL; m_goalSpeed = -1.0f; // init to an invalid speed m_goalStoppingDistance = 0.0; // How close to we want to get to the goal } //----------------------------------------------------------------------------- // Purpose: // Input : // Output : //----------------------------------------------------------------------------- Navigation_t CAI_Path::CurWaypointNavType() const { if (!GetCurWaypoint()) { return NAV_NONE; } else { return GetCurWaypoint()->NavType(); } } int CAI_Path::CurWaypointFlags() const { if (!GetCurWaypoint()) { return 0; } else { return GetCurWaypoint()->Flags(); } } //----------------------------------------------------------------------------- // Purpose: Get the goal's flags // Output : unsigned //----------------------------------------------------------------------------- unsigned CAI_Path::GoalFlags( void ) const { return m_goalFlags; } //----------------------------------------------------------------------------- // Purpose: Returns true if current waypoint is my goal // Input : // Output : //----------------------------------------------------------------------------- bool CAI_Path::CurWaypointIsGoal( void ) const { // Assert( GetCurWaypoint() ); if( !GetCurWaypoint() ) return false; if ( GetCurWaypoint()->Flags() & bits_WP_TO_GOAL ) { #ifdef _DEBUG if (GetCurWaypoint()->GetNext()) { DevMsg( "!!ERROR!! Goal is not last waypoint!\n"); } if ((GetCurWaypoint()->GetPos() - m_goalPos).Length() > 0.1) { DevMsg( "!!ERROR!! Last waypoint isn't in goal position!\n"); } #endif return true; } if ( GetCurWaypoint()->Flags() & bits_WP_TO_PATHCORNER ) { // UNDONE: Refresh here or somewhere else? } #ifdef _DEBUG if (!GetCurWaypoint()->GetNext()) { DevMsg( "!!ERROR!! GetCurWaypoint has no goal!\n"); } #endif return false; } //----------------------------------------------------------------------------- // Computes the goal distance for each waypoint along the route //----------------------------------------------------------------------------- void CAI_Path::ComputeRouteGoalDistances(AI_Waypoint_t *pGoalWaypoint) { // The goal distance is the distance from any waypoint to the goal waypoint // Backup through the list and calculate distance to goal AI_Waypoint_t *pPrev; AI_Waypoint_t *pCurWaypoint = pGoalWaypoint; pCurWaypoint->flPathDistGoal = 0; while (pCurWaypoint->GetPrev()) { pPrev = pCurWaypoint->GetPrev(); float flWaypointDist = ComputePathDistance(pCurWaypoint->NavType(), pPrev->GetPos(), pCurWaypoint->GetPos()); pPrev->flPathDistGoal = pCurWaypoint->flPathDistGoal + flWaypointDist; pCurWaypoint = pPrev; } } //----------------------------------------------------------------------------- // Purpose: Constructor // Input : // Output : //----------------------------------------------------------------------------- CAI_Path::CAI_Path() { m_goalType = GOALTYPE_NONE; // Type of goal m_goalPos = vec3_origin; // Our ultimate goal position m_goalTolerance = 0.0; // How close do we need to get to the goal m_activity = ACT_INVALID; // The activity to use during motion m_sequence = ACT_INVALID; m_target = NULL; m_goalFlags = 0; m_routeStartTime = FLT_MAX; m_arrivalActivity = ACT_INVALID; m_arrivalSequence = ACT_INVALID; m_iLastNodeReached = NO_NODE; m_waypointTolerance = DEF_WAYPOINT_TOLERANCE; } CAI_Path::~CAI_Path() { DeleteAll( GetCurWaypoint() ); }