C++ AngleNormalize函数代码示例

void CSDKPlayerAnimState::ComputePoseParam_MoveYaw( CStudioHdr *pStudioHdr )
{
	// Get the estimated movement yaw.
	EstimateYaw();

	// Get the view yaw.
	float flAngle = AngleNormalize( m_flEyeYaw );

	// Calc side to side turning - the view vs. movement yaw.
	float flYaw = flAngle - m_PoseParameterData.m_flEstimateYaw;
	flYaw = AngleNormalize( -flYaw );

	// Get the current speed the character is running.
	bool bIsMoving;
	float flPlaybackRate = CalcMovementPlaybackRate( &bIsMoving );

	// Setup the 9-way blend parameters based on our speed and direction.
	Vector2D vecCurrentMoveYaw( 0.0f, 0.0f );
	if ( bIsMoving )
	{
		if ( mp_slammoveyaw.GetBool() )
		{
			flYaw = SnapYawTo( flYaw );
		}
		vecCurrentMoveYaw.x = cos( DEG2RAD( flYaw ) ) * flPlaybackRate;
		vecCurrentMoveYaw.y = -sin( DEG2RAD( flYaw ) ) * flPlaybackRate;
	}

	// Set the 9-way blend movement pose parameters.
	GetBasePlayer()->SetPoseParameter( pStudioHdr, m_PoseParameterData.m_iMoveX, vecCurrentMoveYaw.x );
	GetBasePlayer()->SetPoseParameter( pStudioHdr, m_PoseParameterData.m_iMoveY, -vecCurrentMoveYaw.y ); //Tony; flip it

	m_DebugAnimData.m_vecMoveYaw = vecCurrentMoveYaw;
}

//-----------------------------------------------------------------------------
// Purpose: 
//-----------------------------------------------------------------------------
void CObjectBaseMannedGun::ProcessMovement( CBasePlayer *pPlayer, CMoveData *pMove )
{
	m_Movement.ProcessMovement( pPlayer, pMove );

	m_flGunPitch = AngleNormalize( m_flGunPitch );
	m_flBarrelPitch = AngleNormalize( m_flBarrelPitch );
}

//-----------------------------------------------------------------------------
// Purpose: 
// Input  : flGoalYaw - 
//			flYawRate - 
//			flDeltaTime - 
//			&flCurrentYaw - 
//-----------------------------------------------------------------------------
void CSDKPlayerAnimState::ConvergeYawAngles( float flGoalYaw, float flYawRate, float flDeltaTime, float &flCurrentYaw )
{
#define FADE_TURN_DEGREES 60.0f

	// Find the yaw delta.
	float flDeltaYaw = flGoalYaw - flCurrentYaw;
	float flDeltaYawAbs = fabs( flDeltaYaw );
	flDeltaYaw = AngleNormalize( flDeltaYaw );

	// Always do at least a bit of the turn (1%).
	float flScale = 1.0f;
	flScale = flDeltaYawAbs / FADE_TURN_DEGREES;
	flScale = clamp( flScale, 0.01f, 1.0f );

	float flYaw = flYawRate * flDeltaTime * flScale;
	if ( flDeltaYawAbs < flYaw )
	{
		flCurrentYaw = flGoalYaw;
	}
	else
	{
		float flSide = ( flDeltaYaw < 0.0f ) ? -1.0f : 1.0f;
		flCurrentYaw += ( flYaw * flSide );
	}

	flCurrentYaw = AngleNormalize( flCurrentYaw );

#undef FADE_TURN_DEGREES
}

