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