Creating a new behaviour area module

This is an example of a behaviour area module. It fills the context steering data using the curve tangents and applies a force to keep the agents on the curve.

#pragma once
#include "CoreMinimal.h"
#include "AtomsUnreal/Public/Components/AtomsBehaviourAreaComponent.h"
#include <AtomsUtils/SampledCurve.h>
#include "MyFollowCurveBehaviourAreaComponent.generated.h"

UCLASS(ClassGroup = "AtomsBehaviourArea", meta = (BlueprintSpawnableComponent))
class ATOMSUNREAL_API UMyFollowCurveBehaviourAreaComponent: public UAtomsBehaviourAreaComponent
{
	GENERATED_BODY()

public:

	/*Number of curve samples*/
	UPROPERTY(EditAnywhere, Category = "Behaviour")
	int32 NumSamples;

	/*The max distance from the curve. Any agent farther than this value from the curve will be ignored*/
	UPROPERTY(EditAnywhere, Category = "Behaviour")
	float MaxDistance;

	/* Drop off value for smoothly assume curve closest tangent direction*/
	UPROPERTY(EditAnywhere, Category = "Behaviour")
	float MaxDistanceDropoff;

	/* Affects only the orientation and not the direction */
	UPROPERTY(EditAnywhere, Category = "Behaviour")
	bool OrientationOnly;

	/* Enable influence on the left side of the curve */
	UPROPERTY(EditAnywhere, Category = "Behaviour")
	bool EnableLeftSide;

	/* Enable influence on the right side of the curve */
	UPROPERTY(EditAnywhere, Category = "Behaviour")
	bool EnableRightSide;

	/* Enable the influence arount the first and the last point,
	 * if off the influence is limited to the right and left side of the curve*/
	UPROPERTY(EditAnywhere, Category = "Behaviour")
	bool EnableCaps;

	/* Generate a force to keep the agent position onthe curve */
	UPROPERTY(EditAnywhere, Category = "Behaviour")
	bool KeepAgentOnCurve;

	UPROPERTY(EditAnywhere, Category = "Behaviour", meta=(EditCondition=KeepAgentOnCurve))
	float MaxForce;

	UMyFollowCurveBehaviourAreaComponent();

	virtual void BeginPlay() override;

	virtual void TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override;

	virtual FBoxSphereBounds CalcBounds(const FTransform& LocalToWorld) const override;

	UFUNCTION(BlueprintCallable, Category="My Follow Curve Behaviour Area")
	void UpdatedCurve();

	virtual bool FillContextSteeringMaps(
		Atoms::AgentGroup* AgentGroup,
		Atoms::Agent* Agent,
		const FVector& MapOrigin,
		const FVector& MapOrientation,
		const double Distance,
		const double DeltaTime) override;

	virtual bool EvaluateForce(
		Atoms::AgentGroup* AgentGroup,
		Atoms::Agent* Agent,
		const FVector& Direction,
		const FVector& Velocity,
		const float DeltaTime,
		FVector& OutForce) override;

private:
	FBoxSphereBounds CurveBound;
	AtomsUtils::SampledCurve Curve;
	AtomsMath::Matrix WorldMatrix;
	AtomsMath::Matrix WorldMatrixNormal;
	AtomsMath::Matrix WorldMatrixInverse;
};

#include "MyFollowCurveBehaviourAreaComponent.h"
#include "Components/SplineComponent.h"
#include <Atoms/Agent.h>
#include "AtomsConversions.h"

UMyFollowCurveBehaviourAreaComponent::UMyFollowCurveBehaviourAreaComponent():
	NumSamples(10),
	MaxDistance(1000.0f),
	MaxDistanceDropoff(0.0f),
	OrientationOnly(false),
	EnableLeftSide(true),
	EnableRightSide(true),
	EnableCaps(true),
	KeepAgentOnCurve(false),
	MaxForce(500.0f)
{
	
}

void UMyFollowCurveBehaviourAreaComponent::BeginPlay()
{
	UpdatedCurve();
	WorldMatrix = AtomsConverter::ToMatrix(GetComponentTransform());
	WorldMatrixInverse = WorldMatrix.inverse();
	Super::BeginPlay();
}

void UMyFollowCurveBehaviourAreaComponent::TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction)
{
	WorldMatrix = AtomsConverter::ToMatrix(GetComponentTransform());

	WorldMatrixInverse = WorldMatrix.inverse();
	WorldMatrixNormal = WorldMatrix.transposed();

	Super::TickComponent(DeltaTime, TickType, ThisTickFunction);

}

FBoxSphereBounds UMyFollowCurveBehaviourAreaComponent::CalcBounds(const FTransform& LocalToWorld) const
{
	return CurveBound.TransformBy(LocalToWorld);
}