//-----------------------------------------------------------------------------
// Purpose: 
//-----------------------------------------------------------------------------
void CSDKPlayerAnimState::Update( float eyeYaw, float eyePitch )
{
	// Profile the animation update.
	VPROF( "CMultiPlayerAnimState::Update" );

	// Clear animation overlays because we're about to completely reconstruct them.
	ClearAnimationLayers();

	// Some mods don't want to update the player's animation state if they're dead and ragdolled.
	if ( !ShouldUpdateAnimState() )
	{
		ClearAnimationState();
		return;
	}

	// Get the SDK player.
	CSDKPlayer *pSDKPlayer = GetSDKPlayer();
	if ( !pSDKPlayer )
		return;

	// Get the studio header for the player.
	CStudioHdr *pStudioHdr = pSDKPlayer->GetModelPtr();
	if ( !pStudioHdr )
		return;

	// Check to see if we should be updating the animation state - dead, ragdolled?
	if ( !ShouldUpdateAnimState() )
	{
		ClearAnimationState();
		return;
	}

	// Store the eye angles.
	m_flEyeYaw = AngleNormalize( eyeYaw );
	m_flEyePitch = AngleNormalize( eyePitch );

	// Compute the player sequences.
	ComputeSequences( pStudioHdr );

	if ( SetupPoseParameters( pStudioHdr ) )
	{
		// Pose parameter - what direction are the player's legs running in.
		ComputePoseParam_MoveYaw( pStudioHdr );

		// Pose parameter - Torso aiming (up/down).
		ComputePoseParam_AimPitch( pStudioHdr );

		// Pose parameter - Torso aiming (rotation).
		ComputePoseParam_AimYaw( pStudioHdr );
	}

#ifdef CLIENT_DLL 
	if ( C_BasePlayer::ShouldDrawLocalPlayer() )
	{
		m_pSDKPlayer->SetPlaybackRate( 1.0f );
	}
#endif
}

Real AngleInterval::clamp(Real x) const
{
  Real dc=AngleCCWDiff(x,c);
  if(dc <= d) return x;
  Real da=AngleCCWDiff(c,x);
  Real db=AngleCCWDiff(x,AngleNormalize(c+d));
  if(da < db) return c;
  else return AngleNormalize(c+d);
}

void AngleInterval::normalize()
{
  if (d<0) {
    c=AngleNormalize(c+d);
    d=-d;
  }
  else
    c=AngleNormalize(c);
}

void CSDKPlayerAnimState::EstimateYaw( void )
{
	// Get the frame time.
	float flDeltaTime = gpGlobals->frametime * m_pSDKPlayer->GetSlowMoMultiplier();
	if ( flDeltaTime == 0.0f )
		return;

	// Get the player's velocity and angles.
	Vector vecEstVelocity;
	GetOuterAbsVelocity( vecEstVelocity );
	QAngle angles = GetBasePlayer()->GetLocalAngles();

	// If we are not moving, sync up the feet and eyes slowly.
	if (m_pSDKPlayer->m_Shared.IsProne())
	{
		// Don't touch it
	}
	else if ( vecEstVelocity.x == 0.0f && vecEstVelocity.y == 0.0f )
	{
		float flYawDelta = angles[YAW] - m_PoseParameterData.m_flEstimateYaw;
		flYawDelta = AngleNormalize( flYawDelta );

		if ( flDeltaTime < 0.25f )
		{
			flYawDelta *= ( flDeltaTime * 4.0f );
		}
		else
		{
			flYawDelta *= flDeltaTime;
		}

		m_PoseParameterData.m_flEstimateYaw += flYawDelta;
		m_PoseParameterData.m_flEstimateYaw = AngleNormalize( m_PoseParameterData.m_flEstimateYaw );
	}
	else if (m_pSDKPlayer->m_Shared.IsAimedIn() || m_pSDKPlayer->m_Shared.IsDiving() || m_pSDKPlayer->m_Shared.IsRolling() || m_pSDKPlayer->m_Shared.IsSliding())
	{
		m_PoseParameterData.m_flEstimateYaw = ( atan2( vecEstVelocity.y, vecEstVelocity.x ) * 180.0f / M_PI );
		m_PoseParameterData.m_flEstimateYaw = clamp( m_PoseParameterData.m_flEstimateYaw, -180.0f, 180.0f );
	}
	else
	{
		QAngle angDir;
		VectorAngles(vecEstVelocity, angDir);

		if (fabs(AngleNormalize(angDir[YAW] - m_flEyeYaw)) <= 90)
			m_bFacingForward = true;
		else if (fabs(AngleNormalize(angDir[YAW] - m_flEyeYaw)) >= 91)
			m_bFacingForward = false;

		float flYawDelta = AngleNormalize(m_flGoalFeetYaw - m_flCurrentFeetYaw);

		if (m_bFacingForward)
			m_PoseParameterData.m_flEstimateYaw = flYawDelta;
		else
			m_PoseParameterData.m_flEstimateYaw = 180-flYawDelta;
	}
}

