void ParticleFilter::weight(double x, double y, double d){
// Calculate likelihood
double sumWeight = 0.0;
for(int i = 0; i < particleCount; i++){
double dx = (x - particles[i].x) / 20.0;
double dy = (y - particles[i].y) / 20.0;
double dd = dirDiff(d, particles[i].d);
particles[i].w = exp(-(dx*dx + dy*dy + dd*dd)) + 0.00001;
sumWeight += particles[i].w;
}
// Normalize weights
for(int i = 0; i < particleCount; i++){
particles[i].w /= sumWeight;
}
}
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(¶mPos, 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));
}
