UE4の標準スプライン機能で最近点を求めてみた。

すみません、遅れてしまいました。

#概要
空間上の一点を指定して、スプラインとの最近点探索を行います。
関数名でだいたい何やってるか分かるよう名前つけています。
決してコメントが少ないことへのいi（ｒｙ

#SeekClosestPoint
SeekClosestPoint
SeekClosestPoint2Line
これら2つの関数で最近点を探索します。
SeekClosestPointが
有る一点に対するスプライン上の最近点を求めます

SeekClosestPoint2Line
ある直線に対するスプライン上の最近点を求めます。

#使い方
![Clipboard02.jpg](https://qiita-image-store.s3.amazonaws.com/0/32364/e2a6f0da-f413-8014-f813-5aba153c88c8.jpeg)
こんな感じになります。
Retuan Valueで帰ってきた数値を次のTestCenterに入れる必要がありますので注意して下さい。

#方法
単純な２分岐による探索になります。
１，指定した領域全てをfirstStepSizeで指定した最小単位で探索。
この最初のステップのみに関しては２分岐ではなく全て探索しています。
ローカルミニマムを避けるためのプロセスになります。

２，全体を探索して出てきた領域に対し、２分岐による検索を開始します。
探索対象の領域を２つに分割
２つの領域の中央と、探索する点(2lineの場合は直線)との距離をテストします。

３，２の結果の近い領域をまた２つに割って、再び同じことをします。

#負荷対策
コードを読むとわかると思いますが
”float _testCenter, float _testWidth”で、テストする範囲を指定
”float _okGosa”で、合格とする誤差を指定します。
尚、２分岐で分割したサイズがokGosaより小さくなったらテスト終了です。
この探索は重さとの戦いです。

#振動対策
検索する場合、どの”_testCenter”を指定したとしても
”FVector _testPoint”が同じである限り必ず同じ結果が求められるようにしないといけません。
これは画面が停止しているのにも関わらず結果が振動してしまうと非常に見栄えがよろしくないからです。
この為まず初めにどっからどう検索しても結果が一定になるように
スタート地点を調整します。
startAllPos = _firstStepSize * ceilf((_testCenter - (0.5 * _testWidth)) / _firstStepSize);
ここがその一文で、_firstStepSizeで割り切れる場所に強制的に合わせます。
（よく考えたらココってバグりそうな処理ですね…。）
※ _testWidthは_firstStepSizeの半分より大きいサイズにして下さい。
多分バグります。



```cpp:code
UCLASS()
class CONTENTEXAMPLES_API UMyBlueprintFunctionLibrary : public UBlueprintFunctionLibrary
{
	GENERATED_UCLASS_BODY()



	UFUNCTION(BlueprintCallable, Category = "MyBPLibrary")
		static float SeekClosestPoint(USplineComponent *_spline, bool _isCloseLine,
		FVector _testPoint, float _testCenter, float _testWidth, float _okGosa, float _firstStepSize
		, FVector &debugOut, FVector &debugOut2);

	UFUNCTION(BlueprintCallable, Category = "MyBPLibrary")
		static float SeekClosestPoint2Line(USplineComponent *_spline, bool _isCloseLine,
		FVector _testPoint, FVector _testOrient , float _testCenter, float _testWidth, float _okGosa, float _firstStepSize
		, FVector &debugOut, FVector &debugOut2);
};
```


```cpp:code

#include "ContentExamples.h"
#include "MyBlueprintFunctionLibrary.h"

float clamp(float value, float lowerLimit, float upperLimit)
{
	if (value < lowerLimit)
	{ 
		return lowerLimit;
	}
	else if (value > upperLimit)
	{ 
		return upperLimit; 
	}
	else 
	{ 
		return value; 
	}
}
/*
double LengthSqr(FVector vec)
{
	return ((double)vec.X * (double)vec.X + (double)vec.Y * (double)vec.Y + (double)vec.Z * (double)vec.Z);
}
*/

float LengthSqr(FVector vec)
{
	return (vec.X * vec.X + vec.Y * vec.Y + vec.Z * vec.Z);
}


float Line2PointLength(FVector _unitOrient, FVector _point)
{
	FVector cross = FVector::CrossProduct(_unitOrient, _point);
	return cross.X * cross.X + cross.Y * cross.Y + cross.Z * cross.Z;

}


float MyClamp( float _inNum )
{
	if (_inNum > 1.0 || _inNum < 0.0)
	{
		return _inNum - floor(_inNum);
	}
	else
	{
		return _inNum;
	}

}

void BunkatuMethodOpen(USplineComponent *_spline ,FVector _testPoint , float _start, float _end, float &_outStart, float &_outEnd)
{
	float lengthQuad =  0.25 * (_end - _start) ;
	float firstPoint = clamp(_start + lengthQuad, 0.0, 1.0);
	float secondPoint = clamp(_start + lengthQuad * 3.0, 0.0, 1.0);

	float firstDistance = LengthSqr(_spline->GetWorldLocationAtTime(firstPoint) - _testPoint);
	float secondDistance = LengthSqr(_spline->GetWorldLocationAtTime(secondPoint) - _testPoint);

	if (firstDistance < secondDistance)
	{
		_outStart = clamp(_start, 0.0, 1.0);
		_outEnd = clamp(_start + lengthQuad * 2.0, 0.0, 1.0);
	}
	else
	{
		_outStart = clamp(_start + lengthQuad * 2.0, 0.0, 1.0);
		_outEnd = clamp(_end, 0.0, 1.0);
	}
}



void BunkatuMethodClosed(USplineComponent *_spline, FVector _testPoint, float _start, float _end, float &_outStart, float &_outEnd)
{
	float lengthQuad = 0.25 * ((_end - _start));
	float firstPoint = MyClamp(_start + lengthQuad);
	float secondPoint = MyClamp(_start + lengthQuad * 3.0);

	float firstDistance = LengthSqr(_spline->GetWorldLocationAtTime(firstPoint) - _testPoint);
	float secondDistance = LengthSqr(_spline->GetWorldLocationAtTime(secondPoint) - _testPoint);

	if (firstDistance < secondDistance)
	{
		_outStart = _start;
		_outEnd = (_start + lengthQuad * 2.0);
	}
	else
	{
		_outStart = (_start + lengthQuad * 2.0);
		_outEnd = _end;
	}
}

void BunkatuMethodOpenLine(USplineComponent *_spline, FVector _testPos, FVector _testOrient, float _start, float _end, float &_outStart, float &_outEnd)
{
	float lengthQuad = 0.25 * (_end - _start);
	float firstPoint = (_start + lengthQuad);
	float secondPoint = (_start + lengthQuad * 3.0);

	float firstDistance = Line2PointLength(_testOrient, _spline->GetWorldLocationAtTime(firstPoint) - _testPos);
	float secondDistance = Line2PointLength(_testOrient, _spline->GetWorldLocationAtTime(secondPoint) - _testPos);

	if (firstDistance < secondDistance)
	{
		_outStart = clamp(_start, 0.0, 1.0);
		_outEnd = clamp(_start + lengthQuad * 2.0, 0.0, 1.0);
	}
	else
	{
		_outStart = clamp(_start + lengthQuad * 2.0, 0.0, 1.0);
		_outEnd = clamp(_end, 0.0, 1.0);
	}
}


void BunkatuMethodClosedLine(USplineComponent *_spline, FVector _testPos, FVector _testOrient, float _start, float _end, float &_outStart, float &_outEnd)
{
	float lengthQuad = 0.25 * ((_end - _start));
	float firstPoint = MyClamp(_start + lengthQuad);
	float secondPoint = MyClamp(_start + lengthQuad * 3.0);

	float firstDistance = LengthSqr(_spline->GetWorldLocationAtTime(firstPoint) - _testPos);
	float secondDistance = LengthSqr(_spline->GetWorldLocationAtTime(secondPoint) - _testPos);

	if (firstDistance < secondDistance)
	{
		_outStart = _start;
		_outEnd = (_start + lengthQuad * 2.0);
	}
	else
	{
		_outStart = (_start + lengthQuad * 2.0);
		_outEnd = _end;
	}
}

UMyBlueprintFunctionLibrary::UMyBlueprintFunctionLibrary(const class FPostConstructInitializeProperties& PCIP)
	: Super(PCIP)
{

}



float SeekClosestPointOpen(USplineComponent *_spline ,
	FVector _testPoint, float _testCenter, float _testWidth, float _okGosa, float _firstStepSize)
{
	float bestStep = 0.0;

	int bestAllPIndex = -1;
	float bestAllPDistanceQured = FLT_MAX;
	float startAllPos = 0.0;
	float endAllPPos = 0.0;

	float nowBunkiStart =  0.0;
	float nowBunkiEnd = 0.0;
	float resStart = 0.0;
	float resEnd = 0.0;

	if (_testCenter < 0.0)
	{
		startAllPos = 0.0;
		endAllPPos = 1.0;
	}
	else
	{
		startAllPos = _firstStepSize * ceilf((_testCenter - (0.5 * _testWidth)) / _firstStepSize);
		endAllPPos = _testCenter + (0.5 * _testWidth);
	}



	if ((endAllPPos - startAllPos) > _firstStepSize * 2.0)
	{

		for (float nowStep = startAllPos; nowStep < endAllPPos; nowStep += _firstStepSize)
		{

			float testingDistance = LengthSqr(_spline->GetWorldLocationAtTime(clamp(nowStep, 0.0, 1.0)) - _testPoint);

			if (bestAllPDistanceQured > testingDistance)
			{
				bestAllPDistanceQured = testingDistance;
				bestStep = nowStep;
			}

		}
		nowBunkiStart = bestStep - _firstStepSize ;
		nowBunkiEnd = bestStep + _firstStepSize ;

	}
	else
	{
		nowBunkiStart = startAllPos;
		nowBunkiEnd = endAllPPos;
	}

	//return clamp(nowBunkiStart + (nowBunkiEnd - nowBunkiStart) * 0.5, 0.0, 1.0);

	while ((nowBunkiEnd - nowBunkiStart) > _okGosa)
	{
		BunkatuMethodOpen(_spline, _testPoint, nowBunkiStart, nowBunkiEnd, resStart, resEnd);
		nowBunkiStart = resStart;
		nowBunkiEnd = resEnd;
	}
	return clamp(resStart + (resEnd - resStart) * 0.5, 0.0, 1.0);


}

float SeekClosestPointClosed(USplineComponent *_spline ,
	FVector _testPoint, float _testCenter, float _testWidth, float _okGosa, float _firstStepSize)
{
	float bestPos = 0.0;

	float bestAllPDistanceQured = FLT_MAX;
	float startAllPos = 0.0;
	float endAllPPos = 0.0;

	float nowBunkiStart = 0.0;
	float nowBunkiEnd = 0.0;
	float resStart = 0.0;
	float resEnd = 0.0;

	if (_testCenter < 0.0)
	{
		startAllPos = 0.0;
		endAllPPos = 1.0;
	}
	else
	{
		startAllPos = _firstStepSize * ceilf((_testCenter - (0.5 * _testWidth)) / _firstStepSize);
		endAllPPos = (_testCenter + (0.5 * _testWidth));
	}



	if ((endAllPPos - startAllPos) > _firstStepSize * 2.0)
	{

		for (float nowStep = startAllPos; nowStep < endAllPPos; nowStep += _firstStepSize)
		{
			float testingDistance = LengthSqr(_spline->GetWorldLocationAtTime(MyClamp(nowStep)) - _testPoint);

			if (bestAllPDistanceQured > testingDistance)
			{
				bestAllPDistanceQured = testingDistance;
				bestPos = nowStep;
			}

		}
		nowBunkiStart = (bestPos - _firstStepSize);
		nowBunkiEnd = (bestPos + _firstStepSize);

	}
	else
	{
		nowBunkiStart = startAllPos;
		nowBunkiEnd = endAllPPos;
	}

	while (MyClamp(nowBunkiEnd - nowBunkiStart)  > _okGosa)
	{
		BunkatuMethodClosed(_spline, _testPoint, nowBunkiStart, nowBunkiEnd, resStart, resEnd);
		nowBunkiStart = resStart;
		nowBunkiEnd = resEnd;
	}


	return MyClamp(resStart + (resEnd - resStart) * 0.5);
}


float UMyBlueprintFunctionLibrary::SeekClosestPoint(USplineComponent *_spline, bool _isCloseLine,
	FVector _testPoint, float _testCenter, float _testWidth, float _okGosa, float _firstStepSize
	, FVector &debugOut, FVector &debugOut2)
{
	float nowStep = 0.0;

	if (_okGosa < 0.000001)
	{
		return 0.0;
	}

	if (_firstStepSize < 0.000001)
	{
		_firstStepSize = 0.000001;
	}

	if (_testWidth < 0.000001)
	{
		_testWidth = 0.000001;
	}

	float resPoint;
	if (_isCloseLine)
	{
		resPoint = SeekClosestPointClosed(_spline,
			_testPoint, _testCenter, _testWidth, _okGosa, _firstStepSize);
	}
	else
	{
		resPoint = SeekClosestPointOpen(_spline,
			_testPoint, _testCenter, _testWidth, _okGosa, _firstStepSize);
	}

	return resPoint;
}




float SeekClosestLineOpen(USplineComponent *_spline,
	FVector _testPos , FVector _testOrient, float _testCenter, float _testWidth, float _okGosa, float _firstStepSize)
{
	float bestStep = 0.0;

	int bestAllPIndex = -1;
	float bestAllPDistanceQured = FLT_MAX;
	float startAllPos = 0.0;
	float endAllPPos = 0.0;

	float nowBunkiStart = 0.0;
	float nowBunkiEnd = 0.0;
	float resStart = 0.0;
	float resEnd = 0.0;

	if (_testCenter < 0.0)
	{
		startAllPos = 0.0;
		endAllPPos = 1.0;
	}
	else
	{
		startAllPos = _firstStepSize * ceilf((_testCenter - (0.5 * _testWidth)) / _firstStepSize);
		endAllPPos = _testCenter + (0.5 * _testWidth);
	}



	if ((endAllPPos - startAllPos) > _firstStepSize * 2.0)
	{

		for (float nowStep = startAllPos; nowStep < endAllPPos; nowStep += _firstStepSize)
		{

			float testingDistance = Line2PointLength(_testOrient, _spline->GetWorldLocationAtTime(clamp(nowStep, 0.0, 1.0)) - _testPos);

			if (bestAllPDistanceQured > testingDistance)
			{
				bestAllPDistanceQured = testingDistance;
				bestStep = nowStep;
			}

		}
		nowBunkiStart = bestStep - _firstStepSize;
		nowBunkiEnd = bestStep + _firstStepSize;

	}
	else
	{
		nowBunkiStart = startAllPos;
		nowBunkiEnd = endAllPPos;
	}



	while ((nowBunkiEnd - nowBunkiStart) > _okGosa)
	{
		BunkatuMethodOpenLine(_spline, _testPos, _testOrient, nowBunkiStart, nowBunkiEnd, resStart, resEnd);
		nowBunkiStart = resStart;
		nowBunkiEnd = resEnd;
	}


	return clamp(resStart + (resEnd - resStart) * 0.5, 0.0, 1.0);


}

float SeekClosestLineClosed(USplineComponent *_spline,
	FVector _testPos, FVector _testOrient, float _testCenter, float _testWidth, float _okGosa, float _firstStepSize)
{
	float bestPos = 0.0;

	float bestAllPDistanceQured = FLT_MAX;
	float startAllPos = 0.0;
	float endAllPPos = 0.0;

	float nowBunkiStart = 0.0;
	float nowBunkiEnd = 0.0;
	float resStart = 0.0;
	float resEnd = 0.0;

	if (_testCenter < 0.0)
	{
		startAllPos = 0.0;
		endAllPPos = 1.0;
	}
	else
	{
		startAllPos = _firstStepSize * ceilf((_testCenter - (0.5 * _testWidth)) / _firstStepSize);
		endAllPPos = (_testCenter + (0.5 * _testWidth));
	}



	if ((endAllPPos - startAllPos) > _firstStepSize * 2.0)
	{

		for (float nowStep = startAllPos; nowStep < endAllPPos; nowStep += _firstStepSize)
		{
			float testingDistance = Line2PointLength(_testOrient, _spline->GetWorldLocationAtTime(MyClamp(nowStep)) - _testPos);

			if (bestAllPDistanceQured > testingDistance)
			{
				bestAllPDistanceQured = testingDistance;
				bestPos = nowStep;
			}

		}
		nowBunkiStart = (bestPos - _firstStepSize);
		nowBunkiEnd = (bestPos + _firstStepSize);

	}
	else
	{
		nowBunkiStart = startAllPos;
		nowBunkiEnd = endAllPPos;
	}

	while (MyClamp(nowBunkiEnd - nowBunkiStart)  > _okGosa)
	{
		BunkatuMethodClosedLine(_spline, _testPos, _testOrient, nowBunkiStart, nowBunkiEnd, resStart, resEnd);
		nowBunkiStart = resStart;
		nowBunkiEnd = resEnd;
	}


	return MyClamp(resStart + (resEnd - resStart) * 0.5);
}



float UMyBlueprintFunctionLibrary::SeekClosestPoint2Line(USplineComponent *_spline, bool _isCloseLine,
	FVector _testPoint, FVector _testOrient, float _testCenter, float _testWidth, float _okGosa, float _firstStepSize
	, FVector &debugOut, FVector &debugOut2)
{
	float nowStep = 0.0;

	if (_okGosa < 0.000001)
	{
		return 0.0;
	}

	if (_firstStepSize < 0.000001)
	{
		_firstStepSize = 0.000001;
	}

	if (_testWidth < 0.000001)
	{
		_testWidth = 0.000001;
	}

	float resPoint;
	if (_isCloseLine)
	{
		resPoint = SeekClosestLineClosed(_spline,
			_testPoint, _testOrient, _testCenter, _testWidth, _okGosa, _firstStepSize);
	}
	else
	{
		resPoint = SeekClosestLineOpen(_spline,
			_testPoint, _testOrient, _testCenter, _testWidth, _okGosa, _firstStepSize);
	}

	return resPoint;
}


```