Ivazovsky/plugins/approximatedSpline/Source/approximatedSpline/Private/MySplineComponent.cpp

// Fill out your copyright notice in the Description page of Project Settings.


#include "MySplineComponent.h"
#include "kismet/kismetmathlibrary.h"
#include "drawdebughelpers.h"





void UApproximatedSplineComponent::approximate(int precision, double optimizeTolerance)
{
	if (precision < 1) {
		UE_LOG(LogActor, Warning, TEXT("%s: precision parameter is less 1, override to 1"), *GetOwner()->GetActorNameOrLabel());
		precision = 1;
	};
	if (optimizeTolerance < 0.001) {
		UE_LOG(LogActor, Warning, TEXT("%s: optimizeTolerance parameter is less 0.001, override to 0.001"), *GetOwner()->GetActorNameOrLabel());
		optimizeTolerance = 0.001;
	};

	double temp = 0;
	map.Empty();
	TArray<double> dist;
	TArray<FVector> loc;
	TArray<FTransform> tra;
	auto lastI = UKismetMathLibrary::FTrunc(GetSplineLength() / float(precision)) ;
	int delta = 0;
	for (int i = 0; i <= lastI; i++)
	{
		auto v1 = GetTransformAtDistanceAlongSpline(i * precision, ESplineCoordinateSpace::Local);
		auto v2 = GetTransformAtDistanceAlongSpline((i + 1) * precision, ESplineCoordinateSpace::Local);
		temp += FVector::Distance(v1.GetLocation(), v2.GetLocation());
		if (i == 0) {
			dist.Add(0);
			loc.Add(v1.GetLocation());
			tra.Add(v1);

		}

		dist.Add(temp);
		loc.Add(v2.GetLocation());
		tra.Add(v2);

		/*if (i == lastI) {
			dist.Add(GetSplineLength());
			loc.Add(GetLocationAtSplinePoint(GetNumberOfSplinePoints() - 1, ESplineCoordinateSpace::Local));
		}*/ //add last point

		if ((i > 1) && (optimizeTolerance > 0) && loc.IsValidIndex(i + delta)) {
			if (UKismetMathLibrary::NearlyEqual_FloatFloat((loc[i + delta] - loc[i - 2 + delta]).Length(), (loc[i - 1 + delta] - loc[i - 2 + delta]).Length() + (loc[i + delta] - loc[i - 1 + delta]).Length(), optimizeTolerance * pow(float(precision) / 100, 2) / ((loc[i - 1 + delta] - loc[i - 2 + delta]).Length() / 100))) {

				dist.RemoveAt(i - 1 + delta);
				loc.RemoveAt(i - 1 + delta);
				tra.RemoveAt(i - 1 + delta);
				delta--;
			}
		}
	}
	int ii = 0;
	auto offsetMultiplier = GetSplineLength() / dist.Last(); //approximated len is a little shorter then native spline len, so make every dis longer to fit
	for (auto dis : dist) {
		map.Add(dis* offsetMultiplier, tra[ii]);
		ii++;
	}

	numOfApproximatedPoints = map.Num();
	//UE_LOG(LogActorComponent, Warning, TEXT("%s: distance: %f, approDistance:%f"), *GetOwner()->GetActorNameOrLabel(),GetSplineLength(), dist.Last()*offsetMultiplier);

}

void UApproximatedSplineComponent::drawApproximatedSplineDebug(float pointSize, FLinearColor lineColor, float debugDuration, float thickness)
{
	TArray<FTransform> arr;
	map.GenerateValueArray(arr);
	
	for (int i = 1; i < arr.Num(); i++) {
		
		DrawDebugDirectionalArrow(GetWorld(), UKismetMathLibrary::TransformLocation(GetComponentTransform(), (arr[i - 1].GetLocation())), UKismetMathLibrary::TransformLocation(GetComponentTransform(),  arr[i].GetLocation()), pointSize, lineColor.ToFColor(false), false, debugDuration, 0U, thickness);
	}

}