void UMyFollowCurveBehaviourAreaComponent::UpdatedCurve()
{
	FVector MaxDistVec(MaxDistance, MaxDistance, MaxDistance);
	USplineComponent* SplineComponent = Cast<USplineComponent>(GetAttachParent());
	FBox CurrentBound;
	CurrentBound.Init();
	if (SplineComponent)
	{
		float length = SplineComponent->GetSplineLength();
		auto PointsNo = SplineComponent->GetNumberOfSplinePoints();
		auto numSamples = PointsNo * NumSamples;
		auto& AtomsPoints = Curve.cvs();
		AtomsPoints.reserve(numSamples);
		for (auto i = 0; i < numSamples; i++)
		{
			auto Point = SplineComponent->GetLocationAtDistanceAlongSpline(float(i) * length / static_cast<float>(numSamples - 1), ESplineCoordinateSpace::Local);
			AtomsPoints.push_back(AtomsConverter::ToVector3(Point));
			CurrentBound += Point + MaxDistVec;
			CurrentBound += Point - MaxDistVec;
		}
		Curve.setDegree(3);
		Curve.computeUniformKnots();
		Curve.resample(numSamples);
	}

	CurveBound.BoxExtent = CurrentBound.GetExtent();
	CurveBound.Origin = CurrentBound.GetCenter();
	CurveBound.SphereRadius = CurrentBound.GetExtent().Size() * 0.5;
	CurveBound.ExpandBy(MaxDistance);
}

bool UMyFollowCurveBehaviourAreaComponent::FillContextSteeringMaps(
	Atoms::AgentGroup* AgentGroup,
	Atoms::Agent* Agent,
	const FVector& MapOrigin,
	const FVector& MapOrientation,
	const double Distance,
	const double DeltaTime)
{
	if (Curve.cvs().size() == 0) return false;
	Atoms::ContextSteeringData& data = Agent->contextSteeringData();
	auto sample = Curve.closestTangent(data.position * WorldMatrixInverse);
	if (sample.first < 0.0) return false;

	AtomsMath::Vector3 closetsLocalP = Curve.pointOnCurve(sample.first);
	AtomsMath::Vector3 closetsP = closetsLocalP * WorldMatrix;
	AtomsMath::Vector3 vecToClosestP = closetsP - data.position;
	vecToClosestP.y = 0.0;
	double distanceSq = vecToClosestP.length2();
	if (distanceSq > (MaxDistance * MaxDistance))
	{
		return false;
	}
	const double errorRange = 0.0001 * 0.0001;
	if (!EnableCaps && ((closetsLocalP - Curve.cvs().front()).length2() < errorRange ||
		(closetsLocalP - Curve.cvs().back()).length2() < errorRange))
	{
		return false;
	}

	AtomsMath::Vector3 outTangent = sample.second;
	AtomsMath::Vector4 tangent4(outTangent.x, outTangent.y, outTangent.z, 0.0);
	tangent4 = tangent4 * WorldMatrixNormal;
	outTangent.x = tangent4.x;
	outTangent.y = tangent4.y;
	outTangent.z = tangent4.z;
	outTangent.normalize();



	double Weight = 1.0;
	double agentDistance = FMath::Sqrt(distanceSq);

	float startDropOff = MaxDistance - MaxDistanceDropoff;
	if (MaxDistanceDropoff > 0.0 && agentDistance >= startDropOff)
	{
		Weight *= 1.0 - (agentDistance - startDropOff) / MaxDistanceDropoff;
	}
	if (!EnableLeftSide || !EnableRightSide)
	{
		double dotSide = (vecToClosestP / agentDistance).cross(outTangent).normalize().dot(data.up);
		if ((!EnableLeftSide && dotSide < 0.0) || (!EnableRightSide && dotSide > 0.0))
		{
			Weight = 0.0f;
		}
	}

	if (OrientationOnly && data.direction.dot(outTangent) < 0.0)
	{
		outTangent = -outTangent;
	}
	if (Weight == 1.0)
	{
		data.direction = outTangent;
		return true;
	}

	AtomsMath::Quaternion rotation;
	rotation.setRotation(data.direction, outTangent);
	AtomsCore::Vector3 rotAxis = rotation.axis();
	double angle = rotation.angle() * Weight;
	rotation.setAxisAngle(rotAxis, angle);
	data.direction = rotation.rotateVector(data.direction);
	if (FMath::IsNaN(data.direction.x) ||
	FMath::IsNaN(data.direction.y) ||
	FMath::IsNaN(data.direction.z))
	{
		check(false);
		//AtomsUtils::Logger::warning() << "Nan ";
	}
	return true;
}

bool UMyFollowCurveBehaviourAreaComponent::EvaluateForce(
	Atoms::AgentGroup* AgentGroup,
	Atoms::Agent* Agent,
	const FVector& Direction,
	const FVector& Velocity,
	const float DeltaTime,
	FVector& OutForce)
{
	
	if (!KeepAgentOnCurve || Curve.cvs().size() == 0) return false;
	Atoms::ContextSteeringData& data = Agent->contextSteeringData();
	auto sample = Curve.closestPoint(data.position * WorldMatrixInverse);
	if (sample.first < 0.001 ||
		sample.first > (Curve.cvs().size() - Curve.degree() - 0.5)) return false;

	AtomsMath::Vector3 closestP(sample.second);

	float agentRadius = Agent->agentType()->collisionRadius() * Agent->scale()->get().x * 0.5;

	AtomsMath::Vector3 vecToClosestP = closestP * WorldMatrix - data.position;
	vecToClosestP.y = 0.0;
	double distanceSq = vecToClosestP.length2();
	if (distanceSq > (MaxDistance * MaxDistance))
	{
		return false;
	}
	FVector CurrentForce = AtomsConverter::FromVector3(vecToClosestP / DeltaTime) / DeltaTime;
	auto CurrentMagnitude = CurrentForce.Size();
	if (CurrentMagnitude < 0.0001)
		return false;
	auto ForceMagnitude = FMath::Min(CurrentMagnitude, MaxForce);
	CurrentForce *= ForceMagnitude / CurrentMagnitude;
	OutForce += CurrentForce;
	return true;
}