codemp/game/g_nav.c

Go to the documentation of this file.

#include "b_local.h"
#include "g_nav.h"

extern qboolean G_EntIsUnlockedDoor( int entityNum );
extern qboolean G_EntIsDoor( int entityNum );
extern qboolean G_EntIsBreakable( int entityNum );
extern qboolean G_EntIsRemovableUsable( int entNum );
extern qboolean G_FindClosestPointOnLineSegment( const vec3_t start, const vec3_t end, const vec3_t from, vec3_t result );
//For debug graphics
//rwwFIXMEFIXME: Write these at some point for the sake of being able to debug visually
void G_Line( vec3_t start, vec3_t end, vec3_t color, float alpha )
{

}

void G_Cube( vec3_t mins, vec3_t maxs, vec3_t color, float alpha )
{

}

void G_CubeOutline( vec3_t mins, vec3_t maxs, int time, unsigned int color, float alpha )
{

}

void G_DrawEdge( vec3_t start, vec3_t end, int type )
{

}

void G_DrawNode( vec3_t origin, int type )
{

}

void G_DrawCombatPoint( vec3_t origin, int type )
{

}

void TAG_ShowTags( int flags )
{

}

qboolean FlyingCreature( gentity_t *ent )
{
        if (ent->client && ent->client->ps.gravity <= 0)
        {
                return qtrue;
        }
        return qfalse;
}

qboolean NAV_CheckAhead( gentity_t *self, vec3_t end, trace_t *trace, int clipmask );
void NAV_StoreWaypoint( gentity_t *ent );

extern vec3_t NPCDEBUG_RED;


/*
-------------------------
NPC_Blocked
-------------------------
*/


void NPC_Blocked( gentity_t *self, gentity_t *blocker )
{
        if ( self->NPC == NULL )
                return;

        //Don't do this too often
        if ( self->NPC->blockedSpeechDebounceTime > level.time )
                return;

        //Attempt to run any blocked scripts
        if ( G_ActivateBehavior( self, BSET_BLOCKED ) )
        {
                return;
        }

        //If this is one of our enemies, then just attack him
        if ( blocker->client && ( blocker->client->playerTeam == self->client->enemyTeam ) )
        {
                G_SetEnemy( self, blocker );
                return;
        }

        //Debug_Printf( debugNPCAI, DEBUG_LEVEL_WARNING, "%s: Excuse me, %s %s!\n", self->targetname, blocker->classname, blocker->targetname );
        
        //If we're being blocked by the player, say something to them
        if ( ( blocker->s.number == 0 ) && ( ( blocker->client->playerTeam == self->client->playerTeam ) ) )
        {
                //guys in formation are not trying to get to a critical point, 
                //don't make them yell at the player (unless they have an enemy and
                //are in combat because BP thinks it sounds cool during battle)
                //NOTE: only imperials, misc crewmen and hazard team have these wav files now
                //G_AddVoiceEvent( self, Q_irand(EV_BLOCKED1, EV_BLOCKED3), 0 );
        }

        self->NPC->blockedSpeechDebounceTime = level.time + MIN_BLOCKED_SPEECH_TIME + ( random() * 4000 );
        self->NPC->blockingEntNum = blocker->s.number;
}

/*
-------------------------
NPC_SetMoveGoal
-------------------------
*/

void NPC_SetMoveGoal( gentity_t *ent, vec3_t point, int radius, qboolean isNavGoal, int combatPoint, gentity_t *targetEnt )
{
        //Must be an NPC
        if ( ent->NPC == NULL )
        {
                return;
        }

        if ( ent->NPC->tempGoal == NULL )
        {//must still have a goal
                return;
        }

        //Copy the origin
        //VectorCopy( point, ent->NPC->goalPoint );     //FIXME: Make it use this, and this alone!
        VectorCopy( point, ent->NPC->tempGoal->r.currentOrigin );
        
        //Copy the mins and maxs to the tempGoal
        VectorCopy( ent->r.mins, ent->NPC->tempGoal->r.mins );
        VectorCopy( ent->r.mins, ent->NPC->tempGoal->r.maxs );

        ent->NPC->tempGoal->target = NULL;
        ent->NPC->tempGoal->clipmask = ent->clipmask;
        ent->NPC->tempGoal->flags &= ~FL_NAVGOAL;
        if ( targetEnt && targetEnt->waypoint >= 0 )
        {
                ent->NPC->tempGoal->waypoint = targetEnt->waypoint;
        }
        else
        {
                ent->NPC->tempGoal->waypoint = WAYPOINT_NONE;
        }
        ent->NPC->tempGoal->noWaypointTime = 0;

        if ( isNavGoal )
        {
                assert(ent->NPC->tempGoal->parent);
                ent->NPC->tempGoal->flags |= FL_NAVGOAL;
        }

        ent->NPC->tempGoal->combatPoint = combatPoint;
        ent->NPC->tempGoal->enemy = targetEnt;

        ent->NPC->goalEntity = ent->NPC->tempGoal;
        ent->NPC->goalRadius = radius;

        trap_LinkEntity( ent->NPC->goalEntity );
}

/*
-------------------------
NAV_HitNavGoal
-------------------------
*/