FVector UApproximatedSplineComponent::getRealLocationByDistance(double distance, ESplineCoordinateSpace::Type space)
{
	return getRealTransformByDistance(distance, space).GetLocation();
}

FRotator UApproximatedSplineComponent::getRealRotationByDistance(double distance, ESplineCoordinateSpace::Type space)
{
	return getRealTransformByDistance(distance, space).Rotator();
}

FTransform UApproximatedSplineComponent::getRealTransformByDistance(double distance, ESplineCoordinateSpace::Type space)
{
	TArray<double> keys;
	map.GetKeys(keys);
	distance = UKismetMathLibrary::FClamp(distance, 0, keys.Last());
	int32 ii = 0;
	for (auto& key : keys) {

		auto prevkey = keys[ii > 0 ? ii - 1 : ii];
		if (key >= distance) {
			return UKismetMathLibrary::TLerp(
				space == ESplineCoordinateSpace::Local ? *map.Find(key) :
				FTransform(UKismetMathLibrary::TransformRotation(
					GetComponentTransform(),
					map.Find(key)->Rotator()
				),
					UKismetMathLibrary::TransformLocation(GetComponentTransform(), map.Find(key)->GetLocation()),
					FVector(1)
				),
				space == ESplineCoordinateSpace::Local ? *map.Find(prevkey) :
				FTransform(
					UKismetMathLibrary::TransformRotation(
						GetComponentTransform(),
						map.Find(prevkey)->Rotator()
					)
					,UKismetMathLibrary::TransformLocation(
					GetComponentTransform(), 
					map.Find(prevkey)->GetLocation()
					),
					FVector(1)
				),
				UKismetMathLibrary::MapRangeClamped(distance, key, prevkey, 0, 1)
				
			);
		}
		ii++;
	}
	return FTransform();
}


/* old and more expensive approximation optimization

void UMySplineComponent::approximate(int precision = 100, double optimizeTolerance = 0.1, ESplineCoordinateSpace::Type space = ESplineCoordinateSpace::Local)
{
	if (precision < 1) {
		UE_LOG(LogTemp, Warning, TEXT("precision parameter is less 1, override to 1"));
		precision = 1;
	};
	double temp = 0;
	map.Empty();
	auto lastI = UKismetMathLibrary::FTrunc(GetSplineLength() / float(precision)) - 1;
	for (int i = 0; i <= lastI; i++)
	{
		auto v1 = GetLocationAtDistanceAlongSpline(i * precision, space);
		auto v2 = GetLocationAtDistanceAlongSpline((i + 1) * precision, space);
		temp += FVector::Distance(v1, v2);
		if (i == 0) map.Add(0, v1); //add first point
		map.Add(temp, v2);
		if (i == lastI)	map.Add(GetSplineLength(), GetLocationAtSplinePoint(GetNumberOfSplinePoints() - 1, space)); //add last point
	}
	UE_LOG(LogTemp, Display, TEXT("len is %d"), map.Num());
	auto farr = map.Array();

	if (optimizeTolerance > 0) {
		for (int32 i = 0; i < farr.Num() - 2; i++) {
			if (UKismetMathLibrary::NearlyEqual_FloatFloat((farr[i + 2].Value - farr[i].Value).Length(), (farr[i + 1].Value - farr[i].Value).Length() + (farr[i + 2].Value - farr[i + 1].Value).Length(), optimizeTolerance * pow( float(precision) / 100,2)/ ((farr[i + 1].Value - farr[i].Value).Length()/100))) {

				map.Remove(farr[i + 1].Key);
				farr.RemoveAt(i + 1);
				i--; //i+1 becomes i so its annigilate for's i++
			}
		}
		UE_LOG(LogTemp, Display, TEXT("newlen is %d"), map.Num());
	}

}*/