//-----------------------------------------------------------------------------
// Purpose: Causes the turret to face its desired angles
//-----------------------------------------------------------------------------
bool CNPC_CeilingTurret::UpdateFacing( void )
{
	bool  bMoved = false;
	matrix3x4_t localToWorld;
	
	GetAttachment( LookupAttachment( "eyes" ), localToWorld );

	Vector vecGoalDir;
	AngleVectors( m_vecGoalAngles, &vecGoalDir );

	Vector vecGoalLocalDir;
	VectorIRotate( vecGoalDir, localToWorld, vecGoalLocalDir );

	if ( g_debug_turret_ceiling.GetBool() )
	{
		Vector	vecMuzzle, vecMuzzleDir;
		QAngle	vecMuzzleAng;

		GetAttachment( "eyes", vecMuzzle, vecMuzzleAng );
		AngleVectors( vecMuzzleAng, &vecMuzzleDir );

		NDebugOverlay::Cross3D( vecMuzzle, -Vector(2,2,2), Vector(2,2,2), 255, 255, 0, false, 0.05 );
		NDebugOverlay::Cross3D( vecMuzzle+(vecMuzzleDir*256), -Vector(2,2,2), Vector(2,2,2), 255, 255, 0, false, 0.05 );
		NDebugOverlay::Line( vecMuzzle, vecMuzzle+(vecMuzzleDir*256), 255, 255, 0, false, 0.05 );
		
		NDebugOverlay::Cross3D( vecMuzzle, -Vector(2,2,2), Vector(2,2,2), 255, 0, 0, false, 0.05 );
		NDebugOverlay::Cross3D( vecMuzzle+(vecGoalDir*256), -Vector(2,2,2), Vector(2,2,2), 255, 0, 0, false, 0.05 );
		NDebugOverlay::Line( vecMuzzle, vecMuzzle+(vecGoalDir*256), 255, 0, 0, false, 0.05 );
	}

	QAngle vecGoalLocalAngles;
	VectorAngles( vecGoalLocalDir, vecGoalLocalAngles );

	// Update pitch
	float flDiff = AngleNormalize( UTIL_ApproachAngle(  vecGoalLocalAngles.x, 0.0, 0.1f * MaxYawSpeed() ) );
	
	SetPoseParameter( m_poseAim_Pitch, GetPoseParameter( m_poseAim_Pitch ) + ( flDiff / 1.5f ) );

	if ( fabs( flDiff ) > 0.1f )
	{
		bMoved = true;
	}

	// Update yaw
	flDiff = AngleNormalize( UTIL_ApproachAngle(  vecGoalLocalAngles.y, 0.0, 0.1f * MaxYawSpeed() ) );

	SetPoseParameter( m_poseAim_Yaw, GetPoseParameter( m_poseAim_Yaw ) + ( flDiff / 1.5f ) );

	if ( fabs( flDiff ) > 0.1f )
	{
		bMoved = true;
	}

	InvalidateBoneCache();

	return bMoved;
}