qboolean NAV_HitNavGoal( vec3_t point, vec3_t mins, vec3_t maxs, vec3_t dest, int radius, qboolean flying )
{
        vec3_t  dmins, dmaxs, pmins, pmaxs;

        if ( radius & NAVGOAL_USE_RADIUS )
        {
                radius &= ~NAVGOAL_USE_RADIUS;
                //NOTE:  This needs to do a DistanceSquared on navgoals that had
                //                      a radius manually set! We can't do the smaller navgoals against
                //                      walls to get around this because player-sized traces to them
                //                      from angles will not work... - MCG
                if ( !flying )
                {//Allow for a little z difference
                        vec3_t  diff;
                        VectorSubtract( point, dest, diff );
                        if ( fabs(diff[2]) <= 24 )
                        {
                                diff[2] = 0;
                        }
                        return ( VectorLengthSquared( diff ) <= (radius*radius) );
                }
                else
                {//must hit exactly
                        return ( DistanceSquared(dest, point) <= (radius*radius) );
                }
                //There is probably a better way to do this, either by preserving the original
                //              mins and maxs of the navgoal and doing this check ONLY if the radius 
                //              is non-zero (like the original implementation) or some boolean to
                //              tell us to do this check rather than the fake bbox overlap check...
        }
        else
        {
                //Construct a dummy bounding box from our radius value
                VectorSet( dmins, -radius, -radius, -radius );
                VectorSet( dmaxs,  radius,  radius,  radius );

                //Translate it
                VectorAdd( dmins, dest, dmins );
                VectorAdd( dmaxs, dest, dmaxs );

                //Translate the starting box
                VectorAdd( point, mins, pmins );
                VectorAdd( point, maxs, pmaxs );

                //See if they overlap
                return G_BoundsOverlap( pmins, pmaxs, dmins, dmaxs );
        }
}

/*
-------------------------
NAV_ClearPathToPoint
-------------------------
*/

qboolean NAV_ClearPathToPoint( gentity_t *self, vec3_t pmins, vec3_t pmaxs, vec3_t point, int clipmask, int okToHitEntNum )
{
//      trace_t trace;
//      return NAV_CheckAhead( self, point, trace, clipmask|CONTENTS_BOTCLIP );

        vec3_t  mins, maxs;
        trace_t trace;

        //Test if they're even conceivably close to one another
        if ( !trap_InPVS( self->r.currentOrigin, point ) )
                return qfalse;

        if ( self->flags & FL_NAVGOAL )
        {
                if ( !self->parent )
                {
                        //SHOULD NEVER HAPPEN!!!
                        assert(self->parent);
                        return qfalse;
                }
                VectorCopy( self->parent->r.mins, mins );
                VectorCopy( self->parent->r.maxs, maxs );
        }
        else
        {
                VectorCopy( pmins, mins );
                VectorCopy( pmaxs, maxs );
        }
        
        if ( self->client || ( self->flags & FL_NAVGOAL ) )
        {
                //Clients can step up things, or if this is a navgoal check, a client will be using this info
                mins[2] += STEPSIZE;

                //don't let box get inverted
                if ( mins[2] > maxs[2] )
                {       
                        mins[2] = maxs[2];
                }
        }

        if ( self->flags & FL_NAVGOAL )
        {
                //Trace from point to navgoal
                trap_Trace( &trace, point, mins, maxs, self->r.currentOrigin, self->parent->s.number, (clipmask|CONTENTS_MONSTERCLIP|CONTENTS_BOTCLIP)&~CONTENTS_BODY );
                if ( trace.startsolid&&(trace.contents&CONTENTS_BOTCLIP) )
                {//started inside do not enter, so ignore them
                        clipmask &= ~CONTENTS_BOTCLIP;
                        trap_Trace( &trace, point, mins, maxs, self->r.currentOrigin, self->parent->s.number, (clipmask|CONTENTS_MONSTERCLIP)&~CONTENTS_BODY );
                }
                
                if ( trace.startsolid || trace.allsolid )
                {
                        return qfalse;
                }
                
                //Made it
                if ( trace.fraction == 1.0 )
                {
                        return qtrue;
                }
                
                if ( okToHitEntNum != ENTITYNUM_NONE && trace.entityNum == okToHitEntNum )
                {
                        return qtrue;
                }

                //Okay, didn't get all the way there, let's see if we got close enough:
                if ( NAV_HitNavGoal( self->r.currentOrigin, self->parent->r.mins, self->parent->r.maxs, trace.endpos, NPCInfo->goalRadius, FlyingCreature( self->parent ) ) )
                {
                        return qtrue;
                }
                else
                {
                        if ( NAVDEBUG_showCollision )
                        {
                                if ( trace.entityNum < ENTITYNUM_WORLD && (&g_entities[trace.entityNum] != NULL) && g_entities[trace.entityNum].s.eType != ET_MOVER )
                                {
                                        vec3_t  p1, p2;
                                        G_DrawEdge( point, trace.endpos, EDGE_PATH );
                                        VectorAdd(g_entities[trace.entityNum].r.mins, g_entities[trace.entityNum].r.currentOrigin, p1);
                                        VectorAdd(g_entities[trace.entityNum].r.maxs, g_entities[trace.entityNum].r.currentOrigin, p2);
                                        G_CubeOutline( p1, p2, FRAMETIME, 0x0000ff, 0.5 );
                                }
                                //FIXME: if it is a bmodel, light up the surf?
                        }
                }
        }
        else
        {
                trap_Trace( &trace, self->r.currentOrigin, mins, maxs, point, self->s.number, clipmask|CONTENTS_MONSTERCLIP|CONTENTS_BOTCLIP);
                if ( trace.startsolid&&(trace.contents&CONTENTS_BOTCLIP) )
                {//started inside do not enter, so ignore them
                        clipmask &= ~CONTENTS_BOTCLIP;
                        trap_Trace( &trace, self->r.currentOrigin, mins, maxs, point, self->s.number, clipmask|CONTENTS_MONSTERCLIP);
                }

                if( ( ( trace.startsolid == qfalse ) && ( trace.allsolid == qfalse ) ) && ( trace.fraction == 1.0f ) )
                {//FIXME: check for drops
                        return qtrue;
                }

                if ( okToHitEntNum != ENTITYNUM_NONE && trace.entityNum == okToHitEntNum )
                {
                        return qtrue;
                }

                if ( NAVDEBUG_showCollision )
                {
                        if ( trace.entityNum < ENTITYNUM_WORLD && (&g_entities[trace.entityNum] != NULL) && g_entities[trace.entityNum].s.eType != ET_MOVER )
                        {
                                vec3_t  p1, p2;
                                G_DrawEdge( self->r.currentOrigin, trace.endpos, EDGE_PATH );
                                VectorAdd(g_entities[trace.entityNum].r.mins, g_entities[trace.entityNum].r.currentOrigin, p1);
                                VectorAdd(g_entities[trace.entityNum].r.maxs, g_entities[trace.entityNum].r.currentOrigin, p2);
                                G_CubeOutline( p1, p2, FRAMETIME, 0x0000ff, 0.5 );
                        }
                        //FIXME: if it is a bmodel, light up the surf?
                }
        }

        return qfalse;
}

