//
// Other calculations
//
//Calculate the length of an circle arc depending on which direction we are driving
//isLeftCircle should be true if we are reversing in a right circle
public float GetArcLength(
Vector3 circleCenterPos,
Vector3 startPos,
Vector3 goalPos,
OneReedsSheppSegment oneSegment)
{
//To get the arc length of each path we need to know which way we are turning
bool isLeftCircle = IsTurningLeft(oneSegment.isTurningLeft, oneSegment.isReversing);
Vector3 V1 = startPos - circleCenterPos;
Vector3 V2 = goalPos - circleCenterPos;
float theta = Mathf.Atan2(V2.z, V2.x) - Mathf.Atan2(V1.z, V1.x);
if (theta < 0f && isLeftCircle)
{
theta += 2f * Mathf.PI;
}
else if (theta > 0 && !isLeftCircle)
{
theta -= 2f * Mathf.PI;
}
float arcLength = Mathf.Abs(theta * turningRadius);
return(arcLength);
}
//Loops through segments of a path and add new coordinates to the final path
public void AddCoordinatesToPath(
ref Vector3 currentPos,
ref float theta,
List <Node> finalPath,
OneReedsSheppSegment segmentData)
{
//How far we are driving each update, the accuracy will improve if we lower the driveDistance
//But not too low because then the path will be less accurate because of rounding errors
float driveDistance = 0.02f;
//How many segments has this line?
int segments = Mathf.FloorToInt(segmentData.pathLength / driveDistance);
//Always add the first position manually
Node newNode = new Node();
newNode.carPos = currentPos;
newNode.heading = theta;
if (segmentData.isReversing)
{
newNode.isReversing = true;
}
finalPath.Add(newNode);
//Loop through all segments
//i = 1 because we already added the first coordinate
for (int i = 1; i < segments; i++)
{
//Can we improve the accuracy with Heuns method?
//Probably not because speed is constant
//Update the position
if (segmentData.isReversing)
{
currentPos.x -= driveDistance * Mathf.Sin(theta);
currentPos.z -= driveDistance * Mathf.Cos(theta);
}
else
{
currentPos.x += driveDistance * Mathf.Sin(theta);
currentPos.z += driveDistance * Mathf.Cos(theta);
}
//Don't update the heading if we are driving straight
if (segmentData.isTurning)
{
//Which way are we turning?
float turnParameter = -1f;
if (!segmentData.isTurningLeft)
{
turnParameter = 1f;
}
if (segmentData.isReversing)
{
turnParameter *= -1f;
}
//Update the heading
theta += (driveDistance / turningRadius) * turnParameter;
}
//Add the new position and heading to the final path
//Don't add all segments because 0.02 m per segment is too detailed
//The real drive distance in the Hybrid A* tree is 1.05, so 52 = 1.05 / 0.02
if (i % 52 == 0)
{
newNode = new Node();
newNode.carPos = currentPos;
newNode.heading = theta;
if (segmentData.isReversing)
{
newNode.isReversing = true;
}
finalPath.Add(newNode);
}
}
}