diff --git a/components/NavigationComponent.cs b/components/NavigationComponent.cs index 5a7d216..0c4d374 100644 --- a/components/NavigationComponent.cs +++ b/components/NavigationComponent.cs @@ -2,12 +2,10 @@ using System; using System.Collections.Generic; using System.Diagnostics; using System.Linq; -using System.Numerics; using Godot; using GodotComponentTest.utils; using Vector2 = Godot.Vector2; using Vector3 = Godot.Vector3; -using GoDotLog; /// /// @@ -24,7 +22,7 @@ public class NavigationComponent : Spatial public Vector3 WorldPosition = Vector3.Zero; public Quat WorldOrientation = Quat.Identity; - public NavigationFlags Flags = NavigationFlags.Position | NavigationFlags.Orientation; + public readonly NavigationFlags Flags; public NavigationPoint(Vector3 worldPosition) { @@ -49,22 +47,22 @@ public class NavigationComponent : Spatial { bool goalReached = false; float positionErrorSquared = (worldTransform.origin - WorldPosition).LengthSquared(); - Quat own_orientation = worldTransform.basis.Quat(); + worldTransform.basis.Quat(); float orientationError = Mathf.Abs(worldTransform.basis.Quat().AngleTo(WorldOrientation)); - if (Flags.HasFlag(NavigationPoint.NavigationFlags.Position) - && Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation) + if (Flags.HasFlag(NavigationFlags.Position) + && Flags.HasFlag(NavigationFlags.Orientation) && positionErrorSquared < Globals.EpsPositionSquared && orientationError < Globals.EpsRadians) { goalReached = true; } - else if (Flags == NavigationPoint.NavigationFlags.Position && + else if (Flags == NavigationFlags.Position && positionErrorSquared < Globals.EpsPositionSquared) { goalReached = true; } - else if (Flags == NavigationPoint.NavigationFlags.Orientation && + else if (Flags == NavigationFlags.Orientation && orientationError < Globals.EpsRadians) { goalReached = true; @@ -434,7 +432,7 @@ public class NavigationComponent : Spatial previousPoint = parentNode.GlobalTranslation; foreach (NavigationPoint point in _smoothedPathWorldNavigationPoints) { - debugGeometry.SetColor(new Color(0, 0, 1, 1)); + debugGeometry.SetColor(new Color(0, 0, 1)); debugGeometry.AddVertex(previousPoint + yOffset); debugGeometry.AddVertex(point.WorldPosition + yOffset); @@ -444,7 +442,7 @@ public class NavigationComponent : Spatial previousPoint = parentNode.GlobalTranslation; foreach (NavigationPoint point in _planningPathWorldNavigationPoints) { - debugGeometry.SetColor(new Color(1, 0, 1, 1)); + debugGeometry.SetColor(new Color(1, 0, 1)); debugGeometry.AddVertex(previousPoint + yOffset); debugGeometry.AddVertex(point.WorldPosition + yOffset); @@ -454,7 +452,7 @@ public class NavigationComponent : Spatial previousPoint = parentNode.GlobalTranslation; foreach (NavigationPoint point in _planningPathSmoothedWorldNavigationPoints) { - debugGeometry.SetColor(new Color(1, 1, 0, 1)); + debugGeometry.SetColor(new Color(1, 1, 0)); debugGeometry.AddVertex(previousPoint + yOffset); debugGeometry.AddVertex(point.WorldPosition + yOffset);