a.cpp

// crssnky


#include "MyBlueprintFunctionLibrary.h"
#include <cmath>

float UMyBlueprintFunctionLibrary::GetEarthRadius()noexcept {
	return m_earthRadius;
}

FVector UMyBlueprintFunctionLibrary::LLA2XYZ(const FVector& LLA) noexcept
{
	const auto cosLat = std::cos(LLA.X * PI / 180.0);
	const auto sinLat = std::sin(LLA.X * PI / 180.0);
	const auto cosLon = std::cos(LLA.Y * PI / 180.0);
	const auto sinLon = std::sin(LLA.Y * PI / 180.0);

	return {
		static_cast<float>((m_earthRadius + LLA.Z) * cosLat * cosLon),
		static_cast<float>((m_earthRadius + LLA.Z) * cosLat * sinLon),
		static_cast<float>((m_earthRadius + LLA.Z) * sinLat)
	};
}

void UMyBlueprintFunctionLibrary::calc_destination(
	const FGeographicCoordinates& src, const float direction, const float distance, FGeographicCoordinates& dst, float& backDirection)
{
	const double distance_m = distance * 1000.;
	const double b = kA * (1. - kF);
	const double phi_1 = FMath::DegreesToRadians(src.Latitude);
	const double l_1 = FMath::DegreesToRadians(src.Longitude);
	const double tan_u_1 = (1. - kF) * std::tan(phi_1);
	const double u_1 = std::atan(tan_u_1);

	const double alp_1 = FMath::DegreesToRadians(static_cast<double>(-direction));
	const double cos_alp_1 = std::cos(alp_1);
	const double sin_alp_1 = std::sin(alp_1);
	const double sgm_1 = std::atan2(tan_u_1, cos_alp_1);
	const double sin_alp = std::cos(u_1) * std::sin(alp_1);
	const double sin2_alp = sin_alp * sin_alp;
	const double cos2_alp = 1. - sin2_alp;
	const double u2 = cos2_alp * (kA * kA - b * b) / (b * b);
	const double aa = calc_a(u2);
	const double bb = calc_b(u2);
	double sgm = distance_m / (b * aa);
	double sgm_prev = kPi2;
	double cos_sgm = 0.;
	double sin_sgm = 0.;
	double cos_2_sgm_m = 0.;

	while (std::abs(sgm - sgm_prev) > kEps) {
		cos_sgm = std::cos(sgm);
		sin_sgm = std::sin(sgm);
		cos_2_sgm_m = std::cos(2. * sgm_1 + sgm);
		const double d_sgm = calc_dlt_sgm(bb, cos_sgm, sin_sgm, cos_2_sgm_m);
		sgm_prev = sgm;
		sgm = distance_m / (b * aa) + d_sgm;
	}

	const double cos_u_1 = std::cos(u_1);
	const double sin_u_1 = std::sin(u_1);
	const double cu1_cs = cos_u_1 * cos_sgm;
	const double cu1_ss = cos_u_1 * sin_sgm;
	const double su1_cs = sin_u_1 * cos_sgm;
	const double su1_ss = sin_u_1 * sin_sgm;
	const double tmp = su1_ss - cu1_cs * cos_alp_1;
	const double phi_2 = std::atan2(
		su1_cs + cu1_ss * cos_alp_1,
		(1. - kF) * std::sqrt(sin_alp * sin_alp + tmp * tmp)
	);
	const double lmd = std::atan2(sin_sgm * sin_alp_1, cu1_cs - su1_ss * cos_alp_1);
	const double cc = calc_c(cos2_alp);
	const double l = lmd - (1. - cc) * kF * sin_alp
		* (sgm + cc * sin_sgm
			* (cos_2_sgm_m + cc * cos_sgm
				* (-1. + 2. * cos_2_sgm_m * cos_2_sgm_m)));
	const double l_2 = l + l_1;
	const double alp_2 = std::atan2(sin_alp, -su1_ss + cu1_cs * cos_alp_1) + kPi;
	dst.Latitude = FMath::RadiansToDegrees(phi_2);
	dst.Longitude = FMath::RadiansToDegrees(norm_long(l_2));
	dst.Altitude = src.Altitude;
	backDirection = FMath::RadiansToDegrees(norm_az(alp_2));
}

FGeographicCoordinates UMyBlueprintFunctionLibrary::CenterCrd(const FGeographicCoordinates& A, const FGeographicCoordinates& B, const float alt) noexcept
{
	auto a= FGeographicCoordinates((A.Latitude + B.Latitude) / 2., (A.Longitude + B.Longitude) / 2., alt * 1000.);
	return a;
}

FVector UMyBlueprintFunctionLibrary::LargeCoordinateToVector(const FCartesianCoordinates& from, const float scale) noexcept
{
	return FVector(from.X * scale, from.Y * scale, from.Z * scale);
}

const FTimeBins& UMyBlueprintFunctionLibrary::FindMissileDataByYear(const TArray<FTimeBins>& from, const int32 year) noexcept
{
	const auto data = from.FindByPredicate([&](const FTimeBins& x) {
		return x.year == year;
		});
	if (data) {
		return *data;
	}
	static const FTimeBins ret = {};
	return ret;
}

const FMissile& UMyBlueprintFunctionLibrary::FindMissileData(const TArray<FMissile>& from, const FString key) noexcept
{
	const auto data = from.FindByPredicate([&](const FMissile& x) {
		return x.key == key;
		});
	if (data) {
		return *data;
	}
	static const FMissile ret;
	return ret;
}

double UMyBlueprintFunctionLibrary::calc_a(const double u2)
{
	return 1. + u2 / 16384. * (4096. + u2 * (-768. + u2 * (320. - 175. * u2)));
}

