text: #include "cbase.h"
  #include "asw_drone_movement.h"
  #include "igamemovement.h"
  
  // memdbgon must be the last include file in a .cpp file!!!
  #include "tier0/memdbgon.h"
  
  
  CASW_Drone_Movement::CASW_Drone_Movement()
  {
  	mv = NULL;
  	m_pNPC = NULL;
  	m_flInterval = 0;
  	m_LastHitWallNormal.Init(0,0,0);
  	m_surfaceFriction = 0;	
  }
  
  const Vector&	CASW_Drone_Movement::GetOuterMins() const { return m_pNPC->GetHullMins(); }
  const Vector&	CASW_Drone_Movement::GetOuterMaxs() const { return m_pNPC->GetHullMins(); }
  
  extern bool g_bMovementOptimizations;
  extern ConVar sv_bounce;
  extern ConVar sv_gravity;
  extern const char *DescribeAxis( int axis );
  
  #define	STOP_EPSILON		0.1
  #define	MAX_CLIP_PLANES		5
  #define STEP_SIZE		24
  
  int CASW_Drone_Movement::ClipVelocity( Vector& in, Vector& normal, Vector& out, float overbounce )
  {
  	float	backoff;
  	float	change;
  	float angle;
  	int		i, blocked;
  	
  	angle = normal[ 2 ];
  
  	blocked = 0x00;         // Assume unblocked.
  	if (angle > 0)			// If the plane that is blocking us has a positive z component, then assume it's a floor.
  		blocked |= 0x01;	// 
  	if (!angle)				// If the plane has no Z, it is vertical (wall/step)
  		blocked |= 0x02;	// 
  	
  
  	// Determine how far along plane to slide based on incoming direction.
  	backoff = DotProduct (in, normal) * overbounce;
  
  	for (i=0 ; i<3 ; i++)
  	{
  		change = normal[i]*backoff;
  		out[i] = in[i] - change; 
  		// If out velocity is too small, zero it out.
  		if (out[i] > -STOP_EPSILON && out[i] < STOP_EPSILON)
  			out[i] = 0;
  	}
  	
  #if 0
  	// slight adjustment - hopefully to adjust for displacement surfaces
  	float adjust = DotProduct( out, normal );
  	if( adjust < 0.0f )
  	{
  		out += ( normal * -adjust );
  //		Msg( "Adjustment = %lf\n", adjust );
  	}
  #endif
  
  	// Return blocking flags.
  	return blocked;
  }
  
  int CASW_Drone_Movement::TryMove( Vector *pFirstDest, trace_t *pFirstTrace )
  {
  	int			bumpcount, numbumps;
  	Vector		dir;
  	float		d;
  	int			numplanes;
  	Vector		planes[MAX_CLIP_PLANES];
  	Vector		primal_velocity, original_velocity;
  	Vector      new_velocity;
  	int			i, j;
  	trace_t	pm;
  	Vector		end;
  	float		time_left, allFraction;
  	int			blocked;		
  	
  	numbumps  = 4;           // Bump up to four times
  	
  	blocked   = 0;           // Assume not blocked
  	numplanes = 0;           //  and not sliding along any planes
  
  	VectorCopy (mv->m_vecVelocity, original_velocity);  // Store original velocity
  	VectorCopy (mv->m_vecVelocity, primal_velocity);
  	
  	allFraction = 0;
  	time_left = m_flInterval;   // Total time for this movement operation.
  
  	new_velocity.Init();
  
  	// set the last wall normal z high, so any planes found are 'more upright'
  	m_LastHitWallNormal.z = 2.0f;
  
  	for (bumpcount=0 ; bumpcount < numbumps; bumpcount++)
  	{
  		if ( mv->m_vecVelocity.Length() == 0.0 )
  			break;
  
  		// Assume we can move all the way from the current origin to the
  		//  end point.
  		VectorMA( mv->GetAbsOrigin(), time_left, mv->m_vecVelocity, end );
  
  		// See if we can make it from origin to end point.
  		if ( g_bMovementOptimizations )
  		{
  			// If their velocity Z is 0, then we can avoid an extra trace here during WalkMove.
  			if ( pFirstDest && end == *pFirstDest )
  				pm = *pFirstTrace;
  			else
  				UTIL_TraceEntity( m_pNPC, mv->GetAbsOrigin(), end, MASK_NPCSOLID, &pm );
  				//TraceBBox( mv->GetAbsOrigin(), end, MASK_NPCSOLID, m_pNPC->GetCollisionGroup(), pm );
  		}
  		else
  		{
  			UTIL_TraceEntity( m_pNPC, mv->GetAbsOrigin(), end, MASK_NPCSOLID, &pm );
  			//TraceBBox( mv->GetAbsOrigin(), end, MASK_NPCSOLID, m_pNPC->GetCollisionGroup(), pm );
  		}
  
  		allFraction += pm.fraction;
  		
  		// If we started in a solid object, or we were in solid space
  		//  the whole way, zero out our velocity and return that we
  		//  are blocked by floor and wall.
  		if (pm.allsolid)
  		{	
  			// entity is trapped in another solid
  			VectorCopy (vec3_origin, mv->m_vecVelocity);
  			return 4;
  		}
  
  		if (pm.fraction < 1.0f)	// we've hit something, store the most wall-ish normal
  		{
  			// store the most upright plane
  			if (m_LastHitWallNormal.z > pm.plane.normal.z)
  				VectorCopy(pm.plane.normal, m_LastHitWallNormal);
  		}
  
  		// If we moved some portion of the total distance, then
  		//  copy the end position into the pmove.origin and 
  		//  zero the plane counter.
  		if( pm.fraction > 0 )
  		{	
  			// actually covered some distance
  			mv->SetAbsOrigin(pm.endpos);
  			VectorCopy (mv->m_vecVelocity, original_velocity);
  			numplanes = 0;
  		}
  
  		// If we covered the entire distance, we are done
  		//  and can return.
  		if (pm.fraction == 1)
  		{
  			 break;		// moved the entire distance
  		}
  
  		// Save entity that blocked us (since fraction was < 1.0)
  		//  for contact
  		// Add it if it's not already in the list!!!
  		//MoveHelper( )->AddToTouched( pm, mv->m_vecVelocity );
  
  		// If the plane we hit has a high z component in the normal, then
  		//  it's probably a floor
  		if (pm.plane.normal[2] > 0.7)
  		{
  			blocked |= 1;		// floor
  		}
  		
  		// If the plane has a zero z component in the normal, then it's a 
  		//  step or wall
  		if (!pm.plane.normal[2])
  		{
  			blocked |= 2;		// step / wall
  		}
  
  		// Reduce amount of m_flFrameTime left by total time left * fraction
  		//  that we covered.
  		time_left -= time_left * pm.fraction;
  
  		// Did we run out of planes to clip against?
  		if (numplanes >= MAX_CLIP_PLANES)
  		{	
  			// this shouldn't really happen
  			//  Stop our movement if so.
  			VectorCopy (vec3_origin, mv->m_vecVelocity);
  			//Con_DPrintf("Too many planes 4\n");
  
  			break;
  		}
  
  		// Set up next clipping plane
  		VectorCopy (pm.plane.normal, planes[numplanes]);
  		numplanes++;
  
  		// modify original_velocity so it parallels all of the clip planes
  		//
  
  		// relfect npc velocity 
  		// Only give this a try for first impact plane because you can get yourself stuck in an acute corner by jumping in place
  		//  and pressing forward and nobody was really using this bounce/reflection feature anyway...
  		if ( numplanes == 1 &&
  			m_pNPC->GetMoveType() == MOVETYPE_WALK &&
  			m_pNPC->GetGroundEntity() == NULL )	
  		{
  			for ( i = 0; i < numplanes; i++ )
  			{
  				if ( planes[i][2] > 0.7  )
  				{
  					// floor or slope
  					ClipVelocity( original_velocity, planes[i], new_velocity, 1 );
  					VectorCopy( new_velocity, original_velocity );
  				}
  				else
  				{
  					ClipVelocity( original_velocity, planes[i], new_velocity, 1.0 + sv_bounce.GetFloat() * (1 - m_surfaceFriction) );
  				}
  			}
  
  			VectorCopy( new_velocity, mv->m_vecVelocity );
  			VectorCopy( new_velocity, original_velocity );
  		}
  		else
  		{
  			for (i=0 ; i < numplanes ; i++)
  			{
  				ClipVelocity (
  					original_velocity,
  					planes[i],
  					mv->m_vecVelocity,
  					1);
  
  				for (j=0 ; j<numplanes ; j++)
  					if (j != i)
  					{
  						// Are we now moving against this plane?
  						if (mv->m_vecVelocity.Dot(planes[j]) < 0)
  							break;	// not ok
  					}
  				if (j == numplanes)  // Didn't have to clip, so we're ok
  					break;
  			}
  			
  			// Did we go all the way through plane set
  			if (i != numplanes)
  			{	// go along this plane
  				// pmove.velocity is set in clipping call, no need to set again.
  				;  
  			}
  			else
  			{	// go along the crease
  				if (numplanes != 2)
  				{
  					VectorCopy (vec3_origin, mv->m_vecVelocity);
  					break;
  				}
  				CrossProduct (planes[0], planes[1], dir);
  				d = dir.Dot(mv->m_vecVelocity);
  				VectorScale (dir, d, mv->m_vecVelocity );
  			}
  
  			//
  			// if original velocity is against the original velocity, stop dead
  			// to avoid tiny occilations in sloping corners
  			//
  			d = mv->m_vecVelocity.Dot(primal_velocity);
  			if (d <= 0)
  			{
  				//Con_DPrintf("Back\n");
  				VectorCopy (vec3_origin, mv->m_vecVelocity);
  				break;
  			}
  		}
  	}
  
  	if ( allFraction == 0 )
  	{
  		VectorCopy (vec3_origin, mv->m_vecVelocity);
  	}
  
  	return blocked;
  }
  
  //-----------------------------------------------------------------------------
  // Purpose: Does the basic move attempting to climb up step heights.  It uses
  //          the mv->m_vecAbsOrigin and mv->m_vecVelocity.  It returns a new
  //          new mv->m_vecAbsOrigin, mv->m_vecVelocity, and mv->m_outStepHeight.
  //-----------------------------------------------------------------------------
  void CASW_Drone_Movement::StepMove( Vector &vecDestination, trace_t &trace )
  {
  //	CheckStuck();
  	Vector vecEndPos;
  	VectorCopy( vecDestination, vecEndPos );
  
  	// Try sliding forward both on ground and up 16 pixels
  	//  take the move that goes farthest
  	Vector vecPos, vecVel;
  	VectorCopy( mv->GetAbsOrigin(), vecPos );
  	VectorCopy( mv->m_vecVelocity, vecVel );
  
  	// Slide move down.
  	Vector downWallNormal;
  	TryMove( &vecEndPos, &trace );
  	downWallNormal = m_LastHitWallNormal;
  	
  	// Down results.
  	Vector vecDownPos, vecDownVel;
  	VectorCopy( mv->GetAbsOrigin(), vecDownPos );
  	VectorCopy( mv->m_vecVelocity, vecDownVel );
  	
  	// Reset original values.
  	mv->SetAbsOrigin( vecPos );
  	VectorCopy( vecVel, mv->m_vecVelocity );
  	
  	// Move up a stair height.
  	VectorCopy( mv->GetAbsOrigin(), vecEndPos );
  	vecEndPos.z += STEP_SIZE;	
  	
  	UTIL_TraceEntity( m_pNPC, mv->GetAbsOrigin(), vecEndPos, MASK_NPCSOLID, &trace );
  	//TraceBBox( mv->GetAbsOrigin(), vecEndPos, MASK_NPCSOLID, m_pNPC->GetCollisionGroup(), trace );
  	if ( !trace.startsolid && !trace.allsolid )
  	{
  		mv->SetAbsOrigin( trace.endpos );
  	}
  	
  	// Slide move up.
  	Vector upWallNormal;
  	TryMove(NULL, NULL);
  	upWallNormal = m_LastHitWallNormal;
  	
  	// Move down a stair (attempt to).
  	VectorCopy( mv->GetAbsOrigin(), vecEndPos );
  	vecEndPos.z -= STEP_SIZE;
  		
  	UTIL_TraceEntity( m_pNPC, mv->GetAbsOrigin(), vecEndPos, MASK_NPCSOLID, &trace );
  	//TraceBBox( mv->GetAbsOrigin(), vecEndPos, MASK_NPCSOLID, m_pNPC->GetCollisionGroup(), trace );
  	
  	// If we are not on the ground any more then use the original movement attempt.
  	if ( trace.plane.normal[2] < 0.7 )
  	{
  		mv->SetAbsOrigin( vecDownPos );
  		VectorCopy( vecDownVel, mv->m_vecVelocity );
  		VectorCopy( downWallNormal, m_LastHitWallNormal );
  		float flStepDist = mv->GetAbsOrigin().z - vecPos.z;
  		if ( flStepDist > 0.0f )
  		{
  			mv->m_outStepHeight += flStepDist;
  		}
  
  		return;
  	}
  	
  	// If the trace ended up in empty space, copy the end over to the origin.
  	if ( !trace.startsolid && !trace.allsolid )
  	{
  		mv->SetAbsOrigin( trace.endpos );
  	}
  	
  	// Copy this origin to up.
  	Vector vecUpPos;
  	VectorCopy( mv->GetAbsOrigin(), vecUpPos );
  	VectorCopy( upWallNormal, m_LastHitWallNormal);
  	
  	// decide which one went farther
  	float flDownDist = ( vecDownPos.x - vecPos.x ) * ( vecDownPos.x - vecPos.x ) + ( vecDownPos.y - vecPos.y ) * ( vecDownPos.y - vecPos.y );
  	float flUpDist = ( vecUpPos.x - vecPos.x ) * ( vecUpPos.x - vecPos.x ) + ( vecUpPos.y - vecPos.y ) * ( vecUpPos.y - vecPos.y );
  	if ( flDownDist > flUpDist )
  	{
  		mv->SetAbsOrigin( vecDownPos );
  		VectorCopy( vecDownVel, mv->m_vecVelocity );
  		VectorCopy( downWallNormal, m_LastHitWallNormal);
  	}
  	else 
  	{
  		// copy z value from slide move
  		mv->m_vecVelocity.z = vecDownVel.z;
  	}
  	
  	float flStepDist = mv->GetAbsOrigin().z - vecPos.z;
  	if ( flStepDist > 0 )
  	{
  		mv->m_outStepHeight += flStepDist;
  	}
  }
  
  void CASW_Drone_Movement::WalkMove()
  {
  	// Add in any base velocity to the current velocity.
  	VectorAdd (mv->m_vecVelocity, m_pNPC->GetBaseVelocity(), mv->m_vecVelocity );
  
  	// if we're barely moving, then zero the velocity and stop
  	float spd = VectorLength( mv->m_vecVelocity );
  	if ( spd < 1.0f || m_flInterval <= 0)
  	{
  		mv->m_vecVelocity.Init();
  		// Now pull the base velocity back out.   Base velocity is set if you are on a moving object, like a conveyor (or maybe another monster?)
  		VectorSubtract( mv->m_vecVelocity, m_pNPC->GetBaseVelocity(), mv->m_vecVelocity );
  		return;
  	}
  
  	// first try just moving to the destination	
  	Vector dest;
  	dest[0] = mv->GetAbsOrigin()[0] + mv->m_vecVelocity[0]*m_flInterval;
  	dest[1] = mv->GetAbsOrigin()[1] + mv->m_vecVelocity[1]*m_flInterval;
  	dest[2] = mv->GetAbsOrigin()[2];
  	trace_t pm;
  	UTIL_TraceEntity( m_pNPC, mv->GetAbsOrigin(), dest, MASK_NPCSOLID, &pm );
  	//TraceBBox( mv->GetAbsOrigin(), dest, MASK_NPCSOLID, m_pNPC->GetCollisionGroup(), pm );
  
  	// if we made it all the way there, then set that as our new origin and return
  	if ( pm.fraction == 1 )
  	{
  		mv->SetAbsOrigin( pm.endpos );
  		m_pNPC->PhysicsTouchTriggers();
  		// Now pull the base velocity back out.   Base velocity is set if you are on a moving object, like a conveyor (or maybe another monster?)
  		VectorSubtract( mv->m_vecVelocity, m_pNPC->GetBaseVelocity(), mv->m_vecVelocity );
  		return;
  	}
  
  	// if NPC started the move on the ground, then try to move up/down steps
  	if ( m_pNPC->GetGroundEntity() != NULL )
  	{
  		StepMove( dest, pm );		
  	}
  
  	// Now pull the base velocity back out.   Base velocity is set if you are on a moving object, like a conveyor (or maybe another monster?)
  	VectorSubtract( mv->m_vecVelocity, m_pNPC->GetBaseVelocity(), mv->m_vecVelocity );
  }
  
  void CASW_Drone_Movement::CategorizePosition()
  {
  	Vector point, bumpOrigin;
  	trace_t pm;
  
  	point = bumpOrigin = mv->GetAbsOrigin();
  	point.z -= 2;
  	bumpOrigin.z += 2;
  
  	// Shooting up really fast.  Definitely not on ground.
  	if ( mv->m_vecVelocity[2] > 140 || 
  		( mv->m_vecVelocity[2] > 0.0f && m_pNPC->GetMoveType() == MOVETYPE_LADDER ) )   
  	{
  		SetGroundEntity( (CBaseEntity *)NULL );
  	}
  	else
  	{
  		// Try and move down.
  		UTIL_TraceEntity( m_pNPC, bumpOrigin, point, MASK_NPCSOLID, &pm );
  		//TraceBBox( bumpOrigin, point, MASK_NPCSOLID, m_pNPC->GetCollisionGroup(), pm );
  		
  		// Moving up two units got us stuck in something, start tracing down exactly at our
  		//  current origin (since CheckStuck allowed us to get here, that pos is valid)
  		if ( pm.startsolid )
  		{
  			bumpOrigin = mv->GetAbsOrigin();
  			UTIL_TraceEntity( m_pNPC, bumpOrigin, point, MASK_NPCSOLID, &pm );
  			//TraceBBox( bumpOrigin, point, MASK_NPCSOLID, m_pNPC->GetCollisionGroup(), pm );
  		}
  
  		// If we hit a steep plane, we are not on ground
  		if ( pm.plane.normal[2] < 0.7)
  		{
  			SetGroundEntity( (CBaseEntity *)NULL );	// too steep
  			// probably want to add a check for a +z velocity too!
  			if ( ( mv->m_vecVelocity.z > 0.0f ) && ( m_pNPC->GetMoveType() != MOVETYPE_NOCLIP ) )
  			{
  				m_surfaceFriction = 0.25f;
  			}
  		}
  		else
  		{
  			SetGroundEntity( pm.m_pEnt );  // Otherwise, point to index of ent under us.
  		}
  
  		// If we are on something...
  		if (m_pNPC->GetGroundEntity() != NULL)
  		{
  			
  			// If we could make the move, drop us down that 1 pixel
  			if (!pm.startsolid && !pm.allsolid )
  			{
  				if( pm.fraction )
  				{
  					mv->SetAbsOrigin( pm.endpos );
  				}
  			}
  		}		
  	}
  }
  
  void CASW_Drone_Movement::SetGroundEntity( CBaseEntity *newGround )
  {
  	CBaseEntity *oldGround = m_pNPC->GetGroundEntity();
  	Vector vecBaseVelocity = m_pNPC->GetBaseVelocity();
  
  	if ( !oldGround && newGround )
  	{
  		// Subtract ground velocity at instant we hit ground jumping
  		vecBaseVelocity -= newGround->GetAbsVelocity(); 
  		vecBaseVelocity.z = newGround->GetAbsVelocity().z;
  	}
  	else if ( oldGround && !newGround )
  	{
  		// Add in ground velocity at instant we started jumping
   		vecBaseVelocity += oldGround->GetAbsVelocity();
  		vecBaseVelocity.z = oldGround->GetAbsVelocity().z;
  	}
  	// TODO
  	//else if ( oldGround && newGround && oldGround != newGround )
  	//{
  	//   subtract old and add new ground velocity?  When might his occur, who knows?  ywb 9/24/03
  	//}
  
  	m_pNPC->SetBaseVelocity( vecBaseVelocity );
  	m_pNPC->SetGroundEntity( newGround );
  }
  
  void CASW_Drone_Movement::StartGravity( void )
  {
  	float ent_gravity;
  	
  	if (m_pNPC->GetGravity())
  		ent_gravity = m_pNPC->GetGravity();
  	else
  		ent_gravity = 1.0; // asw, was 1.0
  	
  	if (!m_pNPC->GetGroundEntity())
  		ent_gravity = 30.0f;
  
  	// Add gravity so they'll be in the correct position during movement
  	// yes, this 0.5 looks wrong, but it's not. 
  	float gravity_effect = (ent_gravity * sv_gravity.GetFloat() * 0.5 * m_flInterval );
  	mv->m_vecVelocity[2] -= gravity_effect;
  	mv->m_vecVelocity[2] += m_pNPC->GetBaseVelocity()[2] * m_flInterval;
  
  	Vector temp = m_pNPC->GetBaseVelocity();
  	temp[ 2 ] = 0;
  	m_pNPC->SetBaseVelocity( temp );
  
  	CheckVelocity();
  }
  
  void CASW_Drone_Movement::FinishGravity( void )
  {
  	/*
  	float ent_gravity;
  
  	if ( m_pNPC->GetGravity() )
  		ent_gravity = m_pNPC->GetGravity();
  	else
  		ent_gravity = 1.0;
  
  	ent_gravity = 3.0;
  
  	// Get the correct velocity for the end of the dt 
    	mv->m_vecVelocity[2] -= (ent_gravity * sv_gravity.GetFloat() * m_flInterval * 0.5);
  	*/
  	// push the drone down by gravity
  	Vector dest = mv->GetAbsOrigin();
  	dest[2] = mv->GetAbsOrigin()[2] + mv->m_vecVelocity.z * m_flInterval;
  	trace_t pm;
  	UTIL_TraceEntity( m_pNPC, mv->GetAbsOrigin(), dest, MASK_NPCSOLID, &pm );
  	if (!pm.startsolid && !pm.allsolid)
  	{
  		dest[2] = mv->GetAbsOrigin()[2] + mv->m_vecVelocity.z * m_flInterval * pm.fraction;
  		mv->SetAbsOrigin( dest );
  	}
  }
  
  void CASW_Drone_Movement::CheckVelocity( void )
  {
  	Vector origin = mv->GetAbsOrigin();
  	bool bFixedOrigin = false;
  	for (int i=0; i < 3; i++)
  	{
  		// See if it's bogus.
  		if (IS_NAN(mv->m_vecVelocity[i]))
  		{
  			DevMsg( 1, "PM  Got a NaN velocity %s\n", DescribeAxis( i ) );
  			mv->m_vecVelocity[i] = 0;
  		}
  		if (IS_NAN(origin[i]))
  		{
  			DevMsg( 1, "PM  Got a NaN origin on %s\n", DescribeAxis( i ) );
  			origin[i] = 0;
  			bFixedOrigin = true;
  		}
  	}
  	if (bFixedOrigin)
  	{
  		mv->SetAbsOrigin( origin );
  	}
  }
  
  void CASW_Drone_Movement::ProcessMovement( CAI_BaseNPC *pNPC, CMoveData *pMove, float flInterval)
  {
  	Assert( pMove && pNPC );
  
  	m_pNPC = pNPC;
  	mv = pMove;
  	m_flInterval = flInterval;
  
  	mv->m_outWishVel.Init();
  	mv->m_outJumpVel.Init();
  
  	Vector start = pMove->GetAbsOrigin();
  
  	CategorizePosition();
  	StartGravity();	
  	WalkMove();
  	FinishGravity();	// pushes him down by gravity
  	CategorizePosition();		
  }
  