446 lines
18 KiB
C#
446 lines
18 KiB
C#
using System;
|
|
using System.Collections;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using UnityEngine;
|
|
|
|
public class PathMover
|
|
{
|
|
private readonly Transform transform;
|
|
private readonly MonoBehaviour monoBehaviour;
|
|
private readonly float defaultSpeed;
|
|
private readonly float rotationSpeed;
|
|
private readonly int trajectoryResolution;
|
|
private readonly ActionExecutor actionExecutor;
|
|
private bool isMoving;
|
|
private volatile bool isCancelled;
|
|
private Coroutine moveCoroutine;
|
|
private float currentTheta;
|
|
private float currentVdaTheta;
|
|
private readonly float rotationThreshold = 0.05f; // Ngưỡng xoay 0.1 rad (~5.7°)
|
|
private bool isBackward = false; // Trạng thái đi lùi
|
|
|
|
public event Action OnMoveCompleted;
|
|
|
|
public PathMover(
|
|
Transform transform,
|
|
MonoBehaviour monoBehaviour,
|
|
float defaultSpeed,
|
|
int trajectoryResolution,
|
|
ActionExecutor actionExecutor,
|
|
float rotationSpeed = 180f)
|
|
{
|
|
this.transform = transform;
|
|
this.monoBehaviour = monoBehaviour;
|
|
this.defaultSpeed = defaultSpeed;
|
|
this.rotationSpeed = rotationSpeed;
|
|
this.trajectoryResolution = trajectoryResolution;
|
|
this.actionExecutor = actionExecutor;
|
|
isMoving = false;
|
|
isCancelled = false;
|
|
currentTheta = 0f;
|
|
currentVdaTheta = 0f;
|
|
isBackward = false;
|
|
}
|
|
|
|
public bool IsMoving => isMoving;
|
|
public float CurrentTheta => currentTheta;
|
|
public float CurrentVdaTheta => currentVdaTheta;
|
|
|
|
public void SetTheta(float vdaTheta)
|
|
{
|
|
currentVdaTheta = vdaTheta;
|
|
currentTheta = NormalizeTheta(-vdaTheta * Mathf.Rad2Deg);
|
|
transform.eulerAngles = new Vector3(0, -vdaTheta * Mathf.Rad2Deg, 0);
|
|
}
|
|
|
|
public void UpdateVdaThetaFromUnity(float eulerYDegrees)
|
|
{
|
|
currentVdaTheta = NormalizeAngle(-eulerYDegrees * Mathf.Deg2Rad);
|
|
currentTheta = NormalizeTheta(eulerYDegrees);
|
|
}
|
|
|
|
public void StartMove(Node[] nodes, Edge[] edges)
|
|
{
|
|
if (!isMoving)
|
|
{
|
|
isCancelled = false;
|
|
isBackward = false; // Reset trạng thái đi lùi
|
|
moveCoroutine = monoBehaviour.StartCoroutine(MoveAlongPath(nodes, edges));
|
|
Debug.Log("Bắt đầu di chuyển mới.");
|
|
}
|
|
else
|
|
{
|
|
Debug.LogWarning("AMR đang di chuyển, bỏ qua lệnh mới.");
|
|
}
|
|
}
|
|
|
|
public void CancelMove()
|
|
{
|
|
if (moveCoroutine != null)
|
|
{
|
|
monoBehaviour.StopCoroutine(moveCoroutine);
|
|
moveCoroutine = null;
|
|
Debug.Log("Đã dừng coroutine MoveAlongPath.");
|
|
}
|
|
isMoving = false;
|
|
isCancelled = true;
|
|
actionExecutor.CancelActions();
|
|
isBackward = false; // Reset trạng thái đi lùi
|
|
Debug.Log("Đã hủy di chuyển.");
|
|
}
|
|
private float RoundThetaToStandardAngle(float thetaRad)
|
|
{
|
|
float thetaDeg = thetaRad * Mathf.Rad2Deg;
|
|
float[] standardAngles = { 0f, 90f, 180f, 270f };
|
|
float roundingThreshold = 10f; // Ngưỡng làm tròn ±5°
|
|
float closestAngle = standardAngles.OrderBy(a => Mathf.Abs(Mathf.DeltaAngle(thetaDeg, a))).First();
|
|
|
|
if (Mathf.Abs(Mathf.DeltaAngle(thetaDeg, closestAngle)) <= roundingThreshold)
|
|
{
|
|
return closestAngle * Mathf.Deg2Rad;
|
|
}
|
|
|
|
return thetaRad;
|
|
}
|
|
private IEnumerator MoveAlongPath(Node[] nodes, Edge[] edges)
|
|
{
|
|
isMoving = true;
|
|
var sortedEdges = edges.OrderBy(e => e.SequenceId).ToList();
|
|
var sortedNodes = nodes.OrderBy(n => n.SequenceId).ToList();
|
|
bool isLastEdge = false;
|
|
|
|
// Xác định edge cong gần node cuối
|
|
var curveEdges = sortedEdges.Where(e => e.Trajectory.Degree > 1).ToList();
|
|
var lastCurveEdge = curveEdges.OrderByDescending(e => e.SequenceId).FirstOrDefault();
|
|
int lastCurveSequenceId = lastCurveEdge != null ? lastCurveEdge.SequenceId : -1;
|
|
|
|
// Xác định các node giao giữa đường thẳng và đường cong
|
|
HashSet<string> intersectionNodeIds = new();
|
|
for (int i = 0; i < sortedEdges.Count - 1; i++)
|
|
{
|
|
var currentEdge = sortedEdges[i];
|
|
var nextEdge = sortedEdges[i + 1];
|
|
bool isCurrentStraight = currentEdge.Trajectory.Degree == 1;
|
|
bool isNextCurve = nextEdge.Trajectory.Degree > 1;
|
|
bool isCurrentCurve = currentEdge.Trajectory.Degree > 1;
|
|
bool isNextStraight = nextEdge.Trajectory.Degree == 1;
|
|
|
|
if ((isCurrentStraight && isNextCurve) || (isCurrentCurve && isNextStraight))
|
|
{
|
|
intersectionNodeIds.Add(currentEdge.EndNodeId);
|
|
}
|
|
}
|
|
|
|
// Xử lý node đầu tiên
|
|
var firstNode = sortedNodes.FirstOrDefault(n => n.SequenceId == 0);
|
|
if (firstNode != null)
|
|
{
|
|
// Xoay theo theta của node đầu tiên
|
|
float vdaTheta = firstNode.NodePosition.Theta;
|
|
float thetaDiff = Mathf.Abs(NormalizeAngle(vdaTheta - currentVdaTheta));
|
|
if (thetaDiff > rotationThreshold)
|
|
{
|
|
currentVdaTheta = vdaTheta;
|
|
float unityEulerY = -vdaTheta * Mathf.Rad2Deg;
|
|
Quaternion targetRotation = Quaternion.Euler(0, unityEulerY, 0);
|
|
while (Quaternion.Angle(transform.rotation, targetRotation) > 1f)
|
|
{
|
|
transform.rotation = Quaternion.Slerp(transform.rotation, targetRotation, rotationSpeed * Time.deltaTime / 180f);
|
|
currentTheta = NormalizeTheta(Mathf.Atan2(transform.right.x, transform.forward.z));
|
|
yield return null;
|
|
}
|
|
transform.rotation = targetRotation;
|
|
}
|
|
|
|
// Thực thi hành động tại node đầu tiên
|
|
if (firstNode.Actions != null && firstNode.Actions.Length > 0)
|
|
{
|
|
yield return monoBehaviour.StartCoroutine(actionExecutor.ExecuteNodeActions(firstNode));
|
|
if (isCancelled) { isMoving = false; yield break; }
|
|
}
|
|
}
|
|
|
|
for (int edgeIndex = 0; edgeIndex < sortedEdges.Count; edgeIndex++)
|
|
{
|
|
var edge = sortedEdges[edgeIndex];
|
|
isLastEdge = (edgeIndex == sortedEdges.Count - 1);
|
|
|
|
if (isCancelled) { isMoving = false; yield break; }
|
|
|
|
if (edge.Trajectory?.ControlPoints == null || edge.Trajectory.ControlPoints.Length == 0)
|
|
continue;
|
|
|
|
var startNode = sortedNodes.FirstOrDefault(n => n.NodeId == edge.StartNodeId);
|
|
var endNode = sortedNodes.FirstOrDefault(n => n.NodeId == edge.EndNodeId);
|
|
|
|
// Thực thi hành động tại startNode
|
|
if (startNode != null && startNode.Actions != null && startNode.Actions.Length > 0)
|
|
{
|
|
yield return monoBehaviour.StartCoroutine(actionExecutor.ExecuteNodeActions(startNode));
|
|
if (isCancelled) { isMoving = false; yield break; }
|
|
}
|
|
|
|
// Kích hoạt đi lùi tại điểm bắt đầu của edge cong cuối
|
|
if (!isBackward && edge.SequenceId == lastCurveSequenceId)
|
|
{
|
|
isBackward = true;
|
|
}
|
|
|
|
// Thực thi các hành động của edge
|
|
foreach (var action in edge.Actions)
|
|
{
|
|
yield return monoBehaviour.StartCoroutine(actionExecutor.ExecuteEdgeAction(edge, action));
|
|
if (isCancelled) { isMoving = false; yield break; }
|
|
}
|
|
|
|
float speed = edge.MaxSpeed > 0 ? edge.MaxSpeed : defaultSpeed;
|
|
List<Vector3> points;
|
|
bool isCurve = edge.Trajectory.Degree > 1;
|
|
|
|
if (!isCurve)
|
|
{
|
|
points = new List<Vector3>
|
|
{
|
|
new Vector3(edge.Trajectory.ControlPoints[0].X, transform.position.y, edge.Trajectory.ControlPoints[0].Y),
|
|
new Vector3(edge.Trajectory.ControlPoints[1].X, transform.position.y, edge.Trajectory.ControlPoints[1].Y)
|
|
};
|
|
}
|
|
else
|
|
{
|
|
points = GenerateBSplinePoints(edge.Trajectory, trajectoryResolution);
|
|
if (points.Count == 0) continue;
|
|
|
|
var lastControlPoint = edge.Trajectory.ControlPoints.Last();
|
|
points[points.Count - 1] = new Vector3(lastControlPoint.X, transform.position.y, lastControlPoint.Y);
|
|
}
|
|
|
|
// Tính tổng quãng đường của edge
|
|
float totalDistance = 0f;
|
|
for (int i = 0; i < points.Count - 1; i++)
|
|
{
|
|
totalDistance += Vector3.Distance(points[i], points[i + 1]);
|
|
}
|
|
|
|
// Tính góc mục tiêu tại endNode, làm tròn ngay từ đầu cho edge cong
|
|
float endTheta = endNode != null ? endNode.NodePosition.Theta : currentVdaTheta;
|
|
if (isCurve && endNode != null)
|
|
{
|
|
endTheta = RoundThetaToStandardAngle(endTheta);
|
|
Debug.Log($"Bắt đầu edge cong {edge.EdgeId}: Tổng quãng đường={totalDistance:F2}m, " +
|
|
$"Góc chuẩn mục tiêu tại endNode={endTheta:F2} rad (eulerY={-endTheta * Mathf.Rad2Deg:F2}°)");
|
|
}
|
|
Quaternion endRotation = Quaternion.Euler(0, -endTheta * Mathf.Rad2Deg, 0);
|
|
|
|
for (int i = 0; i < points.Count - 1; i++)
|
|
{
|
|
if (isCancelled) { isMoving = false; yield break; }
|
|
|
|
Vector3 currentPoint = points[i];
|
|
Vector3 nextPoint = points[i + 1];
|
|
Vector3 forward = (nextPoint - currentPoint).normalized;
|
|
|
|
if (isBackward)
|
|
{
|
|
forward = -forward;
|
|
}
|
|
|
|
float distanceToNext = Vector3.Distance(transform.position, nextPoint);
|
|
float step = speed * Time.deltaTime;
|
|
|
|
while (distanceToNext > 0.01f)
|
|
{
|
|
if (isCancelled) { isMoving = false; yield break; }
|
|
|
|
transform.position = Vector3.MoveTowards(transform.position, nextPoint, step);
|
|
|
|
if (isCurve && forward != Vector3.zero)
|
|
{
|
|
// Tính tiến độ di chuyển trên toàn bộ edge
|
|
float remainingDistance = 0f;
|
|
for (int j = i; j < points.Count - 1; j++)
|
|
{
|
|
remainingDistance += Vector3.Distance(points[j], points[j + 1]);
|
|
}
|
|
float progress = 1f - (remainingDistance / totalDistance);
|
|
if (progress < 0) progress = 0;
|
|
if (progress > 1) progress = 1;
|
|
|
|
// Tính góc hiện tại từ vector forward
|
|
Quaternion tangentRotation = Quaternion.LookRotation(forward, Vector3.up);
|
|
tangentRotation *= Quaternion.Euler(0, -90, 0);
|
|
|
|
// Tính góc cần xoay và thời gian còn lại
|
|
float angleToTarget = Quaternion.Angle(tangentRotation, endRotation);
|
|
float timeRemaining = remainingDistance / speed;
|
|
float adjustedRotationSpeed = timeRemaining > 0 ? (angleToTarget / timeRemaining) : rotationSpeed;
|
|
adjustedRotationSpeed = Mathf.Max(adjustedRotationSpeed, rotationSpeed);
|
|
|
|
// Xoay dần về góc mục tiêu tại endNode (đã làm tròn)
|
|
Quaternion targetRotation = Quaternion.Slerp(tangentRotation, endRotation, progress);
|
|
float currentAngleToTarget = Quaternion.Angle(transform.rotation, targetRotation);
|
|
|
|
if (currentAngleToTarget > 0.1f)
|
|
{
|
|
|
|
transform.rotation = Quaternion.Slerp(transform.rotation, targetRotation, adjustedRotationSpeed * Time.deltaTime / 180f);
|
|
currentTheta = NormalizeTheta(Mathf.Atan2(transform.right.x, transform.forward.z));
|
|
currentVdaTheta = NormalizeAngle(-transform.eulerAngles.y * Mathf.Deg2Rad);
|
|
}
|
|
}
|
|
|
|
distanceToNext = Vector3.Distance(transform.position, nextPoint);
|
|
yield return null;
|
|
}
|
|
|
|
transform.position = nextPoint;
|
|
}
|
|
|
|
// Kiểm tra khớp góc tại endNode của edge cong
|
|
if (isCurve && endNode != null)
|
|
{
|
|
float roundedTheta = endTheta; // Đã làm tròn từ đầu
|
|
float thetaDiff = Mathf.Abs(NormalizeAngle(roundedTheta - currentVdaTheta));
|
|
}
|
|
|
|
// Xử lý endNode
|
|
if (endNode != null)
|
|
{
|
|
bool shouldRotate = false;
|
|
string rotateReason = "";
|
|
|
|
// Kiểm tra xem endNode có cần xoay không
|
|
if (isLastEdge)
|
|
{
|
|
shouldRotate = true;
|
|
rotateReason = $"node cuối của edge cuối {edge.EdgeId}";
|
|
}
|
|
else if (intersectionNodeIds.Contains(endNode.NodeId))
|
|
{
|
|
shouldRotate = true;
|
|
rotateReason = $"node giao giữa đường thẳng và đường cong, NodeId={endNode.NodeId}";
|
|
}
|
|
|
|
if (shouldRotate)
|
|
{
|
|
float vdaTheta = endNode.NodePosition.Theta;
|
|
float roundedTheta = RoundThetaToStandardAngle(vdaTheta); // Làm tròn theta cho node giao hoặc node cuối
|
|
float thetaDiffEnd = Mathf.Abs(NormalizeAngle(roundedTheta - currentVdaTheta));
|
|
if (thetaDiffEnd > rotationThreshold)
|
|
{
|
|
currentVdaTheta = roundedTheta;
|
|
float unityEulerY = -roundedTheta * Mathf.Rad2Deg;
|
|
Quaternion targetRotation = Quaternion.Euler(0, unityEulerY, 0);
|
|
while (Quaternion.Angle(transform.rotation, targetRotation) > 0.1f)
|
|
{
|
|
transform.rotation = Quaternion.Slerp(transform.rotation, targetRotation, 0.07f);
|
|
currentTheta = NormalizeTheta(Mathf.Atan2(transform.right.x, transform.forward.z));
|
|
yield return null;
|
|
}
|
|
transform.rotation = targetRotation;
|
|
}
|
|
}
|
|
|
|
// Thực thi hành động tại endNode
|
|
if (endNode.Actions != null && endNode.Actions.Length > 0)
|
|
{
|
|
yield return monoBehaviour.StartCoroutine(actionExecutor.ExecuteNodeActions(endNode));
|
|
if (isCancelled) { isMoving = false; yield break; }
|
|
}
|
|
}
|
|
|
|
// Tắt chế độ đi lùi
|
|
if (isBackward)
|
|
{
|
|
isBackward = false;
|
|
Debug.Log("Tắt chế độ đi lùi khi kết thúc order: isBackward=false");
|
|
}
|
|
}
|
|
|
|
isMoving = false;
|
|
OnMoveCompleted?.Invoke();
|
|
}
|
|
|
|
private float NormalizeTheta(float eulerYDegrees)
|
|
{
|
|
float theta = eulerYDegrees * Mathf.Deg2Rad;
|
|
while (theta > Mathf.PI) theta -= 2 * Mathf.PI;
|
|
while (theta < -Mathf.PI) theta += 2 * Mathf.PI;
|
|
return theta;
|
|
}
|
|
|
|
private float NormalizeAngle(float angleRad)
|
|
{
|
|
while (angleRad > Mathf.PI) angleRad -= 2 * Mathf.PI;
|
|
while (angleRad < -Mathf.PI) angleRad += 2 * Mathf.PI;
|
|
return angleRad;
|
|
}
|
|
|
|
private List<Vector3> GenerateBSplinePoints(Trajectory trajectory, int resolution)
|
|
{
|
|
List<Vector3> points = new();
|
|
int degree = trajectory.Degree;
|
|
float[] knots = trajectory.KnotVector;
|
|
ControlPoint[] controlPoints = trajectory.ControlPoints;
|
|
|
|
if (controlPoints == null || controlPoints.Length < degree + 1 || knots == null || knots.Length < controlPoints.Length + degree + 1)
|
|
return points;
|
|
|
|
float minU = knots[degree];
|
|
float maxU = knots[controlPoints.Length];
|
|
if (minU >= maxU) return points;
|
|
|
|
float minDistance = 0.1f;
|
|
Vector3? lastPoint = null;
|
|
|
|
for (int i = 0; i <= resolution; i++)
|
|
{
|
|
float u = minU + (maxU - minU) * i / (float)resolution;
|
|
Vector2 point = CalculateBSplinePoint(u, degree, knots, controlPoints);
|
|
if (point != Vector2.zero)
|
|
{
|
|
Vector3 pos = new(point.x, transform.position.y, point.y);
|
|
if (lastPoint == null || Vector3.Distance(pos, lastPoint.Value) >= minDistance)
|
|
{
|
|
points.Add(pos);
|
|
lastPoint = pos;
|
|
}
|
|
}
|
|
}
|
|
|
|
return points;
|
|
}
|
|
|
|
private Vector2 CalculateBSplinePoint(float u, int degree, float[] knots, ControlPoint[] controlPoints)
|
|
{
|
|
Vector2 point = Vector2.zero;
|
|
int n = controlPoints.Length - 1;
|
|
|
|
for (int i = 0; i <= n; i++)
|
|
{
|
|
float basis = CalculateBasisFunction(i, degree, u, knots);
|
|
point += new Vector2(controlPoints[i].X, controlPoints[i].Y) * basis;
|
|
}
|
|
|
|
return point;
|
|
}
|
|
|
|
private float CalculateBasisFunction(int i, int degree, float u, float[] knots)
|
|
{
|
|
if (degree == 0)
|
|
return (u >= knots[i] && u < knots[i + 1]) ? 1f : 0f;
|
|
|
|
float left = 0f;
|
|
float right = 0f;
|
|
|
|
float denom1 = knots[i + degree] - knots[i];
|
|
if (denom1 > 0)
|
|
left = ((u - knots[i]) / denom1) * CalculateBasisFunction(i, degree - 1, u, knots);
|
|
|
|
float denom2 = knots[i + degree + 1] - knots[i + 1];
|
|
if (denom2 > 0)
|
|
right = ((knots[i + degree + 1] - u) / denom2) * CalculateBasisFunction(i + 1, degree - 1, u, knots);
|
|
|
|
return left + right;
|
|
}
|
|
} |