private void UpdateSteeringDecisionLogic()
{
//Debug.Log(targetThrottle);
//Debug.Log(motor);
if (currentPath.ActualPathExists())
{
//currentPath.SetFirstWaypoint();
var dir = Vector3.ProjectOnPlane(currentPath.NextActualWaypoint - transform.position, transform.up);
float distance = dir.magnitude;
if (distance > maxDistanceBeforeResettingToClosestWp)
{
currentPath.SetWaypointClosestToPosition(transform.position);
dir = Vector3.ProjectOnPlane(currentPath.NextActualWaypoint - transform.position, transform.up);
distance = dir.magnitude;
}
//float mar = (!GoingBackwards) ? maxAngleReverse : maxAngleNoReverse;
//float manor = (!GoingBackwards) ? maxAngleNoReverse : maxAngleReverse;
float mar = maxAngleReverse;
float manor = maxAngleNoReverse;
float angle;
angle = Vector3.SignedAngle(forward, dir, transform.up);
float abs180Angle = Mathf.Abs(angle);
if (GoingBackwards)
{
abs180Angle = 180 - abs180Angle;
}
var r = Mathf.Sin(Mathf.Deg2Rad * Mathf.Min(abs180Angle, 100 /*180 - 2 * mar*/));
float turnRadius = Mathf.Lerp(reversingRadiusLowBound, reversingRadiusHighBound, r);
if (reversing == 0)
{
if (GoingBackwards && abs180Angle > 2 * mar && distance > reversingRadiusLowBound)
{
SetSteeringLogicState(2, 0, 1);
currentPath.SetWaypointClosestToPosition(transform.position);
}
else if ((distance <= proximityDistance) || (distance < turnRadius /*reversingRadiusLowBound*/ && abs180Angle < 2 * manor))
{
if (currentPath.IsNextWaypointLast)
{
SetSteeringLogicState(0, 0, 1);
}
else
{
SetSteeringLogicState(0, throttle, 0.5f * (velocityMagnitude / maxVelocity) * (1 + 0.5f * (angularVelocityMagnitude / maxAngularVelocity)));
}
currentPath.ArrivedAtWaypoint();
}
else if (distance < turnRadius && abs180Angle > 2 * manor)
{
SetSteeringLogicState(1, 0, 1);
}
}
else if (reversing == 1)
{
if (distance <= proximityDistance || (distance < turnRadius /*reversingRadiusLowBound*/ && (abs180Angle < 2 * mar || 180 - abs180Angle < 2 * mar)))
{
if (currentPath.IsNextWaypointLast)
{
SetSteeringLogicState(1, 0, 1);
}
else
{
SetSteeringLogicState(1, throttle, 0.5f * (velocityMagnitude / maxVelocity) * (1 + 0.5f * (angularVelocityMagnitude / maxAngularVelocity)));
}
currentPath.ArrivedAtWaypoint();
}
else if (distance > turnRadius)
{
if (abs180Angle < 2 * manor)
{
SetSteeringLogicState(0, 0, 1);
}
}
}
else if (reversing == 2)
{
if (distance <= proximityDistance || (distance < turnRadius /*reversingRadiusLowBound*/ && (abs180Angle < 2 * mar || 180 - abs180Angle < 2 * mar)))
{
if (currentPath.IsNextWaypointLast)
{
SetSteeringLogicState(2, 0, 1);
}
else
{
SetSteeringLogicState(2, throttle, 0.5f * (velocityMagnitude / maxVelocity) * (1 + 0.5f * (angularVelocityMagnitude / maxAngularVelocity)));
}
currentPath.ArrivedAtWaypoint();
}
else if (distance > turnRadius && abs180Angle < 2 * mar)
{
SetSteeringLogicState(0, 0, 1);
}
}
}
}
private void UpdatePathtrackingLogic()
{
if (!pausedCourse && currentPath.ActualPathExists())
{
//currentPath.SetFirstWaypoint();
//float turnRadius = Mathf.Clamp(mainRigidBody.velocity.magnitude/maxAngularVelocity, halfWidth*2, maxVelocity/maxAngularVelocity);
var tratio = (throttle > 0) ? throttle / maxThrottle : -throttle / minThrottle;
float turnRadius = Mathf.Lerp(turnRadiusLowBound, turnRadiusHighBound, tratio);
var dir = Vector3.ProjectOnPlane(currentPath.NextActualWaypoint - transform.position, transform.up);
float distance = dir.magnitude;
if (distance > maxDistanceBeforeResettingToClosestWp)
{
currentPath.SetWaypointClosestToPosition(transform.position);
dir = Vector3.ProjectOnPlane(currentPath.NextActualWaypoint - transform.position, transform.up);
distance = dir.magnitude;
}
float angle;
bool isWpLast = currentPath.IsNextWaypointLast;
if (!isWpLast)
{
angle = Vector3.SignedAngle(transform.forward, dir, transform.up);
}
else
{
angle = Vector3.SignedAngle(transform.forward, currentPath.DesiredOrientationAtLastWaypoint, transform.up);
goingBackwards = false;
}
if (!reversing)
{
if ((distance <= proximityDistance) || (distance < turnRadiusLowBound && Mathf.Abs(angle) < 2 * maxAngleNoReverse))
{
if (!isWpLast)
{
currentPath.orientingAtLastWaypoint = false;
if (currentPath.ArrivedAtWaypoint())
{
throttle = 0; //if the waypoint is the first or second last
}
}
else if (distance <= proximityDistance * 2 && Mathf.Abs(angle) < angleMarginOrientationAtLastWp)
{
currentPath.orientingAtLastWaypoint = false;
currentPath.ArrivedAtWaypoint();
throttle = 0;
}
else
{
currentPath.orientingAtLastWaypoint = true;
reversing = true;
throttle = 0;
}
}
else if (distance < turnRadius && Mathf.Abs(angle) > 2 * maxAngleNoReverse)
{
currentPath.orientingAtLastWaypoint = isWpLast;
reversing = true;
throttle = 0;
}
}
else
{
if (distance <= proximityDistance || (distance < turnRadiusLowBound && (Mathf.Abs(angle) < maxAngleReverse || 180 - Mathf.Abs(angle) < 2 * maxAngleReverse)))
{
if (!isWpLast)
{
currentPath.orientingAtLastWaypoint = false;
if (currentPath.ArrivedAtWaypoint())
{
throttle = 0; //if the waypoint is the first or second last
}
}
else if (distance <= proximityDistance * 2 && Mathf.Abs(angle) < angleMarginOrientationAtLastWp * (maxAngleReverse / maxAngleNoReverse))
{
currentPath.orientingAtLastWaypoint = false;
currentPath.ArrivedAtWaypoint();
throttle = 0;
}
else
{
currentPath.orientingAtLastWaypoint = isWpLast;
reversing = false;
throttle = 0;
}
}
else if (distance > turnRadius && Mathf.Abs(angle) < 2 * maxAngleReverse)
{
currentPath.orientingAtLastWaypoint = false;
reversing = false;
throttle = 0;
}
}
}
}