public void computeNewVelocity()
{
orcaPlanes.Clear();
float invTimeHorizon = 1.0f / timeHorizon;
// Creates agen ORCA planes
for (int i = 0; i < agentNeighbors.Count; i++)
{
RVOAgent other = agentNeighbors[i].Value;
Vector3 relativePosition = other.position - this.position;
Vector3 relativeVelocity = velocity - other.velocity;
float distSquared = relativePosition.sqrMagnitude;
float combinedRadius = radius + other.radius;
float combinedRadiusSq = RVOKdTree.Squared(combinedRadius);
RVOPlane plane = new RVOPlane();
Vector3 u = Vector3.zero;
if(distSquared > combinedRadiusSq)
{
// No collision.
Vector3 w = relativeVelocity - invTimeHorizon * relativePosition;
// Vector from cutoff center to relative velocity.
float wLengthSq = w.sqrMagnitude;
float dotProduct = Vector3.Dot(w, relativePosition);
if(dotProduct < 0.0f && RVOKdTree.Squared(dotProduct) > combinedRadius * wLengthSq)
{
// Project on cut-off circle.
float wLength = Mathf.Sqrt(wLengthSq);
Vector3 unitW = w / wLengthSq;
plane.normal = unitW;
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
}
else
{
// Project on cone.
float a = distSquared;
//always remember when they do v*v it means dot.
float b = Vector3.Dot(relativePosition, relativeVelocity);
float c = relativeVelocity.sqrMagnitude - (Vector3.Cross(relativePosition, relativeVelocity)).sqrMagnitude / (distSquared - combinedRadiusSq);
float t = (b + Mathf.Sqrt((b * b) - a * c)) / a; // whoa, the quadratic formula!
Vector3 wRelative = relativeVelocity - t * relativePosition;
float wLength = wRelative.magnitude;
Vector3 unitW = wRelative / wLength;
plane.normal = unitW;
u = (combinedRadius * t - wLength) * unitW;
}
}
else
{
//collision with our mathematical body
float invTimeStep = 1.0f / Time.deltaTime; //delta time.
Vector3 w = relativeVelocity - invTimeStep * relativePosition;
float wLength = w.magnitude;
Vector3 unitW = w / wLength; // maybe I should replace this with w.normalized later.
plane.normal = unitW;
u = (combinedRadius * invTimeStep - wLength) * unitW;
}
plane.point = velocity + 0.5f * u;
orcaPlanes.Add(plane);
}
int planeFail = linearProgram3(orcaPlanes, maxSpeed, preferedVelocity, false, ref newVelocity);
if(planeFail < orcaPlanes.Count)
{
linearProgram4(orcaPlanes, planeFail, maxSpeed, ref newVelocity);
}
}
void linearProgram4(List<RVOPlane> planes, int beginPlane, float radius, ref Vector3 result)
{
float distance = 0.0f;
for (int i = beginPlane; i < planes.Count; ++i)
{
if (Vector3.Dot(planes[i].normal, (planes[i].point - result)) > distance)
{
/* Result does not satisfy constraint of plane i. */
List<RVOPlane> projPlanes = new List<RVOPlane>();
for (int j = 0; j < i; ++j)
{
RVOPlane plane = new RVOPlane();
Vector3 crossProduct = Vector3.Cross(planes[j].normal, planes[i].normal);
if (crossProduct.sqrMagnitude <= RVO_EPSILON)
{
/* Plane i and plane j are (almost) parallel. */
if (Vector3.Dot(planes[i].normal, planes[j].normal) > 0.0f)
{
/* Plane i and plane j point in the same direction. */
continue;
}
else
{
/* Plane i and plane j point in opposite direction. */
plane.point = 0.5f * (planes[i].point + planes[j].point);
}
}
else
{
/* Plane.point is point on line of intersection between plane i and plane j. */
Vector3 lineNormal = Vector3.Cross(crossProduct, planes[i].normal);
plane.point = planes[i].point
+ (
Vector3.Dot((planes[j].point - planes[i].point) , planes[j].normal) /
Vector3.Dot(lineNormal, planes[j].normal)) * lineNormal;
}
plane.normal = (planes[j].normal - planes[i].normal).normalized;
projPlanes.Add(plane);
}
Vector3 tempResult = result;
if (linearProgram3(projPlanes, radius, planes[i].normal, true, ref result) < projPlanes.Count)
{
/* This should in principle not happen. The result is by definition already in the feasible region of this linear program. If it fails, it is due to small floating point error, and the current result is kept. */
result = tempResult;
}
distance = Vector3.Dot(planes[i].normal, (planes[i].point - result));
}
}
}