/*
-------------------------
NAV_FindClosestWaypointForEnt
-------------------------
*/

int NAV_FindClosestWaypointForEnt( gentity_t *ent, int targWp )
{
        //FIXME: Take the target into account
        return trap_Nav_GetNearestNode( ent, ent->waypoint, NF_CLEAR_PATH, targWp );
}

int NAV_FindClosestWaypointForPoint( gentity_t *ent, vec3_t point )
{
        int     bestWP;
        //FIXME: can we make this a static ent?
        gentity_t *marker = G_Spawn();
        
        if ( !marker )
        {
                return WAYPOINT_NONE;
        }

        G_SetOrigin( marker, point );

        VectorCopy( ent->r.mins, marker->r.mins );//stepsize?
        VectorCopy( ent->r.mins, marker->r.maxs );//crouching?

        marker->clipmask = ent->clipmask;
        marker->waypoint = WAYPOINT_NONE;

        bestWP = trap_Nav_GetNearestNode( marker, marker->waypoint, NF_CLEAR_PATH, WAYPOINT_NONE );

        G_FreeEntity( marker );

        return bestWP;
}

int NAV_FindClosestWaypointForPoint2( vec3_t point )
{
        int     bestWP;
        //FIXME: can we make this a static ent?
        gentity_t *marker = G_Spawn();
        
        if ( !marker )
        {
                return WAYPOINT_NONE;
        }

        G_SetOrigin( marker, point );

        VectorSet( marker->r.mins, -16, -16, -6 );//includes stepsize
        VectorSet( marker->r.maxs, 16, 16, 32 );

        marker->clipmask = MASK_NPCSOLID;
        marker->waypoint = WAYPOINT_NONE;

        bestWP = trap_Nav_GetNearestNode( marker, marker->waypoint, NF_CLEAR_PATH, WAYPOINT_NONE );

        G_FreeEntity( marker );

        return bestWP;
}

/*
-------------------------
NAV_ClearBlockedInfo
-------------------------
*/

void NAV_ClearBlockedInfo( gentity_t *self )
{
        self->NPC->aiFlags &= ~NPCAI_BLOCKED;
        self->NPC->blockingEntNum = ENTITYNUM_WORLD;
}

/*
-------------------------
NAV_SetBlockedInfo
-------------------------
*/

void NAV_SetBlockedInfo( gentity_t *self, int entId )
{
        self->NPC->aiFlags |= NPCAI_BLOCKED;
        self->NPC->blockingEntNum = entId;
}

/*
-------------------------
NAV_Steer
-------------------------
*/

int NAV_Steer( gentity_t *self, vec3_t dir, float distance )
{
        vec3_t  right_test, left_test;
        vec3_t  deviation;
        trace_t tr;
        float   right_push;
        float   left_push;
        float   right_ang       = dir[YAW] + 45;
        float   left_ang        = dir[YAW] - 45;

        //Get the steering angles
        VectorCopy( dir, deviation );
        deviation[YAW] = right_ang;

        AngleVectors( deviation, right_test, NULL, NULL );

        deviation[YAW] = left_ang;

        AngleVectors( deviation, left_test, NULL, NULL );

        //Find the end positions
        VectorMA( self->r.currentOrigin, distance, right_test, right_test );
        VectorMA( self->r.currentOrigin, distance, left_test,  left_test );

        //Draw for debug purposes
        if ( NAVDEBUG_showCollision )
        {
                G_DrawEdge( self->r.currentOrigin, right_test, EDGE_PATH );
                G_DrawEdge( self->r.currentOrigin, left_test,  EDGE_PATH );
        }

        //Find the right influence
        NAV_CheckAhead( self, right_test, &tr, self->clipmask|CONTENTS_BOTCLIP );

        right_push = -45 * ( 1.0f - tr.fraction );

        //Find the left influence
        NAV_CheckAhead( self, left_test, &tr, self->clipmask|CONTENTS_BOTCLIP );

        left_push = 45 * ( 1.0f - tr.fraction );

        //Influence the mover to respond to the steering
        VectorCopy( dir, deviation );
        deviation[YAW] += ( left_push + right_push );

        return deviation[YAW];
}

