//===================================================================
// 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;
}
}
//////////////////////////////////////////////////////////////////////////
// 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);
}
