// TODO: add functions to get and modify preferredDistance
public Vector3 GetForce(Steering steering)
{
Vector3 steeringVector = new Vector3(0f, 0f, 0f);
// steer away from each object that is too close with a weight of up to 0.5 for each
foreach (Neighbour <Steering> neighbour in neighbours)
{
if (neighbour.dd > preferredDistance * preferredDistance)
{
break;
}
Steering otherUnit = neighbour.obj;
Vector3 offset = otherUnit.GetPosition() - steering.GetPosition();
float currentDistance = Mathf.Sqrt(neighbour.dd);
// TODO: consider relative velocity when computing importance
float importance = (preferredDistance - currentDistance) / preferredDistance;
steeringVector += -importance * offset;
}
if (steeringVector.sqrMagnitude > 0)
{
// TODO: do something like arrival to avoid over-separating
return(SteeringUtilities.getForceForDirection(steering, steeringVector));
}
return(Vector3.zero);
}
public Vector3 GetForce(Steering steering)
{
return(SteeringUtilities.getSeekForce(steering, target));
// TODO: actually use stopping distance to arrive
/*
* // can approximate with (if too close : break)
* // alternatively stopdist from current pos and current speed
* // or expected stopdist after the current frame assuming accel wont change the current frame much
*
* // for the duration it takes to go from maxV to 0
* //stoptime = v/ACCEL;
* //stopdist = stoptime * v/2;
* //dist = desiredv * desiredV/(2 * accel)
* //desiredv = sqrt(dist * 2 * accel)
*
* Vector3 offset = dest - ((Vector3) transform.position + (dt * rb.velocity));
*
* float dist = offset.magnitude;
* float stopdist = MAXV * MAXV / (2 * ACCEL);
* float desiredV = dist < stopdist ? Mathf.Sqrt(dist * 2 * ACCEL) : MAXV;
* Vector3 deltaV = scaled (desiredV, offset) - rb.velocity;
* float dvmagn = deltaV.magnitude;
* if (dvmagn > forceRemaining * forceRemaining * dt * dt) {
* rb.AddForce(scaled(forceRemaining, deltaV));
* forceRemaining = 0f;
* } else {
* rb.AddForce(deltaV);
* forceRemaining -= dvmagn;
* }
*/
}
public Vector3 GetForce(Steering steering)
{
if (steering.GetVelocity().sqrMagnitude == 0f)
{
float initialAngle = Random.Range(0f, 2 * Mathf.PI);
float sinAngle = Mathf.Sin(initialAngle);
wanderPoint = 2.4f * new Vector3(Mathf.Cos(initialAngle), Steering.YMult * sinAngle, Steering.ZMult * sinAngle);
}
if (steering.GetVelocity().sqrMagnitude > 0f && !hasMoved)
{
hasMoved = true;
wanderPoint = SteeringUtilities.scaledVector(2.4f, steering.GetVelocity());
}
float xNoise = Time.fixedDeltaTime * wanderNoise * Random.Range(-1f, 1f);
float yzNoise = Time.fixedDeltaTime * wanderNoise * Random.Range(-1f, 1f);
wanderPoint += new Vector3(xNoise, Steering.YMult * yzNoise, Steering.ZMult * yzNoise);
Vector3 forwardPoint = steering.GetPosition() + SteeringUtilities.scaledVector(1.41f, steering.GetVelocity());
// Constrain the wander point to the unit circle in front of the player.
wanderPoint = forwardPoint + (wanderPoint - forwardPoint).normalized;
//return SteeringUtilities.getForceForDirection(steering, wanderDirection);
//SteeringUtilities.drawDebugCircle(forwardPoint, 1f, Color.black, 32);
SteeringUtilities.drawDebugPoint(wanderPoint, Color.red);
return(SteeringUtilities.getSeekForce(steering, wanderPoint));
}
public Vector3 GetForce(Steering steering)
{
if (_target != null)
{
return(SteeringUtilities.getSeekForce(steering, _target.GetPosition()));
}
else
{
return(SteeringUtilities.getSeekForce(steering, _point));
}
}
public Vector3 GetForce(Steering steering)
{
if (isResponsibleForNeighbourUpdate)
{
// TODO: figure out how to add or remove neighbours automatically here or in neighbours
neighbours.Update();
}
/*
* Avoid collisions by determining for each neighbor when their paths will be closest to each other
* and then steer laterally to avoid collision.
* https://www.red3d.com/cwr/steer/Unaligned.html
*/
float distanceToBeginReacting = 4f * (steering.GetSize() + steering.GetStoppingDistance());
//Debug.Log(doubleStopDistance);
foreach (Neighbour <Steering> neighbour in neighbours)
{
if (neighbour.dd > distanceToBeginReacting * distanceToBeginReacting)
{
break;
}
Steering otherUnit = neighbour.obj;
Vector3 offset = otherUnit.GetPosition() - steering.GetPosition();
Vector3 relativeVelocity = steering.GetVelocity() - otherUnit.GetVelocity();
// Decrease the timeToCollision so that closestOffset is nonZero.
float combinedSize = steering.GetSize() + otherUnit.GetSize();
float timeToCollision = (offset.magnitude - combinedSize) / SteeringUtilities.parallelComponent(relativeVelocity, offset).magnitude;
if (timeToCollision > 2 * steering.GetStoppingTime())
{
continue;
}
Vector3 closestOffset = (offset - (timeToCollision * relativeVelocity));
float preferredDistance = 1.5f * combinedSize;
if (closestOffset.sqrMagnitude > preferredDistance * preferredDistance)
{
continue;
}
SteeringUtilities.drawDebugVector(steering, timeToCollision * steering.GetVelocity(), Color.cyan);
SteeringUtilities.drawDebugPoint(steering.GetPosition() + timeToCollision * steering.GetVelocity(), Color.cyan);
SteeringUtilities.drawDebugVector(otherUnit, timeToCollision * otherUnit.GetVelocity(), Color.cyan);
SteeringUtilities.drawDebugPoint(otherUnit.GetPosition() + timeToCollision * otherUnit.GetVelocity(), Color.cyan);
// TODO: for head-on collisions steer to the right
// Steer in the direction of the component of the collision normal that is perpindicular to the current velocity.
// This way the unit will turn instead of just slowing down.
// TODO: use an amount of acceleration proportionate to the time until collision and the severity of the collision
return(SteeringUtilities.scaledVector(steering.GetAcceleration(), SteeringUtilities.perpindicularComponent(-closestOffset, steering.GetVelocity())));
//return SteeringUtilities.getForceForDirection(steering, -closestOffset);
}
return(Vector3.zero);
}
public static ISteeringCommandGenerator SparcVote(ref double prevCurvature, Coordinates relativeGoalPoint, List <Polygon> perimeterPolygons)
{
double newCurvature = FindBestCurvature(prevCurvature, relativeGoalPoint, perimeterPolygons);
prevCurvature = newCurvature;
if (double.IsNaN(newCurvature))
{
return(null);
}
else
{
return(new ConstantSteeringCommandGenerator(SteeringUtilities.CurvatureToSteeringWheelAngle(newCurvature, 2), false));
}
}
public static ISteeringCommandGenerator SparcVote(ref double prevCurvature, Coordinates goalPoint)
{
double newCurvature = FindBestCurvature(prevCurvature, goalPoint);
prevCurvature = newCurvature;
if (double.IsNaN(newCurvature))
{
return(null);
}
else
{
return(new ConstantSteeringCommandGenerator(SteeringUtilities.CurvatureToSteeringWheelAngle(newCurvature, 2), false));
}
}
private RaycastHit2D Raycast2D(Steering steering, Vector3 directionVector, float raycastDistance)
{
RaycastHit2D hitInfo = Physics2D.Raycast(steering.GetPosition(), directionVector, raycastDistance, layerMask);
//SteeringUtilities.drawDebugVector(steering, SteeringUtilities.scaledVector(raycastDistance, directionVector), Color.white);
if (hitInfo.collider != null)
{
SteeringUtilities.drawDebugPoint(hitInfo.point, Color.magenta);
SteeringUtilities.drawDebugVector(steering, (Vector3)hitInfo.point - steering.GetPosition(), Color.magenta);
SteeringUtilities.drawDebugVector((Vector3)hitInfo.point, hitInfo.normal, Color.white);
}
else
{
SteeringUtilities.drawDebugVector(steering, SteeringUtilities.scaledVector(raycastDistance, directionVector), new Color(Color.magenta.r, Color.magenta.g, Color.magenta.b, 0.25f));
}
return(hitInfo);
}
private void TurnToward(float desiredAngle)
{
// TODO: verify that the sign of these numbers is correct for both XY and XZ
float currentAngle = (ZMult * transform.localEulerAngles.y) + (YMult * transform.localEulerAngles.z);
float angleDiff = SteeringUtilities.angleDiff(currentAngle, desiredAngle);
float rotationPerFrame = turnRate * Time.fixedDeltaTime;
if (angleDiff > rotationPerFrame)
{
float newAngle = currentAngle + rotationPerFrame;
transform.localEulerAngles = new Vector3(0, ZMult * newAngle, YMult * newAngle);
}
else if (angleDiff < -rotationPerFrame)
{
float newAngle = currentAngle - rotationPerFrame;
transform.localEulerAngles = new Vector3(0, ZMult * newAngle, YMult * newAngle);
}
else
{
transform.localEulerAngles = new Vector3(0, ZMult * desiredAngle, YMult * desiredAngle);
}
}
// TODO: call GetForce for each behaviour and provide a reference to the Steering object, or provide MAXV ACCEL etc.
// TODO: call rb.AddForce on the weighted sum of the available forces. Also if a behaviour provides a force of size < accel, it counts less towards the total weight
public void FixedUpdate()
{
float totalWeight = 0f;
Vector3 totalForce = new Vector3();
int i = 0;
foreach (KeyValuePair <SteeringBehaviour, float> behaviourAndWeight in weightedBehaviours)
{
Vector3 behaviourForce = behaviourAndWeight.Key.GetForce(this);
totalForce += behaviourAndWeight.Value * behaviourForce;
totalWeight += behaviourAndWeight.Value * behaviourForce.magnitude / acceleration;
// TODO: define a mapping from behaviour-type to color, and provide some way to only draw lines for some behaviours
SteeringUtilities.drawDebugVector(this, 0.1f * behaviourForce, BEHAVIOUR_COLORS[i++ % BEHAVIOUR_COLORS.Length]);
}
// TODO: consider averaging the desired velocities instead of forces
if (totalWeight > 0f)
{
//SteeringUtilities.drawDebugVector(this, 0.1f * totalForce / totalWeight, Color.blue);
// TODO: scale the force with the object's mass
AddForce(totalForce / totalWeight);
}
// Enforce a maximum speed
float velocitySquared = GetVelocity().sqrMagnitude;
if (velocitySquared > maxSpeed * maxSpeed)
{
SetVelocity(SteeringUtilities.scaledVector(maxSpeed, GetVelocity()));
}
// Turn toward the current velocity (so that the x axis is forward)
if (turnAutomatically && velocitySquared > 0.5)
{
TurnToward(SteeringUtilities.angleForVector(GetVelocity()));
}
if (velocitySquared > 0f)
{
_movingDirection = GetVelocity();
}
//SteeringUtilities.drawDebugVector(this, (0.5f * getVelocity()), VELOCITY_COLOR);
}
private void ExecutePass()
{
// determine what to do
// get the moving order
AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
curTimestamp = pose.timestamp;
ParkerVehicleState state = new ParkerVehicleState(pose.xy, Coordinates.FromAngle(pose.heading));
if (pulloutMode)
{
movingOrder = parker.GetNextPulloutOrder(state);
}
else
{
movingOrder = parker.GetNextParkingOrder(state);
}
// test for completion
bool isPullinCompleted = false;
if (parkingSpotLine.P0.DistanceTo(pose.xy) < 1.5 && Coordinates.FromAngle(pose.heading).Dot(parkingSpotLine.UnitVector) >= 0.5)
{
isPullinCompleted = true;
}
finalPass = false;
if (movingOrder != null && movingOrder.DestState != null && parkingSpotLine.P0.DistanceTo(movingOrder.DestState.Loc) < 1.5 && parkingSpotLine.UnitVector.Dot(movingOrder.DestState.Heading) >= 0.5)
{
finalPass = true;
}
if (movingOrder.Completed || (isPullinCompleted && !pulloutMode))
{
Type type;
if (pulloutMode)
{
type = typeof(ZoneParkingPullOutBehavior);
}
else
{
type = typeof(ZoneParkingBehavior);
}
Services.BehaviorManager.ForwardCompletionReport(new SuccessCompletionReport(type));
Services.BehaviorManager.Execute(new HoldBrakeBehavior(), null, false);
}
else
{
// determine the stopping distance
TransmissionGear targetGear;
if (movingOrder.Forward)
{
targetGear = TransmissionGear.First;
}
else
{
targetGear = TransmissionGear.Reverse;
}
bool counterClockwise = false;
if ((movingOrder.Forward && movingOrder.TurningRadius > 0) || (!movingOrder.Forward && movingOrder.TurningRadius < 0))
{
counterClockwise = true;
}
else
{
counterClockwise = false;
}
double radius = pose.xy.DistanceTo(movingOrder.CenterPoint);
double startAngle = (pose.xy - movingOrder.CenterPoint).ArcTan;
double endAngle = (movingOrder.DestState.Loc - movingOrder.CenterPoint).ArcTan;
if (counterClockwise)
{
if (endAngle < startAngle)
{
endAngle += 2 * Math.PI;
}
}
else
{
if (endAngle > startAngle)
{
endAngle -= 2 * Math.PI;
}
}
double stopDistance;
if (Math.Abs(radius) < 100)
{
stopDistance = Math.Abs(radius * (endAngle - startAngle));
}
else
{
stopDistance = pose.xy.DistanceTo(movingOrder.DestState.Loc);
}
this.stopDist = stopDistance;
this.stopTimestamp = curTimestamp;
Services.UIService.PushPoint(movingOrder.DestState.Loc, curTimestamp, "uturn stop point", false);
//double sign = movingOrder.Forward ? 1 : -1;
double steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(1 / movingOrder.TurningRadius, parking_speed);
ISpeedCommandGenerator shiftSpeedCommand = new ShiftSpeedCommand(targetGear);
ISteeringCommandGenerator initialSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, steeringRate, true);
ISpeedCommandGenerator passSpeedCommand = new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new TravelledDistanceProvider(curTimestamp, stopDistance), parking_speed));
ISteeringCommandGenerator passSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, null, false);
ChainedTrackingCommand cmd = new ChainedTrackingCommand(
new TrackingCommand(shiftSpeedCommand, initialSteeringCommand, true),
new TrackingCommand(passSpeedCommand, passSteeringCommand, false));
cmd.Label = command_label;
Services.TrackingManager.QueueCommand(cmd);
}
passCompleted = false;
}
public Vector3 GetForce(Steering steering)
{
return(SteeringUtilities.getForceForDesiredVelocity(steering, Vector3.zero));
}
public Vector3 GetForce(Steering steering)
{
return(SteeringUtilities.getForceForDesiredVelocity(steering, SteeringUtilities.scaledVector(steering.GetMaxSpeed(), steering.GetDirection())));
}
public Vector3 GetForce(Steering steering)
{
Vector3 currentOffset = target.GetPosition() - steering.GetPosition();
float dist = currentOffset.magnitude;
Vector3 unitV = steering.GetVelocity().normalized;
float parallelness = Vector3.Dot(unitV, target.GetVelocity().normalized);
float forwardness = Vector3.Dot(unitV, currentOffset / dist);
float halfsqrt2 = 0.707f;
int f = SteeringUtilities.intervalComp(forwardness, -halfsqrt2, halfsqrt2);
int p = SteeringUtilities.intervalComp(parallelness, -halfsqrt2, halfsqrt2);
// approximate how far to lead the target
float timeFactor = 1f;
// case logic based on (ahead, aside, behind) X (parallel, perp, anti-parallel)
switch (f)
{
case 1: //target is ahead
switch (p)
{
case 1:
timeFactor = 4f;
break;
case 0:
timeFactor = 1.8f;
break;
case -1:
timeFactor = 0.85f;
break;
}
break;
case 0: //target is aside
switch (p)
{
case 1:
timeFactor = 1f;
break;
case 0:
timeFactor = 0.8f;
break;
case -1:
timeFactor = 4f;
break;
}
break;
case -1: //target is behind
switch (p)
{
case 1:
timeFactor = 0.5f;
break;
case 0:
timeFactor = 2f;
break;
case -1:
timeFactor = 2f;
break;
}
break;
}
// Multiply the timeToArrive by some approximate constants based on how similar the two velocities are.
float approximateArrivalTime = dist / steering.GetMaxSpeed();
float improvedArrivalTimeEstimate = Mathf.Min(MAX_PREDICTION_TIME, approximateArrivalTime * timeFactor);
Vector3 newTargetPosition = (Vector3)target.GetPosition() + improvedArrivalTimeEstimate * target.GetVelocity();
SteeringUtilities.drawDebugVector(target, newTargetPosition - target.GetPosition(), Color.white);
SteeringUtilities.drawDebugVector(steering, newTargetPosition - steering.GetPosition(), Color.magenta);
return(SteeringUtilities.getForceForDirection(steering, newTargetPosition - steering.GetPosition()));
}
private ITrackingCommand BuildForwardPass(out bool finalPass)
{
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "UTurn Behavior: Determine Forward Pass");
// rotate the polygon into the current relative frame
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
RelativeTransform relTransform = Services.RelativePose.GetTransform(polygonTimestamp, curTimestamp);
relTransform.TransformPointsInPlace(polygon);
finalOrientation = finalOrientation.Transform(relTransform);
polygonTimestamp = curTimestamp;
// retrieve the vehicle state
Coordinates headingVec = new Coordinates(1, 0);
Coordinates headingVec90 = headingVec.Rotate90();
// check if we can make it out now
Line curLine = new Line(new Coordinates(0, 0), headingVec);
Coordinates intersectionPoint;
LineSegment finalLine = finalOrientation;
Circle outCircle = Circle.FromLines(curLine, (Line)finalLine, out intersectionPoint);
double steeringCommand;
if (!outCircle.Equals(Circle.Infinite) && outCircle.r > minRadius)
{
// we found an out circle
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "found final pass output");
// build the circle segment
double hitAngle = (intersectionPoint - outCircle.center).ArcTan;
double startAngle = (-outCircle.center).ArcTan;
if (hitAngle < startAngle)
{
hitAngle += 2 * Math.PI;
}
hitAngle -= startAngle;
if (stopOnLine)
{
// get the angle of the end point of the line segment
double endPointAngle = (finalLine.P1 - outCircle.center).ArcTan;
if (endPointAngle < startAngle)
{
endPointAngle += 2 * Math.PI;
}
endPointAngle -= startAngle;
if (endPointAngle < hitAngle)
{
hitAngle = endPointAngle;
}
}
// get the obstacles
Coordinates frontLeftPoint = headingVec * TahoeParams.FL + headingVec90 * TahoeParams.T / 2;
Coordinates frontRightPoint = headingVec * TahoeParams.FL - headingVec90 * TahoeParams.T / 2.0;
Coordinates rearRightPoint = -headingVec * TahoeParams.RL - headingVec90 * TahoeParams.T / 2.0;
List <Polygon> obstacles = GetObstacles(curTimestamp);
GetObstacleHitAngle(frontLeftPoint, outCircle.center, obstacles, ref hitAngle);
GetObstacleHitAngle(frontRightPoint, outCircle.center, obstacles, ref hitAngle);
GetObstacleHitAngle(rearRightPoint, outCircle.center, obstacles, ref hitAngle);
// calculate stopping distance
stopDistance = outCircle.r * hitAngle;
stopTimestamp = curTimestamp;
curvature = 1 / outCircle.r;
intersectionPoint = outCircle.center + Coordinates.FromAngle(startAngle + hitAngle) * outCircle.r;
// calculate steering angle
steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(1 / outCircle.r, uturnSpeed);
// mark that this will be the final pass
finalPass = true;
Services.UIService.PushCircle(outCircle, curTimestamp, "uturn circle", true);
Services.UIService.PushPoint(intersectionPoint, curTimestamp, "uturn stop point", true);
Services.UIService.PushPolygon(polygon, curTimestamp, "uturn polygon", true);
}
else
{
finalPass = false;
// draw out 3 circles
// - front right wheel
// - front left wheel
// - rear left wheel
// figure out center point of turn
Circle rearAxleCircle = Circle.FromPointSlopeRadius(Coordinates.Zero, headingVec, minRadius);
Coordinates center = rearAxleCircle.center;
// calculate the points of the wheels
Coordinates rearLeftPoint = headingVec90 * TahoeParams.T / 2;
Coordinates rearRightPoint = -headingVec90 * TahoeParams.T / 2;
Coordinates frontLeftPoint = headingVec * TahoeParams.L + headingVec90 * TahoeParams.T / 2;
Coordinates frontRightPoint = headingVec * TahoeParams.L - headingVec90 * TahoeParams.T / 2;
// initialize min hit angle to slightly less than 90 degrees
double minHit = Math.PI / 2.1;
//GetMinHitAngle(rearLeftPoint, center, true, ref minHit);
//GetMinHitAngle(rearRightPoint, center, true, ref minHit);
GetMinHitAngle(frontLeftPoint, center, ref minHit);
GetMinHitAngle(frontRightPoint, center, ref minHit);
// get the obstacles
List <Polygon> obstacles = GetObstacles(curTimestamp);
frontLeftPoint = headingVec * TahoeParams.FL + headingVec90 * TahoeParams.T / 2;
frontRightPoint = headingVec * TahoeParams.FL - headingVec90 * TahoeParams.T / 2.0;
rearRightPoint = -headingVec * TahoeParams.RL - headingVec90 * TahoeParams.T / 2.0;
GetObstacleHitAngle(frontLeftPoint, center, obstacles, ref minHit);
GetObstacleHitAngle(frontRightPoint, center, obstacles, ref minHit);
GetObstacleHitAngle(rearRightPoint, center, obstacles, ref minHit);
// trim some off the hit for safety
//if (minHit > 0.5/minRadius)
minHit -= (0.5 / minRadius);
minHit = Math.Max(minHit, 0.6 / minRadius);
double startAngle = (Coordinates.Zero - center).ArcTan;
double hitAngle = startAngle + minHit;
// set the stopping point at the min hit point
Coordinates stopPoint = rearAxleCircle.GetPoint(hitAngle);
// calculate the stop distance
stopDistance = minRadius * minHit;
// calculate the required steering angle
steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(1 / minRadius, uturnSpeed);
Services.UIService.PushCircle(new Circle(minRadius, center), curTimestamp, "uturn circle", true);
Services.UIService.PushPoint(stopPoint, curTimestamp, "uturn stop point", true);
Services.UIService.PushPolygon(polygon, curTimestamp, "uturn polygon", true);
}
// build the command
ISpeedCommandGenerator shiftSpeedCommand = new ShiftSpeedCommand(TransmissionGear.First);
ISteeringCommandGenerator initialSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, steeringRate, true);
ISpeedCommandGenerator passSpeedCommand = new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new TravelledDistanceProvider(curTimestamp, stopDistance), uturnSpeed));
ISteeringCommandGenerator passSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, null, false);
ChainedTrackingCommand cmd = new ChainedTrackingCommand(
new TrackingCommand(shiftSpeedCommand, initialSteeringCommand, true),
new TrackingCommand(passSpeedCommand, passSteeringCommand, false));
cmd.Label = forwardLabel;
return(cmd);
}
public Vector3 GetForce(Steering steering)
{
float raycastDistance = 2f * (2f * steering.GetSize()) + 2f * (steering.GetMaxSpeed() * steering.GetMaxSpeed() / steering.GetAcceleration());
// TODO: send out 3 rays and define static quaternions to determine the rotated direction vectors.
Vector3 directionVector = steering.GetVelocity();
// TODO: update the side raycast lengths based on steering's size, and the center raycast based on speed.
// TODO: constrain hit normals to the relevant plane in case the collider has a complicated shape
Vector3 hitNormal1 = RaycastNormal(steering, (Steering.UseXZ ? xzRotateLeft : xyRotateLeft) * directionVector, raycastDistance * 0.5f);
Vector3 hitNormal2 = RaycastNormal(steering, directionVector, raycastDistance);
Vector3 hitNormal3 = RaycastNormal(steering, (Steering.UseXZ ? xzRotateRight : xyRotateRight) * directionVector, raycastDistance * 0.5f);
// If multiple raycasts hit, sum the normals.
// TODO: weight the normals differently based on which collision point is closest.
Vector3 combinedNormal = SteeringUtilities.scaledDownVector(1f, hitNormal1 + hitNormal2 + hitNormal3);
if (combinedNormal.sqrMagnitude > 0f)
{
// For the normal of the wall ahead, steer to have a velocity that is perpindicular to it.
Vector3 leftVector = new Vector3(combinedNormal.y + combinedNormal.z, -Steering.YMult * combinedNormal.x, -Steering.ZMult * combinedNormal.x);
Vector3 rightVector = new Vector3((-combinedNormal.y) - combinedNormal.z, Steering.YMult * combinedNormal.x, Steering.ZMult * combinedNormal.x);
float rayLeftDistance = RaycastDistance(steering, leftVector, raycastDistance);
float rayRightDistance = RaycastDistance(steering, rightVector, raycastDistance);
// TODO: Consider updating the logic when approaching a corner.
// Currently the unit kind of turns toward the side that they are coming from even if the other turn is shorter.
// Case 1: Wall in front, wall on left, wall on right.
if (rayLeftDistance < raycastDistance && rayRightDistance < raycastDistance)
{
// Move towards the left/right wall that is closer. That should be moving away from the corner point.
if (rayLeftDistance < rayRightDistance)
{
return(SteeringUtilities.getForceForDirection(steering, leftVector));
}
else // rayRightDistance < rayLeftDistance
{
return(SteeringUtilities.getForceForDirection(steering, rightVector));
}
}
// Case 2: Wall in front, wall on left
else if (rayLeftDistance < raycastDistance)
{
return(SteeringUtilities.getForceForDirection(steering, rightVector));
}
// Case 3: Wall in front, wall on right
else if (rayRightDistance < raycastDistance)
{
return(SteeringUtilities.getForceForDirection(steering, leftVector));
}
// Case 4: Wall in front
else
{
// Move towards whichever of left or right is closer to the current velocity
Vector3 perpindicularVector = SteeringUtilities.perpindicularComponent(combinedNormal, steering.GetVelocity());
// Edge case: the normal is parallel to velocity. In this case, pick one of the two perpindicular vectors at random.
if (perpindicularVector == Vector3.zero)
{
Debug.Log("Exact perpindicular!");
int randomSign = Random.value < .5 ? 1 : -1;
perpindicularVector = new Vector3((-combinedNormal.y) - combinedNormal.z, Steering.YMult * combinedNormal.x * randomSign, Steering.ZMult * combinedNormal.x * randomSign);
}
return(SteeringUtilities.getForceForDirection(steering, perpindicularVector));
}
}
return(Vector3.zero);
}
public Vector3 GetForce(Steering steering)
{
return(-SteeringUtilities.getSeekForce(steering, target));
}
private ITrackingCommand BuildBackwardPass()
{
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "UTurn Behavior: Initialize Backward Pass");
// rotate the polygon into the current relative frame
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
RelativeTransform relTransform = Services.RelativePose.GetTransform(polygonTimestamp, curTimestamp);
relTransform.TransformPointsInPlace(polygon);
finalOrientation = finalOrientation.Transform(relTransform);
polygonTimestamp = curTimestamp;
// retrieve the vehicle state
Coordinates headingVec = new Coordinates(1, 0);
Coordinates headingVec180 = headingVec.Rotate180();
Coordinates headingVec90 = headingVec.Rotate90();
// figure out center point of turn
Circle rearAxleCircle = Circle.FromPointSlopeRadius(new Coordinates(0, 0), headingVec180, minRadius);
Coordinates center = rearAxleCircle.center;
// calculate the points of the wheels
Coordinates rearLeftPoint = headingVec90 * TahoeParams.T / 2;
Coordinates rearRightPoint = -headingVec90 * TahoeParams.T / 2;
Coordinates frontLeftPoint = headingVec * TahoeParams.L + headingVec90 * TahoeParams.T / 2;
Coordinates frontRightPoint = headingVec * TahoeParams.L - headingVec90 * TahoeParams.T / 2;
double minHit = Math.PI / 2.1;
GetMinHitAngle(rearLeftPoint, center, ref minHit);
GetMinHitAngle(rearRightPoint, center, ref minHit);
//GetMinHitAngle(frontLeftPoint, center, false, ref minHit);
//GetMinHitAngle(frontRightPoint, center, false, ref minHit);
frontLeftPoint = headingVec * TahoeParams.FL + headingVec90 * TahoeParams.T / 2;
frontRightPoint = headingVec * TahoeParams.FL - headingVec90 * TahoeParams.T / 2.0;
rearRightPoint = -headingVec * TahoeParams.RL - headingVec90 * TahoeParams.T / 2.0;
rearLeftPoint = -headingVec * TahoeParams.RL + headingVec90 * TahoeParams.T / 2.0;
List <Polygon> obstacles = GetObstacles(curTimestamp);
GetObstacleHitAngle(frontLeftPoint, center, obstacles, ref minHit);
GetObstacleHitAngle(rearLeftPoint, center, obstacles, ref minHit);
GetObstacleHitAngle(rearRightPoint, center, obstacles, ref minHit);
// trim some off the hit for safety'
minHit -= (0.3 / minRadius);
// move at least 0.6 meters
minHit = Math.Max(minHit, 0.6 / minRadius);
// calculate the exit stopping point
// shift the line by the minimum turning radius
Coordinates u = finalOrientation.P1 - finalOrientation.P0;
u = u.Normalize().Rotate90();
Line offsetLine = new Line();
offsetLine.P0 = finalOrientation.P0 + u * (minRadius + 2);
offsetLine.P1 = finalOrientation.P1 + u * (minRadius + 2);
// final the intersection of the current turn circle with a radius of twice the min turn radius and the offset line
Circle twoTurn = new Circle(2 * minRadius + 2, center);
Coordinates[] intersections;
double startAngle = (-center).ArcTan;
if (twoTurn.Intersect(offsetLine, out intersections))
{
// figure out where there were hits
for (int i = 0; i < intersections.Length; i++)
{
// get the angle of the hit
double angle = (intersections[i] - center).ArcTan;
if (angle < startAngle)
{
angle += 2 * Math.PI;
}
angle -= startAngle;
if (angle < minHit)
{
minHit = angle;
}
}
}
minHit = Math.Max(minHit, 0.6 / minRadius);
// set the stopping point at the min hit point
Coordinates stopPoint = rearAxleCircle.GetPoint(startAngle + minHit);
// calculate the stop distance
stopDistance = rearAxleCircle.r * minHit;
stopTimestamp = curTimestamp;
curvature = 1 / minRadius;
// calculate the required steering angle
double steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(-1 / minRadius, uturnSpeed);
ISpeedCommandGenerator shiftSpeedCommand = new ShiftSpeedCommand(TransmissionGear.Reverse);
ISteeringCommandGenerator initialSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, steeringRate, true);
ISpeedCommandGenerator passSpeedCommand = new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new TravelledDistanceProvider(curTimestamp, stopDistance), uturnSpeed));
ISteeringCommandGenerator passSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, null, false);
ChainedTrackingCommand cmd = new ChainedTrackingCommand(
new TrackingCommand(shiftSpeedCommand, initialSteeringCommand, true),
new TrackingCommand(passSpeedCommand, passSteeringCommand, false));
cmd.Label = backwardLabel;
Services.UIService.PushCircle(new Circle(minRadius, center), curTimestamp, "uturn circle", true);
Services.UIService.PushPoint(stopPoint, curTimestamp, "uturn stop point", true);
Services.UIService.PushPolygon(polygon, curTimestamp, "uturn polygon", true);
return(cmd);
}
