//------------------------------------------------------------------------
void CVehicleMovementHelicopter::Reset()
{
	CVehicleMovementBase::Reset();

	m_damageActual = 0.0f;

	ResetActions();

	m_enginePower		= 0.0f;
	m_EnginePerformance = 1.0f;

	m_rpmScale			= 100.0f;
	m_isEnginePowered	= false;

	m_actionPitch		= 0.0f;
	m_actionRoll		= 0.0f;
	m_actionYaw			= 0.0f;

	m_CurrentSpeed		= 0.0f;
	m_CurrentVel		  = ZERO;
  m_steeringDamage  = ZERO;

	m_pNoise = &m_defaultNoise;

  m_pVehicle->GetGameObject()->EnableUpdateSlot(m_pVehicle, IVehicle::eVUS_EnginePowered);

	if(m_HoverAnim)
		m_HoverAnim->Reset();

	SetAnimationSpeed(eVMA_Engine, (m_enginePower / m_enginePowerMax));
	
	m_arcade.Reset(m_pVehicle, m_pEntity);
}
//------------------------------------------------------------------------
void CVehicleMovementHelicopter::StopDriving()
{
	CVehicleMovementBase::StopDriving();
	ResetActions();

	m_engineStartup += m_engineWarmupDelay;
  m_isEnginePowered = false;
}
//------------------------------------------------------------------------
void CVehicleMovementHelicopter::StopEngine()
{
	if(m_enginePower > 0.0f)
	{
		m_isEngineGoingOff = true;
	}

	CVehicleMovementBase::StopEngine();
	ResetActions();

	m_playerControls.Reset();

	m_engineStartup += m_engineWarmupDelay;
}
//------------------------------------------------------------------------
bool CVehicleMovementHelicopter::Init(IVehicle* pVehicle, const CVehicleParams& table)
{
	if (!CVehicleMovementBase::Init(pVehicle, table))
		return false;

	// Initialise the arcade physics, handling helper
	if (CVehicleParams handlingParams = table.findChild("HandlingArcade"))
		if (!m_arcade.Init(pVehicle, handlingParams))
			return false;

	MOVEMENT_VALUE("engineWarmupDelay", m_engineWarmupDelay);
	MOVEMENT_VALUE("enginePowerMax", m_enginePowerMax);
	MOVEMENT_VALUE("yawPerRoll", m_yawPerRoll);
	MOVEMENT_VALUE("maxSpeed", m_maxSpeed);
	MOVEMENT_VALUE("maxPitchAngle", m_maxPitchAngle);
	MOVEMENT_VALUE("maxRollAngle", m_maxRollAngle);
	MOVEMENT_VALUE("rollDamping", m_rollDamping);

	if(table.haveAttr("extendMoveTarget"))
	{
		table.getAttr("extendMoveTarget", m_bExtendMoveTarget);
	}
	table.getAttr("applyNoiseAsVelocity", m_bApplyNoiseAsVelocity);

	if(CVehicleParams noiseParams = table.findChild("MovementNoise"))
	{
		InitMovementNoise(noiseParams, m_defaultNoise);
	}

	// high-level controller
	Ang3 angles				= m_pEntity->GetWorldAngles();
	m_enginePower			= 0.0f;

	if (table.haveAttr("rotorPartName"))
		m_pRotorPart = m_pVehicle->GetPart(table.getAttr("rotorPartName"));

	m_isEngineGoingOff = true;
	m_isEnginePowered = false;

	ResetActions();

	m_pVehicle->GetGameObject()->EnableUpdateSlot(m_pVehicle, IVehicle::eVUS_EnginePowered);

	return true;
}
//------------------------------------------------------------------------
void CVehicleMovementHelicopter::Reset()
{
	CVehicleMovementBase::Reset();

	m_playerControls.Reset();

	m_isEnginePowered = false;

	m_steeringDamage = 0.0f;
	m_damageActual = 0.0f;
	m_turbulence = 0.0f;

	ResetActions();

	m_powerPID.Reset();
	m_liftPID.Reset();
	m_yawPID.Reset();

	// high-level controller
	Ang3 angles = m_pEntity->GetWorldAngles();
	m_desiredDir = angles.z;

	m_desiredHeight = m_pEntity->GetWorldPos().z;
	m_lastDir = m_desiredDir;
	m_enginePower = 0.0f;

	m_isTouchingGround = false;
	m_timeOnTheGround = 50.0f;

	m_desiredPitch = 0.0f;
	m_desiredRoll = 0.0f;

	m_engineForce = 0.0f;

	m_noHoveringTimer = 0.0f;

	m_xyHelp = 0.0f;
	m_liftPitchAngle = 0.0f;
	m_relaxTimer = 0.0f;

	m_velDamp = 0.0f;

	SetAnimationSpeed(eVMA_Engine, (m_enginePower / m_enginePowerMax));
}
void CVehicleMovementHelicopter::ProcessAI(const float deltaTime)
{
	FUNCTION_PROFILER( GetISystem(), PROFILE_GAME );

	CryAutoCriticalSection lk(m_lock);
	SVehiclePhysicsStatus* physStatus = &m_physStatus[k_physicsThread];
		
	if (m_arcade.m_handling.maxSpeedForward>0.f) // Use the new handling code
	{
		//ResetActions();
		m_movementAction.Clear();
		m_movementAction.isAI = true;

		SVehiclePhysicsHelicopterProcessAIParams params;
		params.pPhysStatus = physStatus;
		params.pInputAction = &m_inputAction;
		params.pAiRequest = &m_aiRequest;
		params.dt = deltaTime;
		params.aiRequiredVel = m_CurrentVel;
		params.aiCurrentSpeed = m_CurrentSpeed;
		params.aiYawResponseScalar = m_yawResponseScalar;
		m_yawResponseScalar = 1.f;
	
		// Use helper class to process the AI input
		// It will return a requested velocity, and change the input
		m_arcade.ProcessAI(params);

		// Get the output velocity
		m_CurrentVel = params.aiRequiredVel;
		m_CurrentSpeed = params.aiCurrentSpeed;
		return;
	}
	////////////////////// OLD DEPRECATED CODE :( //////////////////////////////////

	m_movementAction.Clear();
	ResetActions();

	// Our current state
	const Vec3 worldPos =  physStatus->pos;
	const Matrix33 worldMat( physStatus->q);
	const Matrix33 localMat( physStatus->q.GetInverted());
	const Ang3 worldAngles = Ang3::GetAnglesXYZ(worldMat);
	const Ang3 localAngles = Ang3::GetAnglesXYZ(localMat);

	const Vec3 currentVel = physStatus->v;
	const Vec3 currentVel2D(currentVel.x, currentVel.y, 0.0f);
	m_CurrentSpeed = m_CurrentVel.len(); //currentVel.len();
  float currentSpeed2d = currentVel2D.len();

	// +ve direction mean rotation anti-clocwise about the z axis - 0 means along y
	float currentDir = worldAngles.z;

	// to avoid singularity
	const Vec3 vWorldDir = worldMat.GetRow(1);
	const Vec3 vSideWays = worldMat.GetRow(0);
	const Vec3 vWorldDir2D =  Vec3( vWorldDir.x,  vWorldDir.y, 0.0f ).GetNormalizedSafe();

	// Our inputs
	float desiredSpeed = m_aiRequest.HasDesiredSpeed() ? m_aiRequest.GetDesiredSpeed() : 0.0f;
	Limit(desiredSpeed, -m_maxSpeed, m_maxSpeed);

	const Vec3 desiredMoveDir = m_aiRequest.HasMoveTarget() ? (m_aiRequest.GetMoveTarget() - worldPos).GetNormalizedSafe() : vWorldDir;
	Vec3 desiredMoveDir2D = Vec3(desiredMoveDir.x, desiredMoveDir.y, 0.0f);
	desiredMoveDir2D = desiredMoveDir2D.GetNormalizedSafe(desiredMoveDir2D);

	const Vec3 desiredVel = desiredMoveDir * desiredSpeed; 
	const Vec3 desiredVel2D(desiredVel.x, desiredVel.y, 0.0f);

  Vec3 desiredLookDir(desiredMoveDir);

  if (m_aiRequest.HasDesiredBodyDirectionAtTarget())
  {
	    desiredLookDir = m_aiRequest.GetDesiredBodyDirectionAtTarget().GetNormalizedSafe(desiredMoveDir);
  }
  else if (m_aiRequest.HasLookTarget())
  {
    desiredLookDir = (m_aiRequest.GetLookTarget() - worldPos).GetNormalizedSafe(desiredMoveDir);  
  }


	//const Vec3 desiredLookDir = m_aiRequest.HasLookTarget() ? (m_aiRequest.GetLookTarget() - worldPos).GetNormalizedSafe() : desiredMoveDir;
	const Vec3 desiredLookDir2D = Vec3(desiredLookDir.x, desiredLookDir.y, 0.0f).GetNormalizedSafe(vWorldDir2D);

  Vec3 prediction = m_aiRequest.HasBodyTarget() ? m_aiRequest.GetBodyTarget() : ZERO;

  prediction = (prediction.IsEquivalent(ZERO)) ? desiredMoveDir2D : prediction - worldPos;
  prediction.z = 0.0f;

  float speedLimit = prediction.GetLength2D();

	if(speedLimit > 0.0f)
	{
		prediction *= 1.0f / speedLimit;
	}

  Vec3 tempDir = currentVel2D.IsEquivalent(ZERO) ? localMat.GetRow(1) : currentVel2D;
  tempDir.z = 0.0f;
  tempDir.NormalizeFast();

	float dotProd = tempDir.dot(prediction);
	Limit(dotProd, FLT_EPSILON, 1.0f);

	float accel = m_enginePowerMax * min(2.0f,  1.0f / dotProd); // * dotProd;

	if (!m_aiRequest.HasDesiredBodyDirectionAtTarget())
	{
		dotProd *= dotProd;
		dotProd *= dotProd;
		float tempf = min(max(speedLimit * speedLimit, 2.0f), m_maxSpeed * dotProd);
		Limit(desiredSpeed, -tempf, tempf);
	}
	else if (dotProd < 0.0125f)
	{
		Limit(desiredSpeed, -m_maxSpeed * 0.25f, m_maxSpeed * 0.25f);
	}
  
  float posNeg = (float)__fsel(desiredSpeed - m_CurrentSpeed, 1.0f, -5.0f);

  if (desiredVel2D.GetLengthSquared() > FLT_EPSILON)
  {
	  m_CurrentSpeed = m_CurrentSpeed + posNeg * accel * deltaTime;
  }
  else
  {
    m_CurrentSpeed = m_CurrentSpeed + posNeg * accel * deltaTime;
  }

	if (posNeg > 0.0f && m_CurrentSpeed > desiredSpeed)
	{
		m_CurrentSpeed = desiredSpeed;
	}
	else if (posNeg < 0.0f && m_CurrentSpeed < desiredSpeed)
	{
		m_CurrentSpeed = desiredSpeed;
	}

	
	// ---------------------------- Rotation ----------------------------
	
	float desiredDir = (desiredLookDir2D.GetLengthSquared() > 0.0f) ? atan2f(-desiredLookDir2D.x, desiredLookDir2D.y) : atan2f(-vWorldDir2D.x, vWorldDir2D.y);
	while (currentDir < desiredDir - gf_PI)
		currentDir += 2.0f * gf_PI;
	while (currentDir > desiredDir + gf_PI)
		currentDir -= 2.0f * gf_PI;

	// ---------------------------- Yaw ----------------------------

	Ang3 dirDiff(0.0f, 0.0f, desiredDir - currentDir);
	dirDiff.RangePI();
	float absDiff   = fabsf(dirDiff.z);

	float rotSpeed		= (float)__fsel(dirDiff.z, m_yawPerRoll, -m_yawPerRoll);
	m_actionYaw		= m_actionYaw + deltaTime * (rotSpeed - m_actionYaw);

	float temp = fabsf(m_actionYaw);

	float multiplier = ((absDiff / (temp + 0.001f)) + 1.0f) * 0.5f;
	
	m_actionYaw	*= (float)__fsel(absDiff - temp, 1.0f, multiplier);

	// ---------------------------- Yaw ------------------------------

	m_CurrentVel = desiredMoveDir * m_CurrentSpeed;

  // ---------------------------- Pitch ----------------------------

  if (m_CurrentVel.GetLengthSquared2D() > 0.1f)
  {
    CalculatePitch(worldAngles, desiredMoveDir, currentSpeed2d, desiredSpeed, deltaTime);
  }
  else
  {
    Quat rot;
    rot.SetRotationVDir(desiredLookDir, 0.0f);
    float desiredXRot = Ang3::GetAnglesXYZ(rot).x + m_steeringDamage.x;

    m_actionPitch = worldAngles.x + (desiredXRot - worldAngles.x) * deltaTime/* * 10.0f*/;
    Limit(m_actionPitch, -m_maxPitchAngle * 2.0f, m_maxPitchAngle * 2.0f);
  }


	// ---------------------------- Roll ----------------------------
	float rollSpeed = GetRollSpeed();

	rollSpeed *= deltaTime;
	rollSpeed		= (float)__fsel(absDiff - rollSpeed, rollSpeed, absDiff);
	float roll		=(float) __fsel(dirDiff.z, -rollSpeed, rollSpeed);

	float speedPerUnit	   = 1.5f;
	float desiredRollSpeed = absDiff * speedPerUnit * (float)__fsel(dirDiff.z, 1.0f, -1.0f);
  
	desiredRollSpeed = -m_actionYaw * 2.5f;
  desiredRollSpeed += m_steeringDamage.y;

	m_actionRoll = m_actionRoll + deltaTime * (desiredRollSpeed - m_actionRoll);
	Limit(m_actionRoll, -m_maxRollAngle + m_steeringDamage.y, m_maxRollAngle - m_steeringDamage.y);
	m_actionRoll *= m_rollDamping;
	// ---------------------------- Roll ----------------------------


	// ---------------------------- Convert and apply ----------------------------
	Ang3 angles(m_actionPitch, m_actionRoll, worldAngles.z + deltaTime * m_actionYaw);
	
	pe_params_pos paramPos;
	paramPos.q.SetRotationXYZ(angles);
	paramPos.q.Normalize();

	IPhysicalEntity * pPhysicalEntity = GetPhysics();
	pPhysicalEntity->SetParams(&paramPos, 1);

	pe_action_set_velocity vel;
	vel.v = m_CurrentVel + m_netPosAdjust;
	pPhysicalEntity->Action(&vel, 1);

	// ---------------------------- Convert and apply ----------------------------

  m_rpmScale = max(0.2f, cry_fabsf(m_CurrentSpeed / m_maxSpeed));
}
//------------------------------------------------------------------------
bool CVehicleMovementHelicopter::Init(IVehicle *pVehicle, const CVehicleParams &table)
{
	if(!CVehicleMovementBase::Init(pVehicle, table))
		assert(0);

	MOVEMENT_VALUE("engineWarmupDelay", m_engineWarmupDelay);

	// heli abilities
	MOVEMENT_VALUE("altitudeMax", m_altitudeMax);
	MOVEMENT_VALUE("rotorDiskTiltScale", m_rotorDiskTiltScale);
	MOVEMENT_VALUE("pitchResponsiveness", m_pitchResponsiveness);
	MOVEMENT_VALUE("rollResponsiveness", m_rollResponsiveness);
	MOVEMENT_VALUE("yawResponsiveness", m_yawResponsiveness);
	MOVEMENT_VALUE("enginePowerMax", m_enginePowerMax);
	MOVEMENT_VALUE("rotationDamping", m_rotationDamping);
	MOVEMENT_VALUE("yawPerRoll", m_yawPerRoll);

	// high-level controller abilities
	MOVEMENT_VALUE("maxYawRate", m_maxYawRate);
	MOVEMENT_VALUE("maxFwdSpeed", m_maxFwdSpeed);
	MOVEMENT_VALUE("maxLeftSpeed", m_maxLeftSpeed);
	MOVEMENT_VALUE("maxUpSpeed", m_maxUpSpeed);
	MOVEMENT_VALUE("basicSpeedFraction", m_basicSpeedFraction);
	MOVEMENT_VALUE("yawDecreaseWithSpeed", m_yawDecreaseWithSpeed);
	MOVEMENT_VALUE("tiltPerVelDifference", m_tiltPerVelDifference);
	MOVEMENT_VALUE("maxTiltAngle", m_maxTiltAngle);
	MOVEMENT_VALUE("extraRollForTurn", m_extraRollForTurn);
	MOVEMENT_VALUE("rollForTurnForce", m_rollForTurnForce);
	MOVEMENT_VALUE("yawPerRoll", m_yawPerRoll);
	MOVEMENT_VALUE("pitchActionPerTilt", m_pitchActionPerTilt);
	MOVEMENT_VALUE("pitchInputConst", m_pitchInputConst);
	MOVEMENT_VALUE("powerInputConst", m_powerInputConst);
	MOVEMENT_VALUE("powerInputDamping", m_powerInputDamping);
	MOVEMENT_VALUE("relaxForce", m_relaxForce);
	MOVEMENT_VALUE("yawInputConst", m_yawInputConst);
	MOVEMENT_VALUE("yawInputDamping", m_yawInputDamping);
	MOVEMENT_VALUE("maxRollAngle", m_maxRollAngle);
	MOVEMENT_VALUE("velDamping", m_velDamp);

	// Initialise the power PID.
	m_powerPID.Reset();
	m_powerPID.m_kP = 0.2f;
	m_powerPID.m_kD = 0.01f;
	m_powerPID.m_kI = 0.0001f;

	m_maxSpeed = 40.f; // empirically determined

	m_liftPID.Reset();
	m_yawPID.Reset();

	m_liftPID.m_kP = 0.66f;
	m_liftPID.m_kD = 0.2f;
	m_liftPID.m_kI = 0.0f;

	m_yawPID.m_kP = -0.03f;
	m_yawPID.m_kI = 0.0f;
	m_yawPID.m_kD = 0.0f;

	// high-level controller
	Ang3 angles = m_pEntity->GetWorldAngles();
	m_desiredDir = angles.z;

	m_desiredHeight = m_pEntity->GetWorldPos().z;
	m_lastDir = m_desiredDir;
	m_enginePower = 0.0f;

	m_isTouchingGround = false;
	m_timeOnTheGround = 50.0f;

	if(table.haveAttr("rotorPartName"))
		m_pRotorPart = m_pVehicle->GetPart(table.getAttr("rotorPartName"));
	else
		m_pRotorPart = NULL;

	m_desiredPitch = 0.0f;

	ResetActions();

	m_playerDampingBase = 0.1f;
	m_playerDampingRotation = 0.15f;
	m_playerDimLowInput = 0.25f;

	m_playerRotationMult.x = 60.0f;
	m_playerRotationMult.y = 60.0f;
	m_playerRotationMult.z = 10.0f;

	m_strafeForce = 1.0f;

	m_engineUpDir.Set(0.0f, 0.0f, 1.0f);

	m_boostMult = 2.00f;

	ICVar *pSensitivVar = gEnv->pConsole->GetCVar("cl_sensitivity");

	m_playerControls.RegisterValue(&m_liftAction, false, 0.0f, "lift");
	m_playerControls.RegisterAction(eVAI_MoveUp, CHelicopterPlayerControls::eVM_Positive, &m_liftAction);
	m_playerControls.RegisterAction(eVAI_MoveDown, CHelicopterPlayerControls::eVM_Negative, &m_liftAction);

	m_playerControls.RegisterValue(&m_turnAction, false, 0.0f, "roll");
	m_playerControls.RegisterAction(eVAI_TurnLeft, CHelicopterPlayerControls::eVM_Negative, &m_turnAction);
	m_playerControls.RegisterAction(eVAI_TurnRight, CHelicopterPlayerControls::eVM_Positive, &m_turnAction);
	m_playerControls.RegisterAction(eVAI_RotateYaw, CHelicopterPlayerControls::eVM_Positive, &m_turnAction, pSensitivVar);
	m_playerControls.SetActionMult(eVAI_RotateYaw, m_maxYawRate * 0.65f);

	m_playerControls.RegisterValue(&m_desiredRoll, false, gf_PI * 4.0f, "turn");
	m_playerControls.RegisterAction(eVAI_RollLeft, CHelicopterPlayerControls::eVM_Negative, &m_desiredRoll);
	m_playerControls.RegisterAction(eVAI_RollRight, CHelicopterPlayerControls::eVM_Positive, &m_desiredRoll);
	m_playerControls.RegisterAction(eVAI_RotateRoll, CHelicopterPlayerControls::eVM_Positive, &m_desiredRoll, pSensitivVar);

	m_playerControls.RegisterValue(&m_desiredPitch, false, 0.0f, "pitch");
	m_playerControls.RegisterAction(eVAI_RotatePitch, CHelicopterPlayerControls::eVM_Negative, &m_desiredPitch, pSensitivVar);
	m_playerControls.SetActionMult(eVAI_RotatePitch, m_maxYawRate * 0.65f);

	m_pInvertPitchVar = gEnv->pConsole->GetCVar("v_invertPitchControl");
	m_pAltitudeLimitVar = gEnv->pConsole->GetCVar("v_altitudeLimit");
	m_pAltitudeLimitLowerOffsetVar = gEnv->pConsole->GetCVar("v_altitudeLimitLowerOffset");
	m_pStabilizeVTOL = gEnv->pConsole->GetCVar("v_stabilizeVTOL");

	m_turbulenceMultMax = 0.25f;

	return true;
}
//===================================================================
// ProcessAI
// This treats the helicopter as able to move in any horizontal direction
// by tilting in any direction. Yaw control is thus secondary. Throttle
// control is also secondary since it is adjusted to maintain or change
// the height, and the amount needed depends on the tilt.
//===================================================================
//////////////////////////////////////////////////////////////////////////
// NOTE: This function must be thread-safe. Before adding stuff contact MarcoC.
void CVehicleMovementHelicopter::ProcessAI(const float deltaTime)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_GAME);

	// it's useless to progress further if the engine has yet to be turned on
	if(!m_isEnginePowered)
		return;

	m_movementAction.Clear();
	ResetActions();

	// Our current state
	const Vec3 worldPos =  m_PhysPos.pos;
	const Matrix33 worldMat(m_PhysPos.q);
	const Ang3 worldAngles = Ang3::GetAnglesXYZ(worldMat);

	const Vec3 currentVel = m_PhysDyn.v;
	const Vec3 currentVel2D(currentVel.x, currentVel.y, 0.0f);

	m_velDamp = 0.15f;

	// +ve direction mean rotation anti-clocwise about the z axis - 0 means along y
	float currentDir = worldAngles.z;

	// to avoid singularity
	const Vec3 vWorldDir = worldMat * FORWARD_DIRECTION;
	const Vec3 vWorldDir2D =  Vec3(vWorldDir.x,  vWorldDir.y, 0.0f).GetNormalizedSafe();

	// Our inputs
	const float desiredSpeed = m_aiRequest.HasDesiredSpeed() ? m_aiRequest.GetDesiredSpeed() : 0.0f;
	const Vec3 desiredMoveDir = m_aiRequest.HasMoveTarget() ? (m_aiRequest.GetMoveTarget() - worldPos).GetNormalizedSafe() : vWorldDir;
	const Vec3 desiredMoveDir2D = Vec3(desiredMoveDir.x, desiredMoveDir.y, 0.0f).GetNormalizedSafe(vWorldDir2D);
	const Vec3 desiredVel = desiredMoveDir * desiredSpeed;
	const Vec3 desiredVel2D(desiredVel.x, desiredVel.y, 0.0f);
	const Vec3 desiredLookDir = m_aiRequest.HasLookTarget() ? (m_aiRequest.GetLookTarget() - worldPos).GetNormalizedSafe() : desiredMoveDir;
	const Vec3 desiredLookDir2D = Vec3(desiredLookDir.x, desiredLookDir.y, 0.0f).GetNormalizedSafe(vWorldDir2D);

	// Calculate the desired 2D velocity change
	Vec3 desiredVelChange2D = desiredVel2D - currentVel2D;

	float desiredTiltAngle = m_tiltPerVelDifference * desiredVelChange2D.GetLength();

	float powerfactor = desiredSpeed/m_maxSpeed;

	if(powerfactor < 1.0f)
		powerfactor = 1.0f;

	Limit(desiredTiltAngle, -(m_maxTiltAngle*powerfactor), (m_maxTiltAngle*powerfactor));

	if(desiredTiltAngle > 0.0001f)
	{
		const Vec3 desiredWorldTiltAxis = Vec3(-desiredVelChange2D.y, desiredVelChange2D.x, 0.0f).GetNormalizedSafe();
		const Vec3 desiredLocalTiltAxis = worldMat.GetTransposed() * desiredWorldTiltAxis;

		float desiredPitch = desiredTiltAngle * desiredLocalTiltAxis.x;
		float desiredRoll = desiredTiltAngle * desiredLocalTiltAxis.y;

		m_actionPitch += m_pitchActionPerTilt * (desiredPitch - worldAngles.x);
		m_actionRoll += m_pitchActionPerTilt * (desiredRoll - worldAngles.y);
	}

	float desiredDir = atan2f(-desiredLookDir2D.x, desiredLookDir2D.y);

	while(currentDir < desiredDir - gf_PI)
		currentDir += 2.0f * gf_PI;

	while(currentDir > desiredDir + gf_PI)
		currentDir -= 2.0f * gf_PI;

	m_actionYaw = (desiredDir - currentDir) * m_yawInputConst;
	m_actionYaw += m_yawInputDamping * (currentDir - m_lastDir) / deltaTime;
	m_lastDir = currentDir;

	Limit(m_actionPitch, -1.0f, 1.0f);
	Limit(m_actionRoll, -1.0f, 1.0f);
	Limit(m_actionYaw, -1.0f, 1.0f);
	Limit(m_hoveringPower, -1.0f, 1.0f);

	float currentHeight = worldPos.z;

	if(m_aiRequest.HasMoveTarget())
	{
		m_hoveringPower = m_powerPID.Update(currentVel.z, desiredVel.z, -1.0f, 1.0f);
		m_liftAction = 0.0f;
		m_desiredHeight = currentHeight;
	}
	else
	{
		// to keep hovering at the same place
		m_hoveringPower = m_powerPID.Update(currentVel.z, m_desiredHeight - currentHeight, -1.0f, 1.0f);
		m_liftAction = 0.0f;
	}

}
Esempio n. 9
0
void CViewer::Init() 
{
  m_vMeshes = CORE->GetRenderableObjectsManager()->GetMeshes();
  m_vAnimatedModels = CORE->GetRenderableObjectsManager()->GetAnimatedModels();

  sort(m_vMeshes.begin(),m_vMeshes.end(),SortRenderableObjectByName);
  sort(m_vAnimatedModels.begin(),m_vAnimatedModels.end(),SortRenderableObjectByName);

  m_itCurrentMesh = m_vMeshes.begin();
  m_itCurrentAnimated = m_vAnimatedModels.begin();

  m_pCharacter = 0;
  m_fInitialCharacterYaw = 0.0f;

  if(m_vAnimatedModels.size() > 0)
  {
    m_pCharacter = (CRenderableAnimatedInstanceModel*)m_vAnimatedModels[0];
    m_fInitialCharacterYaw = m_pCharacter->GetYaw();
	  //m_pCharacter->GetAnimatedInstanceModel()->ClearCycle(0);
    m_iCurrentAnimation = 0;
	  m_pCharacter->GetAnimatedInstanceModel()->BlendCycle(0,0);
  }

  m_pTargetObject = new CObject3D();
  m_pTargetObject->SetPosition(Vect3f(0.0f,0.0f,0.0f));

  m_pObjectCamera = new CShoulderCamera(
      0.1f,
      100.0f,
      55.0f * FLOAT_PI_VALUE/180.0f,
      ((float)RENDER_MANAGER->GetScreenWidth())/((float)RENDER_MANAGER->GetScreenHeight()),
      m_pTargetObject,
      2.0f,0.6f,1.5f);

  m_pObjectModeLight = 0;
  //m_pSpotLight = 0;

  //m_pObjectModeLight->SetDynamicObjectsOnly(true);

  //m_pSpotLight = CORE->GetLightManager()->CreateSpotLight("FreeModeLight",
  //                                                        Vect3f(-2.15715f,0.0f,-7.32758f),
  //                                                        Vect3f(-5.4188f,0.0f,3.75613f),
  //                                                        CColor(Vect3f(1.0f,1.0f,1.0f)),
  //                                                        20.0f,
  //                                                        80.0f,
  //                                                        10.0f,
  //                                                        45.0f,
  //                                                        false );

  //m_vOmniColor = Vect3f(1.0f,1.0f,1.0f);
  //m_pOmniLight = CORE->GetLightManager()->CreateOmniLight("OmniViewerLight",Vect3f(0.0f),CColor(m_vOmniColor),0.1f,17.0f);

  //CSceneEffectManager* l_pSceneEffectManager = CORE->GetSceneEffectManager();
  //if(l_pSceneEffectManager)
  //{
  //  l_pSceneEffectManager->SetShadowMapLightCast(m_pSpotLight);
  //}

  //m_vAmbientLight = CORE->GetLightManager()->GetAmbientLight();

  m_bEnableLights = true;
  //m_bGuiActive = false;
  m_bShowInfo = true;
  //m_eNormalRendering = NO_NORMALS;
  m_bShowBoxes = false;
  m_bShowSpheres = false;

  //materials
  //m_eLightmapMode  = CMaterial::RADIOSITY_NORMAL;
  m_fGlowIntensity = 0.0f;
  m_fGlowIntensity = 0.0f;
  m_fGlossiness = 0.0f;

  m_iMode = FREE_MODE;
  m_eCurrentMaterialProperty = CViewer::SPECULAR;

  //SOUND_MANAGER->PlayMusic("bgm",true);

  m_vMouseDelta = 0;

  ResetActions();

  InitMode();
}
//////////////////////////////////////////////////////////////////////////
// NOTE: This function must be thread-safe. Before adding stuff contact MarcoC.
void CVehicleMovementVTOL::ProcessAI(const float deltaTime)
{
	FUNCTION_PROFILER( GetISystem(), PROFILE_GAME );
	
	if (!m_isVTOLMovement)
	{
		CVehicleMovementHelicopter::ProcessAI(deltaTime);
		return;
	}

	m_velDamp = 0.15f;

	const float maxDirChange = 15.0f;

	// it's useless to progress further if the engine has yet to be turned on
	if (!m_isEnginePowered)
		return;

	m_movementAction.Clear();
	m_movementAction.isAI = true;
	ResetActions();

	// Our current state
	const Vec3 worldPos =  m_PhysPos.pos;
	const Matrix33 worldMat( m_PhysPos.q);
	Ang3 worldAngles = Ang3::GetAnglesXYZ(worldMat);

	const Vec3 currentVel = m_PhysDyn.v;
	const Vec3 currentVel2D(currentVel.x, currentVel.y, 0.0f);
	// +ve direction mean rotation anti-clocwise about the z axis - 0 means along y
	float currentDir = worldAngles.z;

	// to avoid singularity
	const Vec3 vWorldDir = worldMat * FORWARD_DIRECTION;
	const Vec3 vWorldDir2D =  Vec3( vWorldDir.x,  vWorldDir.y, 0.0f ).GetNormalizedSafe();

	// Our inputs
	const float desiredSpeed = m_aiRequest.HasDesiredSpeed() ? m_aiRequest.GetDesiredSpeed() : 0.0f;
	const Vec3 desiredMoveDir = m_aiRequest.HasMoveTarget() ? (m_aiRequest.GetMoveTarget() - worldPos).GetNormalizedSafe() : vWorldDir;
	const Vec3 desiredMoveDir2D = Vec3(desiredMoveDir.x, desiredMoveDir.y, 0.0f).GetNormalizedSafe(vWorldDir2D);
	const Vec3 desiredVel = desiredMoveDir * desiredSpeed; 
	const Vec3 desiredVel2D(desiredVel.x, desiredVel.y, 0.0f);
	const Vec3 desiredLookDir = m_aiRequest.HasLookTarget() ? (m_aiRequest.GetLookTarget() - worldPos).GetNormalizedSafe() : desiredMoveDir;
	const Vec3 desiredLookDir2D = Vec3(desiredLookDir.x, desiredLookDir.y, 0.0f).GetNormalizedSafe(vWorldDir2D);

	// Calculate the desired 2D velocity change
	Vec3 desiredVelChange2D = desiredVel2D - currentVel2D;
	float velChangeLength = desiredVelChange2D.GetLength2D();

	bool isLandingMode = false;
	if (m_pLandingGears && m_pLandingGears->AreLandingGearsOpen())
		isLandingMode = true;

	bool isHorizontal = (desiredSpeed >= 5.0f) && (desiredMoveDir.GetLength2D() > desiredMoveDir.z);
	float desiredPitch = 0.0f;
	float desiredRoll = 0.0f;

	float desiredDir = atan2f(-desiredLookDir2D.x, desiredLookDir2D.y);

	while (currentDir < desiredDir - gf_PI)
		currentDir += 2.0f * gf_PI;
	while (currentDir > desiredDir + gf_PI)
		currentDir -= 2.0f * gf_PI;

	float diffDir = (desiredDir - currentDir);
	m_actionYaw = diffDir * m_yawInputConst;
	m_actionYaw += m_yawInputDamping * (currentDir - m_lastDir) / deltaTime;
	m_lastDir = currentDir;

	if (isHorizontal && !isLandingMode)
	{
		float desiredFwdSpeed = desiredVelChange2D.GetLength();

		desiredFwdSpeed *= min(1.0f, diffDir / DEG2RAD(maxDirChange));

		if (!iszero(desiredFwdSpeed))
		{
			const Vec3 desiredWorldTiltAxis = Vec3(-desiredVelChange2D.y, desiredVelChange2D.x, 0.0f);
			const Vec3 desiredLocalTiltAxis = worldMat.GetTransposed() * desiredWorldTiltAxis;

			m_forwardAction = m_fwdPID.Update(currentVel.y, desiredLocalTiltAxis.GetLength(), -1.0f, 1.0f);

		float desiredTiltAngle = m_tiltPerVelDifference * desiredVelChange2D.GetLength();
		Limit(desiredTiltAngle, -m_maxTiltAngle, m_maxTiltAngle);

		if (desiredTiltAngle > 0.0001f)
		{
			const Vec3 desiredWorldTiltAxis2 = Vec3(-desiredVelChange2D.y, desiredVelChange2D.x, 0.0f).GetNormalizedSafe();
			const Vec3 desiredLocalTiltAxis2 = worldMat.GetTransposed() * desiredWorldTiltAxis2;

			Vec3 vVelLocal = worldMat.GetTransposed() * desiredVel;
			vVelLocal.NormalizeSafe();

			float dotup = vVelLocal.Dot(Vec3( 0.0f,0.0f,1.0f ) );
			float currentSpeed = currentVel.GetLength();

			desiredPitch = dotup *currentSpeed / 100.0f;
			desiredRoll = desiredTiltAngle * desiredLocalTiltAxis2.y *currentSpeed/30.0f;
		}

		}
	}
	else
	{
		float desiredTiltAngle = m_tiltPerVelDifference * desiredVelChange2D.GetLength();
		Limit(desiredTiltAngle, -m_maxTiltAngle, m_maxTiltAngle);

		if (desiredTiltAngle > 0.0001f)
		{
			const Vec3 desiredWorldTiltAxis = Vec3(-desiredVelChange2D.y, desiredVelChange2D.x, 0.0f).GetNormalizedSafe();
			const Vec3 desiredLocalTiltAxis = worldMat.GetTransposed() * desiredWorldTiltAxis;

			desiredPitch = desiredTiltAngle * desiredLocalTiltAxis.x;
			desiredRoll = desiredTiltAngle * desiredLocalTiltAxis.y;
		}
	}

	float currentHeight = m_PhysPos.pos.z;
	if ( m_aiRequest.HasMoveTarget() )
	{
		m_hoveringPower = m_powerPID.Update(currentVel.z, desiredVel.z, -1.0f, 4.0f);
		
		//m_hoveringPower = (m_desiredHeight - currentHeight) * m_powerInputConst;
		//m_hoveringPower += m_powerInputDamping * (currentHeight - m_lastHeight) / deltaTime;

		if (isHorizontal)
		{
			if (desiredMoveDir.z > 0.6f || desiredMoveDir.z < -0.85f)
			{
				desiredPitch = max(-0.5f, min(0.5f, desiredMoveDir.z)) * DEG2RAD(35.0f);
				m_forwardAction += abs(desiredMoveDir.z);
			}

			m_liftAction = min(2.0f, max(m_liftAction, m_hoveringPower * 2.0f));
		}
		else
		{
			m_liftAction = 0.0f;
		}
	}
	else
	{
		// to keep hovering at the same place
		m_hoveringPower = m_powerPID.Update(currentVel.z, m_desiredHeight - currentHeight, -1.0f, 1.0f);
		m_liftAction = 0.0f;

		if (m_pVehicle->GetAltitude() > 10.0f)	//TODO: this line is not MTSafe
			m_liftAction = m_forwardAction;
	}

	m_actionPitch += m_pitchActionPerTilt * (desiredPitch - worldAngles.x);
	m_actionRoll += m_pitchActionPerTilt * (desiredRoll - worldAngles.y);

	Limit(m_actionPitch, -1.0f, 1.0f);
	Limit(m_actionRoll, -1.0f, 1.0f);
	Limit(m_actionYaw, -1.0f, 1.0f);

	if (m_horizontal > 0.0001f)
		m_desiredHeight = m_PhysPos.pos.z;
	
	Limit(m_forwardAction, -1.0f, 1.0f);
}