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);
}
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);
}
// 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);
}
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()));
}