/*
-------------------------
NAV_CheckAhead
-------------------------
*/

qboolean NAV_CheckAhead( gentity_t *self, vec3_t end, trace_t *trace, int clipmask )
{
        vec3_t  mins;
        float   radius;
        float   dist;
        float   tFrac;

        //Offset the step height
        VectorSet( mins, self->r.mins[0], self->r.mins[1], self->r.mins[2] + STEPSIZE );
        
        trap_Trace( trace, self->r.currentOrigin, mins, self->r.maxs, end, self->s.number, clipmask );

        if ( trace->startsolid&&(trace->contents&CONTENTS_BOTCLIP) )
        {//started inside do not enter, so ignore them
                clipmask &= ~CONTENTS_BOTCLIP;
                trap_Trace( trace, self->r.currentOrigin, mins, self->r.maxs, end, self->s.number, clipmask );
        }
        //Do a simple check
        if ( ( trace->allsolid == qfalse ) && ( trace->startsolid == qfalse ) && ( trace->fraction == 1.0f ) )
                return qtrue;

        //See if we're too far above
        if ( fabs( self->r.currentOrigin[2] - end[2] ) > 48 )
                return qfalse;

        //This is a work around
        radius = ( self->r.maxs[0] > self->r.maxs[1] ) ? self->r.maxs[0] : self->r.maxs[1];
        dist = Distance( self->r.currentOrigin, end );
        tFrac = 1.0f - ( radius / dist );

        if ( trace->fraction >= tFrac )
                return qtrue;

        //Do a special check for doors
        if ( trace->entityNum < ENTITYNUM_WORLD )
        {
                gentity_t       *blocker = &g_entities[trace->entityNum];
                
                if VALIDSTRING( blocker->classname )
                {
                        if ( G_EntIsUnlockedDoor( blocker->s.number ) )
                        //if ( Q_stricmp( blocker->classname, "func_door" ) == 0 )
                        {
                                //We're too close, try and avoid the door (most likely stuck on a lip)
                                if ( DistanceSquared( self->r.currentOrigin, trace->endpos ) < MIN_DOOR_BLOCK_DIST_SQR )
                                        return qfalse;

                                return qtrue;
                        }
                }
        }

        return qfalse;
}

/*
-------------------------
NAV_TestBypass
-------------------------
*/

static qboolean NAV_TestBypass( gentity_t *self, float yaw, float blocked_dist, vec3_t movedir )
{
        trace_t tr;
        vec3_t  avoidAngles;
        vec3_t  block_test, block_pos;

        VectorClear( avoidAngles );
        avoidAngles[YAW] = yaw;

        AngleVectors( avoidAngles, block_test, NULL, NULL );
        VectorMA( self->r.currentOrigin, blocked_dist, block_test, block_pos );
                
        if ( NAVDEBUG_showCollision )
        {
                G_DrawEdge( self->r.currentOrigin, block_pos, EDGE_BLOCKED );
        }

        //See if we're clear to move in that direction
        if ( NAV_CheckAhead( self, block_pos, &tr, ( self->clipmask & ~CONTENTS_BODY )|CONTENTS_BOTCLIP ) )
        {
                VectorCopy( block_test, movedir );
                
                return qtrue;
        }

        return qfalse;
}

/*
-------------------------
NAV_Bypass
-------------------------
*/

