70 lines
2.1 KiB
C#
70 lines
2.1 KiB
C#
using System;
|
|
using Godot;
|
|
|
|
public class NavigationPoint
|
|
{
|
|
[Flags]
|
|
public enum NavigationFlags
|
|
{
|
|
Position = 1,
|
|
Orientation = 2
|
|
}
|
|
|
|
public readonly NavigationFlags Flags;
|
|
public Quat WorldOrientation = Quat.Identity;
|
|
public float WorldAngle;
|
|
|
|
public Vector3 WorldPosition = Vector3.Zero;
|
|
|
|
public NavigationPoint(Vector3 worldPosition)
|
|
{
|
|
WorldPosition = worldPosition;
|
|
Flags = NavigationFlags.Position;
|
|
}
|
|
|
|
public NavigationPoint(Quat worldOrientation)
|
|
{
|
|
WorldOrientation = worldOrientation;
|
|
Flags = NavigationFlags.Orientation;
|
|
}
|
|
|
|
public NavigationPoint(Transform worldTransform)
|
|
{
|
|
WorldPosition = worldTransform.origin;
|
|
WorldOrientation = worldTransform.basis.Quat();
|
|
WorldAngle = Globals.CalcPlaneAngle(worldTransform);
|
|
|
|
Flags = NavigationFlags.Position | NavigationFlags.Orientation;
|
|
}
|
|
|
|
public bool IsReached(Transform worldTransform)
|
|
{
|
|
var goalReached = false;
|
|
var positionError = new Vector2(WorldPosition.x - worldTransform.origin.x,
|
|
WorldPosition.z - worldTransform.origin.z);
|
|
var positionErrorSquared = positionError.LengthSquared();
|
|
worldTransform.basis.Quat();
|
|
var orientationError = Mathf.Abs(worldTransform.basis.Quat().AngleTo(WorldOrientation));
|
|
var angleError = Mathf.Abs(Globals.CalcPlaneAngle(worldTransform) - WorldAngle);
|
|
|
|
if (Flags.HasFlag(NavigationFlags.Position)
|
|
&& Flags.HasFlag(NavigationFlags.Orientation)
|
|
&& positionErrorSquared < Globals.EpsPositionSquared
|
|
&& angleError < Globals.EpsRadians)
|
|
{
|
|
goalReached = true;
|
|
}
|
|
else if (Flags == NavigationFlags.Position &&
|
|
positionErrorSquared < Globals.EpsPositionSquared)
|
|
{
|
|
goalReached = true;
|
|
}
|
|
else if (Flags == NavigationFlags.Orientation &&
|
|
angleError < Globals.EpsRadians)
|
|
{
|
|
goalReached = true;
|
|
}
|
|
|
|
return goalReached;
|
|
}
|
|
} |