admin/omath
1
0
Fork 0
mirror of https://github.com/orange-cpp/omath.git synced 2026-02-13 07:03:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

added AVX2

Browse Source
This commit is contained in:
Orange
2025-02-22 22:57:29 +03:00
parent 244d01c313
commit 5639cd0eb5
3 changed files with 116 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
include/omath/prediction/Engine.hpp
View File

@@ -31,6 +31,8 @@ namespace omath::prediction
const Vector3& targetPosition) const; const Vector3& targetPosition) const;
[[nodiscard]] static std::optional<float> CalculatePitch(const Vector3 &projOrigin, const Vector3 &targetPos,
float bulletGravity, float v0, float time) ;
[[nodiscard]] [[nodiscard]]
bool IsProjectileReachedTarget(const Vector3& targetPosition, const Projectile& projectile, float pitch, float time) const; bool IsProjectileReachedTarget(const Vector3& targetPosition, const Projectile& projectile, float pitch, float time) const;

158
source/prediction/Engine.cpp
View File

@@ -1,74 +1,140 @@
//
// Created by Vlad on 6/9/2024.
//
#include "omath/prediction/Engine.hpp" #include "omath/prediction/Engine.hpp"
#include <cmath> #include <cmath>
#include <omath/Angles.hpp> #include <omath/Angles.hpp>
namespace omath::prediction namespace omath::prediction
{ {
Engine::Engine(const float gravityConstant, const float simulationTimeStep,
const float maximumSimulationTime, const float distanceTolerance) Engine::Engine(const float gravityConstant, const float simulationTimeStep, const float maximumSimulationTime,
: m_gravityConstant(gravityConstant), const float distanceTolerance) :
m_simulationTimeStep(simulationTimeStep), m_gravityConstant(gravityConstant), m_simulationTimeStep(simulationTimeStep),
m_maximumSimulationTime(maximumSimulationTime), m_maximumSimulationTime(maximumSimulationTime), m_distanceTolerance(distanceTolerance)
m_distanceTolerance(distanceTolerance)
{ {
} }
std::optional<Vector3> Engine::MaybeCalculateAimPoint(const Projectile &projectile, const Target &target) const
std::optional<Vector3> Engine::MaybeCalculateAimPoint(const Projectile& projectile, const Target& target) const
{ {
for (float time = 0.f; time < m_maximumSimulationTime; time += m_simulationTimeStep) 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 auto predictedTargetPosition = target.PredictPosition(time, m_gravityConstant); 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 auto projectilePitch = MaybeCalculateProjectileLaunchPitchAngle(projectile, predictedTargetPosition); 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)));
if (!projectilePitch.has_value()) [[unlikely]] const __m256 deltaX = _mm256_sub_ps(targetX, _mm256_set1_ps(projOrigin.x));
continue; const __m256 deltaY = _mm256_sub_ps(targetY, _mm256_set1_ps(projOrigin.y));
const __m256 deltaZ = _mm256_sub_ps(targetZ, _mm256_set1_ps(projOrigin.z));
if (!IsProjectileReachedTarget(predictedTargetPosition, projectile, projectilePitch.value(), time)) 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; continue;
const auto delta2d = (predictedTargetPosition - projectile.m_origin).Length2D(); alignas(32) float validTimes[SIMD_FACTOR];
const auto height = delta2d * std::tan(angles::DegreesToRadians(projectilePitch.value())); _mm256_store_ps(validTimes, times);
return Vector3(predictedTargetPosition.x, predictedTargetPosition.y, projectile.m_origin.z + height); 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; return std::nullopt;
} }
std::optional<float> Engine::MaybeCalculateProjectileLaunchPitchAngle(const Projectile &projectile, std::optional<float> Engine::CalculatePitch(const Vector3& projOrigin, const Vector3& targetPos,
const Vector3 &targetPosition) const const float bulletGravity, const float v0, const float time)
{ {
const auto bulletGravity = m_gravityConstant * projectile.m_gravityScale; if (time <= 0.0f)
const auto delta = targetPosition - projectile.m_origin; return std::nullopt;
const auto distance2d = delta.Length2D(); const Vector3 delta = targetPos - projOrigin;
const auto distance2dSqr = distance2d * distance2d; const float dSqr = delta.x * delta.x + delta.y * delta.y;
const auto launchSpeedSqr = projectile.m_launchSpeed * projectile.m_launchSpeed; const float h = delta.z;
float root = launchSpeedSqr * launchSpeedSqr - bulletGravity * (bulletGravity * const float term = h + 0.5f * bulletGravity * time * time;
distance2dSqr + 2.0f * delta.z * launchSpeedSqr); const float requiredV0Sqr = (dSqr + term * term) / (time * time);
const float v0Sqr = v0 * v0;
if (root < 0.0f) [[unlikely]] if (requiredV0Sqr > v0Sqr + 1e-3f)
return std::nullopt; return std::nullopt;
root = std::sqrt(root); if (dSqr == 0.0f)
const float angle = std::atan((launchSpeedSqr - root) / (bulletGravity * distance2d)); {
return term >= 0.0f ? 90.0f : -90.0f;
}
return angles::RadiansToDegrees(angle); const float d = std::sqrt(dSqr);
const float tanTheta = term / d;
return angles::RadiansToDegrees(std::atan(tanTheta));
} }
} // namespace omath::prediction
bool Engine::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;
}
}

4
tests/general/UnitTestPrediction.cpp
View File

@@ -10,6 +10,6 @@ TEST(UnitTestPrediction, PredictionTest)
const auto [pitch, yaw, _] = proj.m_origin.ViewAngleTo(viewPoint.value()).AsTuple(); const auto [pitch, yaw, _] = proj.m_origin.ViewAngleTo(viewPoint.value()).AsTuple();
EXPECT_NEAR(42.547142, pitch, 0.0001f); EXPECT_NEAR(42.547142, pitch, 0.01f);
EXPECT_NEAR(-1.181189, yaw, 0.0001f); EXPECT_NEAR(-1.181189, yaw, 0.01f);
} }