qboolean NAV_Bypass( gentity_t *self, gentity_t *blocker, vec3_t blocked_dir, float blocked_dist, vec3_t movedir ) 
{
        float dot, yaw, avoidRadius, arcAngle;
        vec3_t  right;

        //Draw debug info if requested
        if ( NAVDEBUG_showCollision )
        {
                G_DrawEdge( self->r.currentOrigin, blocker->r.currentOrigin, EDGE_NORMAL );
        }

        AngleVectors( self->r.currentAngles, NULL, right, NULL );

        //Get the blocked direction
        yaw = vectoyaw( blocked_dir );

        //Get the avoid radius
        avoidRadius = sqrt( ( blocker->r.maxs[0] * blocker->r.maxs[0] ) + ( blocker->r.maxs[1] * blocker->r.maxs[1] ) ) + 
                                                sqrt( ( self->r.maxs[0] * self->r.maxs[0] ) + ( self->r.maxs[1] * self->r.maxs[1] ) );

        //See if we're inside our avoidance radius
        arcAngle = ( blocked_dist <= avoidRadius ) ? 135 : ( ( avoidRadius / blocked_dist ) * 90 );

        //FIXME: Although the below code will cause the NPC to take the "better" route, it can cause NPCs to become stuck on
        //               one another in certain situations where both decide to take the same direction.

        //Check to see what dir the other guy is moving in (if any) and pick the opposite dir
        if ( blocker->client && !VectorCompare( blocker->client->ps.velocity, vec3_origin ) )
        {
                vec3_t blocker_movedir;
                VectorNormalize2( blocker->client->ps.velocity, blocker_movedir );
                dot = DotProduct( blocker_movedir, blocked_dir );
                if ( dot < 0.35f && dot > -0.35f )
                {//he's moving to the side of me
                        vec3_t  block_pos;
                        trace_t tr;
                        VectorScale( blocker_movedir, -1, blocker_movedir );
                        VectorMA( self->r.currentOrigin, blocked_dist, blocker_movedir, block_pos );
                        if ( NAV_CheckAhead( self, block_pos, &tr, ( self->clipmask & ~CONTENTS_BODY )|CONTENTS_BOTCLIP ) )
                        {
                                VectorCopy( blocker_movedir, movedir );
                                return qtrue;
                        }
                }
        }

        //FIXME: this makes NPCs stack up and ping-pong like crazy.
        //                      Need to keep track of this and stop trying after a while
        dot = DotProduct( blocked_dir, right );

        //Go right on the first try if that works better
        if ( dot < 0.0f )
                arcAngle *= -1;

        //Test full, best position first
        if ( NAV_TestBypass( self, AngleNormalize360( yaw + arcAngle ), blocked_dist, movedir ) )
                return qtrue;

        //Try a smaller arc
        if ( NAV_TestBypass( self, AngleNormalize360( yaw + ( arcAngle * 0.5f ) ), blocked_dist, movedir ) )
                return qtrue;

        //Try the other direction
        if ( NAV_TestBypass( self, AngleNormalize360( yaw + ( arcAngle * -1 ) ), blocked_dist, movedir ) )
                return qtrue;

        //Try the other direction more precisely
        if ( NAV_TestBypass( self, AngleNormalize360( yaw + ( ( arcAngle * -1 ) * 0.5f ) ), blocked_dist, movedir ) )
                return qtrue;

        //Unable to go around
        return qfalse;
}

/*
-------------------------
NAV_MoveBlocker
-------------------------
*/

qboolean NAV_MoveBlocker( gentity_t *self, vec3_t shove_dir )
{
        //FIXME: This is a temporary method for making blockers move
        
        //FIXME: This will, of course, push blockers off of cliffs, into walls and all over the place

        vec3_t  temp_dir, forward;

        vectoangles( shove_dir, temp_dir );

        temp_dir[YAW] += 45;
        AngleVectors( temp_dir, forward, NULL, NULL );

        VectorScale( forward, SHOVE_SPEED, self->client->ps.velocity );
        self->client->ps.velocity[2] += SHOVE_LIFT;

        //self->NPC->shoveDebounce = level.time + 100;

        return qtrue;
}

/*
-------------------------
NAV_ResolveBlock
-------------------------
*/

qboolean NAV_ResolveBlock( gentity_t *self, gentity_t *blocker, vec3_t blocked_dir )
{
        //Stop double waiting
        if ( ( blocker->NPC ) && ( blocker->NPC->blockingEntNum == self->s.number ) )
                return qtrue;

        //For now, just complain about it
        NPC_Blocked( self, blocker );
        NPC_FaceEntity( blocker, qtrue );

        return qfalse;
}

/*
-------------------------
NAV_TrueCollision
-------------------------
*/

qboolean NAV_TrueCollision( gentity_t *self, gentity_t *blocker, vec3_t movedir, vec3_t blocked_dir )
{
        vec3_t  velocityDir;
        float speed, dot;
        vec3_t  testPos;
        vec3_t  ptmins, ptmaxs, tmins, tmaxs;

        //TODO: Handle all ents
        if ( blocker->client == NULL )
                return qfalse;

        //Get the player's move direction and speed
        speed = VectorNormalize2( self->client->ps.velocity, velocityDir );

        //See if it's even feasible
        dot = DotProduct( movedir, velocityDir );

        if ( dot < 0.85 )
                return qfalse;

        VectorMA( self->r.currentOrigin, speed*FRAMETIME, velocityDir, testPos );

        VectorAdd( blocker->r.currentOrigin, blocker->r.mins, tmins );
        VectorAdd( blocker->r.currentOrigin, blocker->r.maxs, tmaxs );

        VectorAdd( testPos, self->r.mins, ptmins );
        VectorAdd( testPos, self->r.maxs, ptmaxs );

        if ( G_BoundsOverlap( ptmins, ptmaxs, tmins, tmaxs ) )
        {
                VectorCopy( velocityDir, blocked_dir );
                return qtrue;
        }

        return qfalse;
}

/*
-------------------------
NAV_StackedCanyon
-------------------------
*/