int CBaseTurret::MoveTurret(void)
{
	bool bDidMove = false;
	int iPose;

	matrix3x4_t localToWorld;
	
	GetAttachment( LookupAttachment( "eyes" ), localToWorld );

	Vector vecGoalDir;
	AngleVectors( m_vecGoalAngles, &vecGoalDir );

	Vector vecGoalLocalDir;
	VectorIRotate( vecGoalDir, localToWorld, vecGoalLocalDir );

	QAngle vecGoalLocalAngles;
	VectorAngles( vecGoalLocalDir, vecGoalLocalAngles );

	float flDiff;
	QAngle vecNewAngles;

  // update pitch
	flDiff = AngleNormalize( UTIL_ApproachAngle(  vecGoalLocalAngles.x, 0.0, 0.1 * m_iBaseTurnRate ) );
	iPose = LookupPoseParameter( TURRET_BC_PITCH );
	SetPoseParameter( iPose, GetPoseParameter( iPose ) + flDiff / 1.5 );

	if (fabs(flDiff) > 0.1)
	{
		bDidMove = true;
	}

	// update yaw, with acceleration
#if 0
	float flDist = AngleNormalize( vecGoalLocalAngles.y );
	float flNewDist;
	float flNewTurnRate;

	ChangeDistance( 0.1, flDist, 0.0, m_fTurnRate, m_iBaseTurnRate, m_iBaseTurnRate * 4, flNewDist, flNewTurnRate );
	m_fTurnRate = flNewTurnRate;
	flDiff = flDist - flNewDist;
#else
	flDiff = AngleNormalize( UTIL_ApproachAngle(  vecGoalLocalAngles.y, 0.0, 0.1 * m_iBaseTurnRate ) );
#endif

	iPose = LookupPoseParameter( TURRET_BC_YAW );
	SetPoseParameter( iPose, GetPoseParameter( iPose ) + flDiff / 1.5 );
	if (fabs(flDiff) > 0.1)
	{
		bDidMove = true;
	}

	if (bDidMove)
	{
		// DevMsg( "(%.2f, %.2f)\n", AngleNormalize( vecGoalLocalAngles.x ), AngleNormalize( vecGoalLocalAngles.y ) );
	}
	return bDidMove;
}

void CAI_Spotlight::UpdateSpotlightDirection( void )
{
	if ( !m_hSpotlight )
	{
		CreateSpotlightEntities();
	}

	// Compute the current beam direction
	Vector vTargetDir;
	VectorSubtract( m_vSpotlightTargetPos, m_hSpotlight->GetAbsStartPos(), vTargetDir ); 
	VectorNormalize(vTargetDir);
	ConstrainToCone( &vTargetDir );

	// Compute the amount to rotate
	float flDot = DotProduct( vTargetDir, m_vSpotlightDir );
	flDot = clamp( flDot, -1.0f, 1.0f );
	float flAngle = AngleNormalize( RAD2DEG( acos( flDot ) ) );
	float flClampedAngle = clamp( flAngle, 0.0f, 45.0f );
	float flBeamTurnRate = SimpleSplineRemapVal( flClampedAngle, 0.0f, 45.0f, 10.0f, 45.0f );
	if ( fabs(flAngle) > flBeamTurnRate * gpGlobals->frametime )
	{
		flAngle = flBeamTurnRate * gpGlobals->frametime;
	}

	// Compute the rotation axis
	Vector vecRotationAxis;
	CrossProduct( m_vSpotlightDir, vTargetDir, vecRotationAxis );
	if ( VectorNormalize( vecRotationAxis ) < 1e-3 )
	{
		vecRotationAxis.Init( 0, 0, 1 );
	}

	// Compute the actual rotation amount, using quat slerp blending
	Quaternion desiredQuat, resultQuat;
	AxisAngleQuaternion( vecRotationAxis, flAngle, desiredQuat );
	QuaternionSlerp( m_vAngularVelocity, desiredQuat, QUAT_BLEND_FACTOR, resultQuat );
	m_vAngularVelocity = resultQuat;

	// If we're really close, and we're not moving very quickly, slam.
	float flActualRotation = AngleNormalize( RAD2DEG(2 * acos(m_vAngularVelocity.w)) );
	if (( flActualRotation < 1e-3 ) && (flAngle < 1e-3 ))
	{
		m_vSpotlightDir = vTargetDir;
		m_vAngularVelocity.Init( 0, 0, 0, 1 );
		return;
	}

	// Update the desired direction
	matrix3x4_t rot;
	Vector vecNewDir;
	QuaternionMatrix( m_vAngularVelocity, rot );
	VectorRotate( m_vSpotlightDir, rot, vecNewDir );
	m_vSpotlightDir = vecNewDir;
	VectorNormalize(m_vSpotlightDir);
}

