diff --git a/include/omath/projectile_prediction/Engine.hpp b/include/omath/projectile_prediction/Engine.hpp deleted file mode 100644 index 512e27a..0000000 --- a/include/omath/projectile_prediction/Engine.hpp +++ /dev/null @@ -1,33 +0,0 @@ -// -// Created by Vlad on 6/9/2024. -// - -#pragma once - -#include -#include "omath/Vector3.hpp" -#include "omath/projectile_prediction/Projectile.hpp" -#include "omath/projectile_prediction/Target.hpp" - -namespace omath::projectile_prediction -{ - class Engine final - { - public: - explicit Engine(float gravityConstant, float simulationTimeStep, - float maximumSimulationTime, float distanceTolerance); - - [[nodiscard]] - std::optional MaybeCalculateAimPoint(const Projectile& projectile, const Target& target) const; - - private: - const float m_gravityConstant; - const float m_simulationTimeStep; - const float m_maximumSimulationTime; - const float m_distanceTolerance; - - - [[nodiscard]] static std::optional CalculatePitch(const Vector3 &projOrigin, const Vector3 &targetPos, - float bulletGravity, float v0, float time); - }; -} \ No newline at end of file diff --git a/include/omath/projectile_prediction/ProjPredEngine.hpp b/include/omath/projectile_prediction/ProjPredEngine.hpp index ba4bc33..9938db7 100644 --- a/include/omath/projectile_prediction/ProjPredEngine.hpp +++ b/include/omath/projectile_prediction/ProjPredEngine.hpp @@ -2,6 +2,8 @@ // Created by Vlad on 2/23/2025. // #pragma once +#include "Projectile.hpp" +#include "Target.hpp" #include "omath/Vector3.hpp" diff --git a/include/omath/projectile_prediction/ProjPredEngineLegacy.hpp b/include/omath/projectile_prediction/ProjPredEngineLegacy.hpp new file mode 100644 index 0000000..6c9a9e8 --- /dev/null +++ b/include/omath/projectile_prediction/ProjPredEngineLegacy.hpp @@ -0,0 +1,41 @@ +// +// Created by Vlad on 6/9/2024. +// + +#pragma once + +#include +#include "omath/Vector3.hpp" +#include "omath/projectile_prediction/ProjPredEngine.hpp" +#include "omath/projectile_prediction/Projectile.hpp" +#include "omath/projectile_prediction/Target.hpp" + + +namespace omath::projectile_prediction +{ + class ProjPredEngineLegacy final : public ProjPredEngine + { + public: + explicit ProjPredEngineLegacy(float gravityConstant, float simulationTimeStep, float maximumSimulationTime, + float distanceTolerance); + + [[nodiscard]] + std::optional MaybeCalculateAimPoint(const Projectile& projectile, + const Target& target) const override; + + private: + const float m_gravityConstant; + const float m_simulationTimeStep; + const float m_maximumSimulationTime; + const float m_distanceTolerance; + + [[nodiscard]] + std::optional MaybeCalculateProjectileLaunchPitchAngle(const Projectile& projectile, + const Vector3& targetPosition) const; + + + [[nodiscard]] + bool IsProjectileReachedTarget(const Vector3& targetPosition, const Projectile& projectile, float pitch, + float time) const; + }; +} // namespace omath::projectile_prediction diff --git a/source/projectile_prediction/CMakeLists.txt b/source/projectile_prediction/CMakeLists.txt index d93ca75..623aa65 100644 --- a/source/projectile_prediction/CMakeLists.txt +++ b/source/projectile_prediction/CMakeLists.txt @@ -1 +1 @@ -target_sources(omath PRIVATE Engine.cpp Projectile.cpp Target.cpp ProjPredEngineAVX2.cpp ProjPredEngine.cpp) \ No newline at end of file +target_sources(omath PRIVATE ProjPredEngineLegacy.cpp Projectile.cpp Target.cpp ProjPredEngineAVX2.cpp ProjPredEngine.cpp) \ No newline at end of file diff --git a/source/projectile_prediction/Engine.cpp b/source/projectile_prediction/Engine.cpp deleted file mode 100644 index 08616c2..0000000 --- a/source/projectile_prediction/Engine.cpp +++ /dev/null @@ -1,140 +0,0 @@ -#include "omath/projectile_prediction/Engine.hpp" -#include -#include - -namespace omath::projectile_prediction -{ - - Engine::Engine(const float gravityConstant, const float simulationTimeStep, const float maximumSimulationTime, - const float distanceTolerance) : - m_gravityConstant(gravityConstant), m_simulationTimeStep(simulationTimeStep), - m_maximumSimulationTime(maximumSimulationTime), m_distanceTolerance(distanceTolerance) - { - } - - - std::optional Engine::MaybeCalculateAimPoint(const Projectile& projectile, const Target& target) const - { - const float bulletGravity = m_gravityConstant * projectile.m_gravityScale; - const float v0 = projectile.m_launchSpeed; - const float v0Sqr = v0 * v0; - const Vector3 projOrigin = projectile.m_origin; - - constexpr int SIMD_FACTOR = 8; - float currentTime = m_simulationTimeStep; - - for (; currentTime <= m_maximumSimulationTime; currentTime += m_simulationTimeStep * SIMD_FACTOR) - { - const __m256 times = - _mm256_setr_ps(currentTime, currentTime + m_simulationTimeStep, - currentTime + m_simulationTimeStep * 2, currentTime + m_simulationTimeStep * 3, - currentTime + m_simulationTimeStep * 4, currentTime + m_simulationTimeStep * 5, - currentTime + m_simulationTimeStep * 6, currentTime + m_simulationTimeStep * 7); - - const __m256 targetX = - _mm256_fmadd_ps(_mm256_set1_ps(target.m_velocity.x), times, _mm256_set1_ps(target.m_origin.x)); - const __m256 targetY = - _mm256_fmadd_ps(_mm256_set1_ps(target.m_velocity.y), times, _mm256_set1_ps(target.m_origin.y)); - const __m256 timesSq = _mm256_mul_ps(times, times); - const __m256 targetZ = _mm256_fmadd_ps(_mm256_set1_ps(target.m_velocity.z), times, - _mm256_fnmadd_ps(_mm256_set1_ps(0.5f * m_gravityConstant), timesSq, - _mm256_set1_ps(target.m_origin.z))); - - const __m256 deltaX = _mm256_sub_ps(targetX, _mm256_set1_ps(projOrigin.x)); - const __m256 deltaY = _mm256_sub_ps(targetY, _mm256_set1_ps(projOrigin.y)); - const __m256 deltaZ = _mm256_sub_ps(targetZ, _mm256_set1_ps(projOrigin.z)); - - const __m256 dSqr = _mm256_add_ps(_mm256_mul_ps(deltaX, deltaX), _mm256_mul_ps(deltaY, deltaY)); - - const __m256 bgTimesSq = _mm256_mul_ps(_mm256_set1_ps(bulletGravity), timesSq); - const __m256 term = _mm256_add_ps(deltaZ, _mm256_mul_ps(_mm256_set1_ps(0.5f), bgTimesSq)); - const __m256 termSq = _mm256_mul_ps(term, term); - const __m256 numerator = _mm256_add_ps(dSqr, termSq); - const __m256 denominator = _mm256_add_ps(timesSq, _mm256_set1_ps(1e-8f)); // Avoid division by zero - const __m256 requiredV0Sqr = _mm256_div_ps(numerator, denominator); - - const __m256 v0SqrVec = _mm256_set1_ps(v0Sqr + 1e-3f); - const __m256 mask = _mm256_cmp_ps(requiredV0Sqr, v0SqrVec, _CMP_LE_OQ); - - const unsigned validMask = _mm256_movemask_ps(mask); - - if (!validMask) - continue; - - alignas(32) float validTimes[SIMD_FACTOR]; - _mm256_store_ps(validTimes, times); - - for (int i = 0; i < SIMD_FACTOR; ++i) - { - if (!(validMask & (1 << i))) - continue; - - const float candidateTime = validTimes[i]; - - if (candidateTime > m_maximumSimulationTime) - continue; - - // Fine search around candidate time - for (float fineTime = candidateTime - m_simulationTimeStep * 2; - fineTime <= candidateTime + m_simulationTimeStep * 2; fineTime += m_simulationTimeStep) - { - if (fineTime < 0) - continue; - - const Vector3 targetPos = target.PredictPosition(fineTime, m_gravityConstant); - const auto pitch = CalculatePitch(projOrigin, targetPos, bulletGravity, v0, fineTime); - if (!pitch) - continue; - - const Vector3 delta = targetPos - projOrigin; - const float d = std::sqrt(delta.x * delta.x + delta.y * delta.y); - const float height = d * std::tan(angles::DegreesToRadians(*pitch)); - return Vector3(targetPos.x, targetPos.y, projOrigin.z + height); - } - } - } - - // Fallback scalar processing for remaining times - for (; currentTime <= m_maximumSimulationTime; currentTime += m_simulationTimeStep) - { - const Vector3 targetPos = target.PredictPosition(currentTime, m_gravityConstant); - const auto pitch = CalculatePitch(projOrigin, targetPos, bulletGravity, v0, currentTime); - if (!pitch) - continue; - - const Vector3 delta = targetPos - projOrigin; - const float d = std::sqrt(delta.x * delta.x + delta.y * delta.y); - const float height = d * std::tan(angles::DegreesToRadians(*pitch)); - return Vector3(targetPos.x, targetPos.y, projOrigin.z + height); - } - - return std::nullopt; - } - - std::optional Engine::CalculatePitch(const Vector3& projOrigin, const Vector3& targetPos, - const float bulletGravity, const float v0, const float time) - { - if (time <= 0.0f) - return std::nullopt; - - const Vector3 delta = targetPos - projOrigin; - const float dSqr = delta.x * delta.x + delta.y * delta.y; - const float h = delta.z; - - const float term = h + 0.5f * bulletGravity * time * time; - const float requiredV0Sqr = (dSqr + term * term) / (time * time); - const float v0Sqr = v0 * v0; - - if (requiredV0Sqr > v0Sqr + 1e-3f) - return std::nullopt; - - if (dSqr == 0.0f) - { - return term >= 0.0f ? 90.0f : -90.0f; - } - - const float d = std::sqrt(dSqr); - const float tanTheta = term / d; - return angles::RadiansToDegrees(std::atan(tanTheta)); - } -} // namespace omath::prediction diff --git a/source/projectile_prediction/ProjPredEngineLegacy.cpp b/source/projectile_prediction/ProjPredEngineLegacy.cpp new file mode 100644 index 0000000..84c5e22 --- /dev/null +++ b/source/projectile_prediction/ProjPredEngineLegacy.cpp @@ -0,0 +1,68 @@ +#include "omath/projectile_prediction/ProjPredEngineLegacy.hpp" +#include +#include + +namespace omath::projectile_prediction +{ + ProjPredEngineLegacy::ProjPredEngineLegacy(const float gravityConstant, const float simulationTimeStep, + const float maximumSimulationTime, const float distanceTolerance) : + m_gravityConstant(gravityConstant), m_simulationTimeStep(simulationTimeStep), + m_maximumSimulationTime(maximumSimulationTime), m_distanceTolerance(distanceTolerance) + { + } + + std::optional ProjPredEngineLegacy::MaybeCalculateAimPoint(const Projectile& projectile, + const Target& target) const + { + for (float time = 0.f; time < m_maximumSimulationTime; time += m_simulationTimeStep) + { + const auto predictedTargetPosition = target.PredictPosition(time, m_gravityConstant); + + const auto projectilePitch = MaybeCalculateProjectileLaunchPitchAngle(projectile, predictedTargetPosition); + + if (!projectilePitch.has_value()) [[unlikely]] + continue; + + if (!IsProjectileReachedTarget(predictedTargetPosition, projectile, projectilePitch.value(), time)) + continue; + + const auto delta2d = (predictedTargetPosition - projectile.m_origin).Length2D(); + const auto height = delta2d * std::tan(angles::DegreesToRadians(projectilePitch.value())); + + return Vector3(predictedTargetPosition.x, predictedTargetPosition.y, projectile.m_origin.z + height); + } + return std::nullopt; + } + + std::optional + ProjPredEngineLegacy::MaybeCalculateProjectileLaunchPitchAngle(const Projectile& projectile, + const Vector3& targetPosition) const + { + const auto bulletGravity = m_gravityConstant * projectile.m_gravityScale; + const auto delta = targetPosition - projectile.m_origin; + + const auto distance2d = delta.Length2D(); + const auto distance2dSqr = distance2d * distance2d; + const auto launchSpeedSqr = projectile.m_launchSpeed * projectile.m_launchSpeed; + + float root = launchSpeedSqr * launchSpeedSqr - + bulletGravity * (bulletGravity * distance2dSqr + 2.0f * delta.z * launchSpeedSqr); + + if (root < 0.0f) [[unlikely]] + return std::nullopt; + + root = std::sqrt(root); + const float angle = std::atan((launchSpeedSqr - root) / (bulletGravity * distance2d)); + + return angles::RadiansToDegrees(angle); + } + + bool ProjPredEngineLegacy::IsProjectileReachedTarget(const Vector3& targetPosition, const Projectile& projectile, + const float pitch, const float time) const + { + const auto yaw = projectile.m_origin.ViewAngleTo(targetPosition).y; + const auto projectilePosition = projectile.PredictPosition(pitch, yaw, time, m_gravityConstant); + + return projectilePosition.DistTo(targetPosition) <= m_distanceTolerance; + } +} // namespace omath::projectile_prediction diff --git a/tests/general/UnitTestPrediction.cpp b/tests/general/UnitTestPrediction.cpp index 59667bf..c77c438 100644 --- a/tests/general/UnitTestPrediction.cpp +++ b/tests/general/UnitTestPrediction.cpp @@ -1,5 +1,5 @@ #include -#include +#include TEST(UnitTestPrediction, PredictionTest) { @@ -8,7 +8,7 @@ TEST(UnitTestPrediction, PredictionTest) constexpr omath::projectile_prediction::Projectile proj = { .m_origin = {3, 2, 1}, .m_launchSpeed = 5000, .m_gravityScale = 0.4}; const auto viewPoint = - omath::projectile_prediction::Engine(400, 1.f / 1000.f, 50, 5.f).MaybeCalculateAimPoint(proj, target); + omath::projectile_prediction::ProjPredEngineLegacy(400, 1.f / 1000.f, 50, 5.f).MaybeCalculateAimPoint(proj, target); const auto [pitch, yaw, _] = proj.m_origin.ViewAngleTo(viewPoint.value()).AsTuple();