qboolean NAV_StackedCanyon( gentity_t *self, gentity_t *blocker, vec3_t pathDir )
{
        vec3_t  perp, cross, test;
        float   avoidRadius;
        int             extraClip = CONTENTS_BOTCLIP;
        trace_t tr;

        PerpendicularVector( perp, pathDir );
        CrossProduct( pathDir, perp, cross );

        avoidRadius =   sqrt( ( blocker->r.maxs[0] * blocker->r.maxs[0] ) + ( blocker->r.maxs[1] * blocker->r.maxs[1] ) ) + 
                                        sqrt( ( self->r.maxs[0] * self->r.maxs[0] ) + ( self->r.maxs[1] * self->r.maxs[1] ) );

        VectorMA( blocker->r.currentOrigin, avoidRadius, cross, test );

        trap_Trace( &tr, test, self->r.mins, self->r.maxs, test, self->s.number, self->clipmask|extraClip );
        if ( tr.startsolid&&(tr.contents&CONTENTS_BOTCLIP) )
        {//started inside do not enter, so ignore them
                extraClip &= ~CONTENTS_BOTCLIP;
                trap_Trace( &tr, test, self->r.mins, self->r.maxs, test, self->s.number, self->clipmask|extraClip );
        }

        if ( NAVDEBUG_showCollision )
        {
                vec3_t  mins, maxs;
                vec3_t  RED = { 1.0f, 0.0f, 0.0f };

                VectorAdd( test, self->r.mins, mins );
                VectorAdd( test, self->r.maxs, maxs );
                G_Cube( mins, maxs, RED, 0.25 );
        }

        if ( tr.startsolid == qfalse && tr.allsolid == qfalse )
                return qfalse;

        VectorMA( blocker->r.currentOrigin, -avoidRadius, cross, test );

        trap_Trace( &tr, test, self->r.mins, self->r.maxs, test, self->s.number, self->clipmask|extraClip );
        if ( tr.startsolid&&(tr.contents&CONTENTS_BOTCLIP) )
        {//started inside do not enter, so ignore them
                extraClip &= ~CONTENTS_BOTCLIP;
                trap_Trace( &tr, test, self->r.mins, self->r.maxs, test, self->s.number, self->clipmask|extraClip );
        }

        if ( tr.startsolid == qfalse && tr.allsolid == qfalse )
                return qfalse;

        if ( NAVDEBUG_showCollision )
        {
                vec3_t  mins, maxs;
                vec3_t  RED = { 1.0f, 0.0f, 0.0f };

                VectorAdd( test, self->r.mins, mins );
                VectorAdd( test, self->r.maxs, maxs );
                G_Cube( mins, maxs, RED, 0.25 );
        }

        return qtrue;
}

/*
-------------------------
NAV_ResolveEntityCollision
-------------------------
*/

qboolean NAV_ResolveEntityCollision( gentity_t *self, gentity_t *blocker, vec3_t movedir, vec3_t pathDir )
{
        vec3_t  blocked_dir;
        float blocked_dist;

        //Doors are ignored
        if ( G_EntIsUnlockedDoor( blocker->s.number ) )
        //if ( Q_stricmp( blocker->classname, "func_door" ) == 0 )
        {
                if ( DistanceSquared( self->r.currentOrigin, blocker->r.currentOrigin ) > MIN_DOOR_BLOCK_DIST_SQR )
                        return qtrue;
        }

        VectorSubtract( blocker->r.currentOrigin, self->r.currentOrigin, blocked_dir );
        blocked_dist = VectorNormalize( blocked_dir );

        //Make sure an actual collision is going to happen
//      if ( NAV_PredictCollision( self, blocker, movedir, blocked_dir ) == qfalse )
//              return qtrue;
        
        //See if we can get around the blocker at all (only for player!)
        if ( blocker->s.number == 0 )
        {
                if ( NAV_StackedCanyon( self, blocker, pathDir ) )
                {
                        NPC_Blocked( self, blocker );
                        NPC_FaceEntity( blocker, qtrue );

                        return qfalse;
                }
        }

        //First, attempt to walk around the blocker
        if ( NAV_Bypass( self, blocker, blocked_dir, blocked_dist, movedir ) )
                return qtrue;

        //Second, attempt to calculate a good move position for the blocker
        if ( NAV_ResolveBlock( self, blocker, blocked_dir ) )
                return qtrue;

        return qfalse;
}

/*
-------------------------
NAV_TestForBlocked
-------------------------
*/

qboolean NAV_TestForBlocked( gentity_t *self, gentity_t *goal, gentity_t *blocker, float distance, int *flags )
{
        if ( goal == NULL )
                return qfalse;

        if ( blocker->s.eType == ET_ITEM )
                return qfalse;

        if ( NAV_HitNavGoal( blocker->r.currentOrigin, blocker->r.mins, blocker->r.maxs, goal->r.currentOrigin, 12, qfalse ) )
        {
                *flags |= NIF_BLOCKED;

                if ( distance <= MIN_STOP_DIST )
                {
                        NPC_Blocked( self, blocker );
                        NPC_FaceEntity( blocker, qtrue );
                        return qtrue;
                }
        }

        return qfalse;
}

/*
-------------------------
NAV_AvoidCollsion
-------------------------
*/

