void DriveActuator(AI.Goal goal)
{
// min and max refers to the speed of the car not the size of the angle
float minForwardAngle = 120f * Mathf.Deg2Rad;
float maxForwardAngle = car.steerMax * Mathf.Deg2Rad / 4;
float minBackAngle = 120f * Mathf.Deg2Rad;
float maxBackAngle = 170f * Mathf.Deg2Rad;
float speedFactor = Mathf.Clamp01(car.speed * 0.036f - 0.2f); // scales from 20 to 120 kph
float backAngle = Mathf.Lerp(minBackAngle, maxBackAngle, speedFactor);
Vector3 backArc = car.transform.TransformDirection(new Vector3(Mathf.Sin(backAngle), 0f, Mathf.Cos(backAngle)));
Debug.DrawRay(car.transform.position, backArc, Color.cyan);
float forwardAngle = Mathf.Lerp(minForwardAngle, maxForwardAngle, speedFactor);
Vector3 forwardArc = car.transform.TransformDirection(new Vector3(Mathf.Sin(forwardAngle), 0f, Mathf.Cos(forwardAngle)));
Debug.DrawRay(car.transform.position, forwardArc, Color.cyan);
Vector2 forward2D = new Vector2(car.transform.forward.x, car.transform.forward.z).normalized;
Vector2 carPos2D = new Vector2(car.transform.position.x, car.transform.position.z);
ControlProxy control = new ControlProxy();
// New steer based on direction
Vector2 targetDir = (goal.position - carPos2D).normalized;
float goalAngle = Vector2.Angle(goal.direction, forward2D) * Mathf.Deg2Rad;
if (goalAngle < forwardAngle)
{
// targetAngle will try to match position with the goal
float targetAngle = Vector2.Angle(targetDir, forward2D) * Mathf.Deg2Rad;
Vector3 localTarget = car.transform.InverseTransformDirection(new Vector3(targetDir.x, 0f, targetDir.y));
control.steer = Mathf.Clamp(targetAngle * Mathf.Sign(localTarget.x) / (car.steerMax * Mathf.Deg2Rad), -1f, 1f); // note Mathf.Sign() is not Mathf.Sin()
control.throttle = Mathf.Max(0.25f, Mathf.Abs(localTarget.z));
if (car.speed < 0)
{
control.steer *= -1;
}
}
else if (goalAngle > backAngle)
{
if (debugUseBreak)
{
Debug.Break();
}
float targetAngle = Vector2.Angle(targetDir, forward2D) * Mathf.Deg2Rad;
Vector3 localTarget = car.transform.InverseTransformDirection(new Vector3(targetDir.x, 0f, targetDir.y));
control.steer = Mathf.Clamp(targetAngle * Mathf.Sign(localTarget.x) / (car.steerMax * Mathf.Deg2Rad), -1f, 1f); // note Mathf.Sign() is not Mathf.Sin()
if (car.speed < 5.0f)
{
control.steer *= -1;
control.throttle = 1.0f;
control.reverse = true;
}
else
{
control.throttle = 0.0f;
control.brake = 1.0f;
}
}
else
{
// steep turns will try to match direction with the goal not position
Vector2 velo2D = new Vector2(car.rigidbody.velocity.x, car.rigidbody.velocity.z);
float approachVelo = Vector2.Dot(velo2D, targetDir);
float distToNextCorner = (goal.cornerPosition - carPos2D).magnitude;
float turnScale = approachVelo / distToNextCorner;
Vector3 localTarget = car.transform.InverseTransformDirection(new Vector3(goal.direction.x, 0f, goal.direction.y));
control.steer = Mathf.Clamp(goalAngle * Mathf.Sign(localTarget.x) / (car.steerMax * Mathf.Deg2Rad) * turnScale, -1f, 1f);
control.throttle = 1f;
}
control.gear = car.currentGear;
if (car.engineRPM > car.autoShiftUp)
{
if (car.currentGear < car.gearRatios.Length - 1)
{
car.currentGear += 1;
}
}
else if (car.engineRPM < car.autoShiftDown)
{
if (car.currentGear > 1)
{
car.currentGear -= 1;
}
}
if (!debugUseAI)
{
return;
}
car.SetControls(control);
}
/* In this context a path param is a 1D measurement along the path.
* This calculates the nearest param on the path to position. currentParam is used
* so that the search will start at a specific spot on the path and continue
* consecutivly rather than search the whole path every time. */
public float GetNearestParam(ref AI.Goal goal, Vector2 position, float currentParam)
{
goal.hasPosition = true;
float segmentTotal = 0f;
int targetSegment = 0;
currentParam %= totalLength; // this just any overflow after each lap
if (currentParam < segmentLengths[0])
{
targetSegment = 0;
}
else
{
for (int i = 0; i < segmentLengths.Length; i++)
{
if (segmentTotal + segmentLengths[i] > currentParam)
{
targetSegment = i;
break;
}
segmentTotal += segmentLengths[i];
}
}
float shortestDistance2 = Mathf.Infinity; //shortest distance squared
goal.position = segments2D[targetSegment];
float newDistance2; // new distanceSquared
int searchSegment = targetSegment;
bool fullLap = false;
do
{
int nextSegment = searchSegment == segments2D.Length - 1 ? 0 : searchSegment + 1; //if current segment is the last segment then next is 0
Vector2 nearestPoint = NearestPointOnLine(position, segments2D[searchSegment], segments2D[nextSegment]);
newDistance2 = (nearestPoint - position).sqrMagnitude;
if (newDistance2 > shortestDistance2)
{
break;
}
if (nextSegment == 0)
{
fullLap = true;
}
targetSegment = searchSegment;
shortestDistance2 = newDistance2;
goal.position = nearestPoint;
searchSegment = nextSegment; //currentSegment == segments2D.Length - 1 ? 0 : currentSegment + 1;// increment unless it is the last segment then go to 0
} while (true);
float newParam = 0;
for (int i = 0; i < targetSegment; i++)
{
newParam += segmentLengths[i];
}
float sumOfSegments = newParam;
newParam += (goal.position - segments2D[targetSegment]).magnitude;
int directionSegment = targetSegment == segments2D.Length - 1 ? 0 : targetSegment + 1;
goal.hasDirection = true;
goal.direction = (segments2D[directionSegment] - segments2D[targetSegment]).normalized;
int nextDirectionSegment = directionSegment == segments2D.Length - 1 ? 0 : directionSegment + 1;
Vector2 nextDirection = (segments2D[nextDirectionSegment] - segments2D[directionSegment]).normalized;
goal.hasCornerWarning = true;
goal.cornerPosition = segments2D[directionSegment];
goal.cornerAngle = 1 - Vector2.Angle(goal.direction, nextDirection) / 90f;
if (newParam >= currentParam)
{
// moving forward normally. use position calculated above
return(newParam);
}
else if (fullLap)
{
// moving forward edge case where hte new point is just past the zero point in the track. Use position from above.
return(newParam);
}
else
{
// moving backwards in not allowed. Recalculate position based on param passed in.
float partialSegment = currentParam - sumOfSegments;
goal.position = segments2D[targetSegment] + goal.direction * partialSegment;
return(currentParam);
}
}