void CSDKPlayerAnimState::ComputePoseParam_StuntYaw( CStudioHdr *pStudioHdr )
{
	// Get the view yaw.
	float flAngle = AngleNormalize( m_flEyeYaw );

	// Calc side to side turning - the view vs. movement yaw.
	float flYaw = flAngle - m_PoseParameterData.m_flEstimateYaw;
	flYaw = AngleNormalize( flYaw );

	GetBasePlayer()->SetPoseParameter( pStudioHdr, m_iStuntYawPose, flYaw );
}

/**
 * Orient head and eyes towards m_lookAt.
 */
void C_HL2MP_Player::UpdateLookAt( void )
{
	// head yaw
	if (m_headYawPoseParam < 0 || m_headPitchPoseParam < 0)
		return;

	// orient eyes
	m_viewtarget = m_vLookAtTarget;

	// blinking
	if (m_blinkTimer.IsElapsed())
	{
		m_blinktoggle = !m_blinktoggle;
		m_blinkTimer.Start( RandomFloat( 1.5f, 4.0f ) );
	}

	// Figure out where we want to look in world space.
	QAngle desiredAngles;
	Vector to = m_vLookAtTarget - EyePosition();
	VectorAngles( to, desiredAngles );

	// Figure out where our body is facing in world space.
	QAngle bodyAngles( 0, 0, 0 );
	bodyAngles[YAW] = GetLocalAngles()[YAW];


	float flBodyYawDiff = bodyAngles[YAW] - m_flLastBodyYaw;
	m_flLastBodyYaw = bodyAngles[YAW];
	

	// Set the head's yaw.
	float desired = AngleNormalize( desiredAngles[YAW] - bodyAngles[YAW] );
	desired = clamp( desired, m_headYawMin, m_headYawMax );
	m_flCurrentHeadYaw = ApproachAngle( desired, m_flCurrentHeadYaw, 130 * gpGlobals->frametime );

	// Counterrotate the head from the body rotation so it doesn't rotate past its target.
	m_flCurrentHeadYaw = AngleNormalize( m_flCurrentHeadYaw - flBodyYawDiff );
	desired = clamp( desired, m_headYawMin, m_headYawMax );
	
	SetPoseParameter( m_headYawPoseParam, m_flCurrentHeadYaw );

	
	// Set the head's yaw.
	desired = AngleNormalize( desiredAngles[PITCH] );
	desired = clamp( desired, m_headPitchMin, m_headPitchMax );
	
	m_flCurrentHeadPitch = ApproachAngle( desired, m_flCurrentHeadPitch, 130 * gpGlobals->frametime );
	m_flCurrentHeadPitch = AngleNormalize( m_flCurrentHeadPitch );
	SetPoseParameter( m_headPitchPoseParam, m_flCurrentHeadPitch );
}

//-----------------------------------------------------------------------------
// Purpose: Causes the camera to face its desired angles
//-----------------------------------------------------------------------------
bool CNPC_CombineCamera::UpdateFacing()
{
	bool  bMoved = false;
	matrix3x4_t localToWorld;
	
	GetAttachment(LookupAttachment("eyes"), localToWorld);

	Vector vecGoalDir;
	AngleVectors(m_vecGoalAngles, &vecGoalDir );

	Vector vecGoalLocalDir;
	VectorIRotate(vecGoalDir, localToWorld, vecGoalLocalDir);

	QAngle vecGoalLocalAngles;
	VectorAngles(vecGoalLocalDir, vecGoalLocalAngles);

	// Update pitch
	float flDiff = AngleNormalize(UTIL_ApproachAngle( vecGoalLocalAngles.x, 0.0, 0.1f * MaxYawSpeed()));
	
	int iPose = LookupPoseParameter(COMBINE_CAMERA_BC_PITCH);
	SetPoseParameter(iPose, GetPoseParameter(iPose) + (flDiff / 1.5f));

	if (fabs(flDiff) > 0.1f)
	{
		bMoved = true;
	}

	// Update yaw
	flDiff = AngleNormalize(UTIL_ApproachAngle( vecGoalLocalAngles.y, 0.0, 0.1f * MaxYawSpeed()));

	iPose = LookupPoseParameter(COMBINE_CAMERA_BC_YAW);
	SetPoseParameter(iPose, GetPoseParameter(iPose) + (flDiff / 1.5f));

	if (fabs(flDiff) > 0.1f)
	{
		bMoved = true;
	}

	if (bMoved && (m_flMoveSoundTime < gpGlobals->curtime))
	{
		EmitSound("NPC_CombineCamera.Move");
		m_flMoveSoundTime = gpGlobals->curtime + CAMERA_MOVE_INTERVAL;
	}

	// You're going to make decisions based on this info.  So bump the bone cache after you calculate everything
	InvalidateBoneCache();

	return bMoved;
}