qboolean NAV_AvoidCollision( gentity_t *self, gentity_t *goal, navInfo_t *info )
{
        vec3_t  movedir;
        vec3_t  movepos;

        //Clear our block info for this frame
        NAV_ClearBlockedInfo( NPC );

        //Cap our distance
        if ( info->distance > MAX_COLL_AVOID_DIST )
        {
                info->distance = MAX_COLL_AVOID_DIST;
        }

        //Get an end position
        VectorMA( self->r.currentOrigin, info->distance, info->direction, movepos );
        VectorCopy( info->direction, movedir );

        if ( self && self->NPC && (self->NPC->aiFlags&NPCAI_NO_COLL_AVOID) )
        {//pretend there's no-one in the way
                return qtrue;
        }
        //Now test against entities
        if ( NAV_CheckAhead( self, movepos, &info->trace, CONTENTS_BODY ) == qfalse )
        {
                //Get the blocker
                info->blocker = &g_entities[ info->trace.entityNum ];
                info->flags |= NIF_COLLISION;

                //Ok to hit our goal entity
                if ( goal == info->blocker )
                        return qtrue;

                //See if we're moving along with them
                //if ( NAV_TrueCollision( self, info.blocker, movedir, info.direction ) == qfalse )
                //      return qtrue;

                //Test for blocking by standing on goal
                if ( NAV_TestForBlocked( self, goal, info->blocker, info->distance, &info->flags ) == qtrue )
                        return qfalse;

                //If the above function said we're blocked, don't do the extra checks
                if ( info->flags & NIF_BLOCKED )
                        return qtrue;

                //See if we can get that entity to move out of our way
                if ( NAV_ResolveEntityCollision( self, info->blocker, movedir, info->pathDirection ) == qfalse )
                        return qfalse;

                VectorCopy( movedir, info->direction );
                
                return qtrue;
        }

        //Our path is clear, just move there
        if ( NAVDEBUG_showCollision )
        {
                G_DrawEdge( self->r.currentOrigin, movepos, EDGE_PATH );
        }

        return qtrue;
}

/*
-------------------------
NAV_TestBestNode
-------------------------
*/

int NAV_TestBestNode( gentity_t *self, int startID, int endID, qboolean failEdge )
{//check only against architectrure
        vec3_t  end;
        trace_t trace;
        vec3_t  mins;
        int             clipmask = (NPC->clipmask&~CONTENTS_BODY)|CONTENTS_BOTCLIP;

        //get the position for the test choice
        trap_Nav_GetNodePosition( endID, end );

        //Offset the step height
        VectorSet( mins, self->r.mins[0], self->r.mins[1], self->r.mins[2] + STEPSIZE );
        
        trap_Trace( &trace, self->r.currentOrigin, mins, self->r.maxs, end, self->s.number, clipmask );

        if ( trace.startsolid&&(trace.contents&CONTENTS_BOTCLIP) )
        {//started inside do not enter, so ignore them
                clipmask &= ~CONTENTS_BOTCLIP;
                trap_Trace( &trace, self->r.currentOrigin, mins, self->r.maxs, end, self->s.number, clipmask );
        }
        //Do a simple check
        if ( ( trace.allsolid == qfalse ) && ( trace.startsolid == qfalse ) && ( trace.fraction == 1.0f ) )
        {//it's clear
                return endID;
        }

        //See if we're too far above
        if ( self->s.weapon != WP_SABER && fabs( self->r.currentOrigin[2] - end[2] ) > 48 )
        {
        }
        else
        {
                //This is a work around
                float   radius = ( self->r.maxs[0] > self->r.maxs[1] ) ? self->r.maxs[0] : self->r.maxs[1];
                float   dist = Distance( self->r.currentOrigin, end );
                float   tFrac = 1.0f - ( radius / dist );

                if ( trace.fraction >= tFrac )
                {//it's clear
                        return endID;
                }
        }

        //Do a special check for doors
        if ( trace.entityNum < ENTITYNUM_WORLD )
        {
                gentity_t       *blocker = &g_entities[trace.entityNum];
                
                if VALIDSTRING( blocker->classname )
                {//special case: doors are architecture, but are dynamic, like entitites
                        if ( G_EntIsUnlockedDoor( blocker->s.number ) )
                        //if ( Q_stricmp( blocker->classname, "func_door" ) == 0 )
                        {//it's unlocked, go for it
                                //We're too close, try and avoid the door (most likely stuck on a lip)
                                if ( DistanceSquared( self->r.currentOrigin, trace.endpos ) < MIN_DOOR_BLOCK_DIST_SQR )
                                {
                                        return startID;
                                }
                                //we can keep heading to the door, it should open
                                if ( self->s.weapon != WP_SABER && fabs( self->r.currentOrigin[2] - end[2] ) > 48 )
                                {//too far above
                                }
                                else
                                {
                                        return endID;
                                }
                        }
                        else if ( G_EntIsDoor( blocker->s.number ) )
                        {//a locked door!
                                //path is blocked by a locked door, mark it as such if instructed to do so
                                if ( failEdge )
                                {
                                        trap_Nav_AddFailedEdge( self->s.number, startID, endID );
                                }
                        }
                        else if ( G_EntIsBreakable( blocker->s.number ) )
                        {//do same for breakable brushes/models/glass?
                                //path is blocked by a breakable, mark it as such if instructed to do so
                                if ( failEdge )
                                {
                                        trap_Nav_AddFailedEdge( self->s.number, startID, endID );
                                }
                        }
                        else if ( G_EntIsRemovableUsable( blocker->s.number ) )
                        {//and removable usables
                                //path is blocked by a removable usable, mark it as such if instructed to do so
                                if ( failEdge )
                                {
                                        trap_Nav_AddFailedEdge( self->s.number, startID, endID );
                                }
                        }
                        else if ( blocker->targetname && blocker->s.solid == SOLID_BMODEL && ((blocker->r.contents&CONTENTS_MONSTERCLIP)|| (blocker->r.contents&CONTENTS_BOTCLIP)) )
                        {//some other kind of do not enter entity brush that will probably be removed
                                //path is blocked by a removable brushent, mark it as such if instructed to do so
                                if ( failEdge )
                                {
                                        trap_Nav_AddFailedEdge( self->s.number, startID, endID );
                                }
                        }
                }
        }
        //path is blocked 
        //use the fallback choice
        return startID;
}

