59 lines
2.0 KiB
C#
59 lines
2.0 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) {
|
|
bool goalReached = false;
|
|
Vector2 positionError = new Vector2(WorldPosition.x - worldTransform.origin.x,
|
|
WorldPosition.z - worldTransform.origin.z);
|
|
float positionErrorSquared = positionError.LengthSquared();
|
|
worldTransform.basis.Quat();
|
|
float orientationError = Mathf.Abs(worldTransform.basis.Quat().AngleTo(WorldOrientation));
|
|
float 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;
|
|
}
|
|
} |