//-----------------------------------------------------------------------------
// Purpose:
// Input  : goal -
//			maxrate -
//			dt -
//			current -
// Output : int
//-----------------------------------------------------------------------------
int CBasePlayerAnimState::ConvergeAngles( float goal,float maxrate, float maxgap, float dt, float& current )
{
    int direction = TURN_NONE;

    float anglediff = goal - current;
    anglediff = AngleNormalize( anglediff );

    float anglediffabs = fabs( anglediff );

    float scale = 1.0f;
    if ( anglediffabs <= FADE_TURN_DEGREES )
    {
        scale = anglediffabs / FADE_TURN_DEGREES;
        // Always do at least a bit of the turn ( 1% )
        scale = clamp( scale, 0.01f, 1.0f );
    }

    float maxmove = maxrate * dt * scale;

    if ( anglediffabs > maxgap )
    {
        // gap is too big, jump
        maxmove = (anglediffabs - maxgap);
    }

    if ( anglediffabs < maxmove )
    {
        // we are close enought, just set the final value
        current = goal;
    }
    else
    {
        // adjust value up or down
        if ( anglediff > 0 )
        {
            current += maxmove;
            direction = TURN_LEFT;
        }
        else
        {
            current -= maxmove;
            direction = TURN_RIGHT;
        }
    }

    current = AngleNormalize( current );

    return direction;
}

//-----------------------------------------------------------------------------
// Purpose: 
//-----------------------------------------------------------------------------
void CHL2MPPlayerAnimState::Update( float eyeYaw, float eyePitch )
{
	// Profile the animation update.
	VPROF( "CHL2MPPlayerAnimState::Update" );

	// Get the HL2MP player.
	CHL2MP_Player *pHL2MPPlayer = GetHL2MPPlayer();
	if ( !pHL2MPPlayer )
		return;

	// Get the studio header for the player.
	CStudioHdr *pStudioHdr = pHL2MPPlayer->GetModelPtr();
	if ( !pStudioHdr )
		return;

	// Check to see if we should be updating the animation state - dead, ragdolled?
	if ( !ShouldUpdateAnimState() )
	{
		ClearAnimationState();
		return;
	}

	// Store the eye angles.
	m_flEyeYaw = AngleNormalize( eyeYaw );
	m_flEyePitch = AngleNormalize( eyePitch );

	// Compute the player sequences.
	ComputeSequences( pStudioHdr );

	if ( SetupPoseParameters( pStudioHdr ) )
	{
		// Pose parameter - what direction are the player's legs running in.
		ComputePoseParam_MoveYaw( pStudioHdr );

		// Pose parameter - Torso aiming (up/down).
		ComputePoseParam_AimPitch( pStudioHdr );

		// Pose parameter - Torso aiming (rotation).
		ComputePoseParam_AimYaw( pStudioHdr );
	}

#ifdef CLIENT_DLL 
	if ( C_BasePlayer::ShouldDrawLocalPlayer() )
	{
		m_pHL2MPPlayer->SetPlaybackRate( 1.0f );
	}
#endif
}