/*
-------------------------
NAV_GetNearestNode
-------------------------
*/

int NAV_GetNearestNode( gentity_t *self, int lastNode )
{
        return trap_Nav_GetNearestNode( self, lastNode, NF_CLEAR_PATH, WAYPOINT_NONE );
}

/*
-------------------------
NAV_MicroError
-------------------------
*/

qboolean NAV_MicroError( vec3_t start, vec3_t end )
{
        if ( VectorCompare( start, end ) )
        {
                if ( DistanceSquared( NPC->r.currentOrigin, start ) < (8*8) )
                {
                        return qtrue;
                }
        }

        return qfalse;
}

/*
-------------------------
NAV_MoveToGoal
-------------------------
*/

int     NAV_MoveToGoal( gentity_t *self, navInfo_t *info )
{
        int bestNode;
        vec3_t  origin, end;

        //Must have a goal entity to move there
        if( self->NPC->goalEntity == NULL )
                return WAYPOINT_NONE;

        //Check special player optimizations
        if ( self->NPC->goalEntity->s.number == 0 )
        {
                //If we couldn't find the point, then we won't be able to this turn
                if ( self->NPC->goalEntity->waypoint == WAYPOINT_NONE )
                        return WAYPOINT_NONE;

                //NOTENOTE: Otherwise trust this waypoint for the whole frame (reduce all unnecessary calculations)
        }
        else
        {
                //Find the target's waypoint
                if ( ( self->NPC->goalEntity->waypoint = NAV_GetNearestNode( self->NPC->goalEntity, self->NPC->goalEntity->waypoint ) ) == WAYPOINT_NONE )
                        return WAYPOINT_NONE;
        }

        //Find our waypoint
        if ( ( self->waypoint = NAV_GetNearestNode( self, self->lastWaypoint ) ) == WAYPOINT_NONE )
                return WAYPOINT_NONE;

        bestNode = trap_Nav_GetBestNode( self->waypoint, self->NPC->goalEntity->waypoint, NODE_NONE );

        if ( bestNode == WAYPOINT_NONE )
        {
                if ( NAVDEBUG_showEnemyPath )
                {
                        vec3_t  origin, torigin;

                        trap_Nav_GetNodePosition( self->NPC->goalEntity->waypoint, torigin );
                        trap_Nav_GetNodePosition( self->waypoint, origin );

                        G_DrawNode( torigin, NODE_GOAL );
                        G_DrawNode( origin, NODE_GOAL );
                        G_DrawNode( self->NPC->goalEntity->r.currentOrigin, NODE_START );
                }
                
                return WAYPOINT_NONE;
        }

        //Check this node
        bestNode = NAV_TestBestNode( self, bestNode, self->NPC->goalEntity->waypoint, qfalse );

        //trace_t       trace;

        //Get this position
        trap_Nav_GetNodePosition( bestNode, origin );
        trap_Nav_GetNodePosition( self->waypoint, end );

        //Basically, see if the path we have isn't helping
        //if ( NAV_MicroError( origin, end ) )
        //      return WAYPOINT_NONE;

        //Test the path connection from our current position to the best node
        if ( NAV_CheckAhead( self, origin, &info->trace, (self->clipmask&~CONTENTS_BODY)|CONTENTS_BOTCLIP ) == qfalse )
        {
                //First attempt to move to the closest point on the line between the waypoints
                G_FindClosestPointOnLineSegment( origin, end, self->r.currentOrigin, origin );

                //See if we can go there
                if ( NAV_CheckAhead( self, origin, &info->trace, (self->clipmask&~CONTENTS_BODY)|CONTENTS_BOTCLIP ) == qfalse )
                {
                        //Just move towards our current waypoint
                        bestNode = self->waypoint;
                        trap_Nav_GetNodePosition( bestNode, origin );
                }
        }

        //Setup our new move information
        VectorSubtract( origin, self->r.currentOrigin, info->direction );
        info->distance = VectorNormalize( info->direction );

        VectorSubtract( end, origin, info->pathDirection );
        VectorNormalize( info->pathDirection );

        //Draw any debug info, if requested
        if ( NAVDEBUG_showEnemyPath )
        {
                vec3_t  dest, start;

                //Get the positions
                trap_Nav_GetNodePosition( self->NPC->goalEntity->waypoint, dest );
                trap_Nav_GetNodePosition( bestNode, start );

                //Draw the route
                G_DrawNode( start, NODE_START );
                G_DrawNode( dest, NODE_GOAL );
                trap_Nav_ShowPath( self->waypoint, self->NPC->goalEntity->waypoint );
        }

        return bestNode;
}

/*
-------------------------
waypoint_testDirection
-------------------------
*/

unsigned int waypoint_testDirection( vec3_t origin, float yaw, unsigned int minDist )
{