double UMyBlueprintFunctionLibrary::calc_b(const double u2)
{
	return u2 / 1024. * (256. + u2 * (-128. + u2 * (74. - 47. * u2)));
}

double UMyBlueprintFunctionLibrary::calc_c(const double cos2_alp)
{
	return kF * cos2_alp * (4. + kF * (4. - 3. * cos2_alp)) / 16;
}

double UMyBlueprintFunctionLibrary::calc_dlt_sgm(const double bb, const double cos_sgm, const double sin_sgm, const double cos_2_sgm_m)
{
	return bb * sin_sgm * (cos_2_sgm_m
		+ bb / 4. * (cos_sgm * (-1. + 2. * cos_2_sgm_m * cos_2_sgm_m)
			- bb / 6. * cos_2_sgm_m * (-3. + 4. * sin_sgm * sin_sgm)
			* (-3. + 4. * cos_2_sgm_m * cos_2_sgm_m)));
}

double UMyBlueprintFunctionLibrary::norm_long(double lng)
{
	while (lng < -kPi)lng += kPi2;
	while (lng > kPi)lng -= kPi2;
	return lng;
}

double UMyBlueprintFunctionLibrary::norm_az(double alp)
{
	if (alp < 0.)alp += kPi2;
	if (alp > kPi2)alp -= kPi2;
	return alp;
}

#if WITH_EDITOR
IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMyBFLTest, "MyTest.calc_destination", EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter)
bool FMyBFLTest::RunTest(const FString&) {
	{
		const FGeographicCoordinates src(36.1037747777777778, 140.087855027777778, 0);
		const FGeographicCoordinates ans(35.6550284722222222, 139.744750444444444, 0);
		const float direction = 211.992527777777778, distance = 58643.804;
		FGeographicCoordinates dst;
		float backDirection;
		UMyBlueprintFunctionLibrary::calc_destination(src, direction, distance, dst, backDirection);
		TestTrue(TEXT("GIAJ - Latitude"), FMath::IsNearlyEqual(dst.Latitude, ans.Latitude, 0.00001));
		TestTrue(TEXT("GIAJ - Longitude"), FMath::IsNearlyEqual(dst.Longitude, ans.Longitude, 0.00001));
		TestTrue(TEXT("GIAJ - Altitude"), FMath::IsNearlyEqual(dst.Altitude, ans.Altitude, 0.00001));
	}
	{
		const FGeographicCoordinates src(35.4681, 133.0486, 0);
		const FGeographicCoordinates ans(35.47222200, 133.05055600, 0);
		const float direction = 21.21518366, distance = 490.58216516;
		FGeographicCoordinates dst;
		float backDirection;
		UMyBlueprintFunctionLibrary::calc_destination(src, direction, distance, dst, backDirection);
		TestTrue(TEXT("mk-mode - Latitude"), FMath::IsNearlyEqual(dst.Latitude, ans.Latitude, 0.00001));
		TestTrue(TEXT("mk-mode - Longitude"), FMath::IsNearlyEqual(dst.Longitude, ans.Longitude, 0.00001));
		TestTrue(TEXT("mk-mode - Altitude"), FMath::IsNearlyEqual(dst.Altitude, ans.Altitude, 0.00001));
	}

	return true;
}
#endif

a.hpp

// crssnky

#pragma once

#include "CoreMinimal.h"
#include "Kismet/BlueprintFunctionLibrary.h"
#include "CartesianCoordinates.h"
#include "GeographicCoordinates.h"
#include "Misc/AutomationTest.h"
#include "MissileTestJsonStructs.h"
#include "MissileJsonStructs.h"
#include "MyBlueprintFunctionLibrary.generated.h"

/**
 *
 */
UCLASS()
class UMyBlueprintFunctionLibrary : public UBlueprintFunctionLibrary
{
	GENERATED_BODY()
public:
	UFUNCTION(BlueprintPure, Category = "MyFunc|BFL")
		static float GetEarthRadius()noexcept;
	UFUNCTION(BlueprintPure, Category = "MyFunc|BFL")
		static FVector LLA2XYZ(const FVector& LLA)noexcept;
	UFUNCTION(BlueprintPure, Category = "MyFunc|BFL")
		static void calc_destination(const FGeographicCoordinates& src, const float direction, const float distance, FGeographicCoordinates& dst, float& backDirection);
	UFUNCTION(BlueprintPure, Category = "MyFunc|BFL")
		static FGeographicCoordinates CenterCrd(const FGeographicCoordinates& A, const FGeographicCoordinates& B, const float alt)noexcept;
	UFUNCTION(BlueprintPure, Category = "MyFunc|BFL")
		static FVector LargeCoordinateToVector(const FCartesianCoordinates& from, const float scale)noexcept;
	UFUNCTION(BlueprintPure, Category = "MyFunc|BFL")
		static const FTimeBins& FindMissileDataByYear(const TArray<FTimeBins>& from, const int32 year)noexcept;
	UFUNCTION(BlueprintPure, Category = "MyFunc|BFL")
		static const FMissile& FindMissileData(const TArray<FMissile>& from, const FString key)noexcept;


private:
	static double calc_a(const double);
	static double calc_b(const double);
	static double calc_c(const double);
	static double calc_dlt_sgm(const double, const double, const double, const double);
	static double norm_long(const double);
	static double norm_az(const double);

	static constexpr double m_earthRadius = 3975000.0 * 160.0;
	static constexpr double kA = 6378137.;
	static constexpr double kF = 1. / 298.257222101;
	static constexpr double kPi = 3.14159;
	static constexpr double kPi2 = kPi * 2.;
	static constexpr double kPi180 = kPi / 180.;
	static constexpr double kPi180Inv = 1. / kPi180;
	static constexpr double kEps = 1.e-11;
};