OrcaLine getOrcaLine(MotionModelSwedish buddy, float time, float dt)
{
Vector3 relPos = buddy.getPosition() - getPosition();
Vector3 relVel = getVelocity() - buddy.getVelocity();
float combRadius = buddy.getRadius() + getRadius();
OrcaLine orcaline = new OrcaLine();
Vector3 u;
Vector3 w = relVel - (relPos / time);
float decBoundary = Vector3.Dot(w, relPos);
if (relPos.sqrMagnitude > Mathf.Pow(combRadius, 2))
{
if (!(decBoundary < 0.0f && Mathf.Pow(decBoundary, 2) > Mathf.Pow(combRadius, 2) * w.sqrMagnitude))
{
// Legs
float distance = Mathf.Sqrt(relPos.sqrMagnitude - Mathf.Pow(combRadius, 2));
if (Vector3.Cross(relPos, w).y < 0.0f)
{
orcaline.direction = new Vector3(relPos.x * distance - relPos.z * combRadius, 0.0f, relPos.x * combRadius + relPos.z * distance) / relPos.sqrMagnitude;
}
else
{
orcaline.direction = -new Vector3(relPos.x * distance + relPos.z * combRadius, 0.0f, -relPos.x * combRadius + relPos.z * distance) / relPos.sqrMagnitude;
}
float uLen = Vector3.Dot(relVel, orcaline.direction);
u = uLen * orcaline.direction - relVel;
}
else
{
Vector3 wNorm = w.normalized;
orcaline.direction = new Vector3(wNorm.z, 0.0f, -wNorm.x);
u = ((combRadius / time) - w.magnitude) * wNorm;
}
}
else
{
w = relVel - (relPos / dt);
Vector3 wNorm = w.normalized;
orcaline.direction = new Vector3(wNorm.z, 0.0f, -wNorm.x);
u = ((combRadius / dt) - w.magnitude) * wNorm;
}
orcaline.point = getVelocity() + 0.5f * u;
//Debug.DrawLine(getPosition() + orcaline.point - (Vector3.Cross(Vector3.up, orcaline.direction)).normalized * vMax, getPosition() + orcaline.point + (Vector3.Cross(Vector3.up, orcaline.direction)).normalized * vMax, Color.blue, Time.deltaTime);
//Debug.DrawLine(getPosition() + orcaline.point, getPosition() + orcaline.point + orcaline.direction, Color.blue, Time.deltaTime);
return(orcaline);
}
// Use this for initialization
public static void linearProgram3(List <OrcaLine> lines, int numObstLines, int beginLine, float radius, ref Vector3 result)
{
float RVO_EPSILON = 0.00001f;
float distance = 0.0f;
for (int i = beginLine; i < lines.Count; ++i)
{
if (Vector3.Cross(lines[i].direction, lines[i].point - result).y < distance)
{
/* Result does not satisfy constraint of line i. */
List <OrcaLine> projLines = new List <OrcaLine>();
for (int ii = 0; ii < numObstLines; ++ii)
{
projLines.Add(lines[ii]);
}
for (int j = numObstLines; j < i; ++j)
{
OrcaLine line = new OrcaLine();
float determinant = -Vector3.Cross(lines[i].direction, lines[j].direction).y;
if (Mathf.Abs(determinant) <= RVO_EPSILON)
{
/* Line i and line j are parallel. */
if (Vector3.Dot(lines[i].direction, lines[j].direction) > 0.0f)
{
/* Line i and line j point in the same direction. */
continue;
}
else
{
/* Line i and line j point in opposite direction. */
line.point = 0.5f * (lines[i].point + lines[j].point);
}
}
else
{
line.point = lines[i].point + (-Vector3.Cross(lines[j].direction, lines[i].point - lines[j].point).y / determinant) * lines[i].direction;
}
line.direction = (lines[j].direction - lines[i].direction).normalized;
projLines.Add(line);
}
Vector2 tempResult = result;
if (false)//linearProgram2(projLines, radius, new Vector2(-lines[i].direction.y(), lines[i].direction.x()), true, ref result) < projLines.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.Cross(lines[i].direction, lines[i].point - result).y;
}
}
}
