private static KInt Min(KInt left, long right)
{
right = right * KInt.divscale / KInt2.divscale;
if (left.IntValue < right)
{
return(left);
}
return(KInt.ToInt(right));
}
private static KInt ReduceMax(KInt value, long right)
{
right = right * KInt.divscale / KInt2.divscale;
KInt data = KInt.ToInt(value.IntValue - right);
if (data <= 0)
{
return(KInt.Zero);
}
return(data);
}
private static KInt ReduceMax(long left, KInt value)
{
left = left * KInt.divscale / KInt2.divscale;
KInt data = KInt.ToInt(left - value.IntValue);
if (data <= 0)
{
return(KInt.Zero);
}
return(data);
}
/**
* <summary>Clears the simulation.</summary>
*/
public void Clear()
{
agents_ = new List <Agent>();
agentNo2indexDict_ = new Dictionary <int, int>();
index2agentNoDict_ = new Dictionary <int, int>();
defaultAgent_ = null;
kdTree_ = new KdTree();
obstacles_ = new List <Obstacle>();
globalTime_ = 0;
isError = false;
timeStep_ = KInt.ToInt(KInt.divscale / 10);
SetNumWorkers(0);
}
/**
* <summary>Recursive method for building an agent k-D tree.</summary>
*
* <param name="begin">The beginning agent k-D tree node node index.
* </param>
* <param name="end">The ending agent k-D tree node index.</param>
* <param name="node">The current agent k-D tree node index.</param>
*/
private void buildAgentTreeRecursive(int begin, int end, int node)
{
agentTree_[node].begin_ = begin;
agentTree_[node].end_ = end;
agentTree_[node].minx = agentTree_[node].maxx = KInt.ToInt(agents_[begin].position_.IntX * KInt.divscale / KInt2.divscale);
agentTree_[node].miny = agentTree_[node].maxy = KInt.ToInt(agents_[begin].position_.IntY * KInt.divscale / KInt2.divscale);
for (int i = begin + 1; i < end; ++i)
{
agentTree_[node].maxx = Max(agentTree_[node].maxx, agents_[i].position_.IntX);
agentTree_[node].minx = Min(agentTree_[node].minx, agents_[i].position_.IntX);
agentTree_[node].maxy = Max(agentTree_[node].maxy, agents_[i].position_.IntY);
agentTree_[node].miny = Min(agentTree_[node].miny, agents_[i].position_.IntY);
}
if (end - begin > MAX_LEAF_SIZE)
{
/* No leaf node. */
bool isVertical = agentTree_[node].maxx - agentTree_[node].minx > agentTree_[node].maxy - agentTree_[node].miny;
KInt splitValue = (isVertical ? agentTree_[node].maxx + agentTree_[node].minx : agentTree_[node].maxy + agentTree_[node].miny) / 2;
long convertvalue = splitValue.IntValue * KInt2.divscale / KInt.divscale;
int left = begin;
int right = end;
while (left < right)
{
while (left < right && (isVertical ? agents_[left].position_.IntX : agents_[left].position_.IntY) < convertvalue)
{
++left;
}
while (right > left && (isVertical ? agents_[right - 1].position_.IntX : agents_[right - 1].position_.IntY) >= convertvalue)
{
--right;
}
if (left < right)
{
Agent tempAgent = agents_[left];
agents_[left] = agents_[right - 1];
agents_[right - 1] = tempAgent;
++left;
--right;
}
}
int leftSize = left - begin;
if (leftSize == 0)
{
++leftSize;
++left;
++right;
}
agentTree_[node].left_ = node + 1;
agentTree_[node].right_ = node + 2 * leftSize;
buildAgentTreeRecursive(begin, left, agentTree_[node].left_);
buildAgentTreeRecursive(left, end, agentTree_[node].right_);
}
}
/**
* <summary>Computes the squared length of a specified two-dimensional
* vector.</summary>
*
* <returns>The squared length of the two-dimensional vector.</returns>
*
* <param name="vector">The two-dimensional vector whose squared length
* is to be computed.</param>
*/
public static KInt absSq(KInt2 vector)
{
return(KInt.ToInt((vector.IntX * vector.IntX + vector.IntY * vector.IntY) * KInt.divscale / KInt2.div2scale));
}
internal static KInt sqrt(KInt scalar)
{
//return scalar.IntSqrt;
return(KInt.ToInt(Sqrt(scalar.IntValue * KInt.divscale)));
}
/**
* <summary>Computes the determinant of a two-dimensional square matrix
* with rows consisting of the specified two-dimensional vectors.
* </summary>
*
* <returns>The determinant of the two-dimensional square matrix.
* </returns>
*
* <param name="vector1">The top row of the two-dimensional square
* matrix.</param>
* <param name="vector2">The bottom row of the two-dimensional square
* matrix.</param>
*/
internal static KInt det(KInt2 vector1, KInt2 vector2)
{
return(KInt.ToInt((vector1.IntX * vector2.IntY - vector1.IntY * vector2.IntX) * KInt.divscale / KInt2.div2scale));
}
public static KInt Dot(KInt2 left, KInt2 right)
{
return(KInt.ToInt((left.IntX * right.IntX + left.IntY * right.IntY) * KInt.divscale / KInt2.div2scale));
}
/**
* <summary>Computes the new velocity of this agent.</summary>
*/
internal void computeNewVelocity()
{
orcaLines_.Clear();
//KInt invTimeHorizonObst = 1 / timeHorizonObst_;
KInt tempradius = radius_ / timeHorizonObst_;
/* Create obstacle ORCA lines. */
for (int i = 0; i < obstacleNeighbors_.Count; ++i)
{
Obstacle obstacle1 = obstacleNeighbors_[i].Value;
Obstacle obstacle2 = obstacle1.next_;
KInt2 relativePosition1 = obstacle1.point_ - position_;
KInt2 relativePosition2 = obstacle2.point_ - position_;
/*
* Check if velocity obstacle of obstacle is already taken care
* of by previously constructed obstacle ORCA lines.
*/
bool alreadyCovered = false;
for (int j = 0; j < orcaLines_.Count; ++j)
{
if (RVOMath.det(relativePosition1 / timeHorizonObst_ - orcaLines_[j].point, orcaLines_[j].direction) - tempradius >= 0 && RVOMath.det(relativePosition2 / timeHorizonObst_ - orcaLines_[j].point, orcaLines_[j].direction) - tempradius >= 0)
{
alreadyCovered = true;
break;
}
}
if (alreadyCovered)
{
continue;
}
/* Not yet covered. Check for collisions. */
KInt distSq1 = RVOMath.absSq(relativePosition1);
KInt distSq2 = RVOMath.absSq(relativePosition2);
KInt radiusSq = RVOMath.sqr(radius_);
KInt2 obstacleVector = obstacle2.point_ - obstacle1.point_;
KInt s = (-RVOMath.Dot(relativePosition1, obstacleVector)) / RVOMath.absSq(obstacleVector);
KInt distSqLine = RVOMath.absSq(-relativePosition1 - s * obstacleVector);
Line line = new Line();
if (s < 0 && distSq1 <= radiusSq)
{
/* Collision with left vertex. Ignore if non-convex. */
if (obstacle1.convex_)
{
line.point = KInt2.zero;
line.direction = RVOMath.normalize(KInt2.ToInt2(-relativePosition1.IntY, relativePosition1.IntX));
orcaLines_.Add(line);
}
continue;
}
else if (s > 1 && distSq2 <= radiusSq)
{
/*
* Collision with right vertex. Ignore if non-convex or if
* it will be taken care of by neighboring obstacle.
*/
if (obstacle2.convex_ && RVOMath.det(relativePosition2, obstacle2.direction_) >= 0)
{
line.point = KInt2.zero;
line.direction = RVOMath.normalize(KInt2.ToInt2(-relativePosition2.IntY, relativePosition2.IntX));
orcaLines_.Add(line);
}
continue;
}
else if (s >= 0 && s < 1 && distSqLine <= radiusSq)
{
/* Collision with obstacle segment. */
line.point = KInt2.zero;
line.direction = -obstacle1.direction_;
orcaLines_.Add(line);
continue;
}
/*
* No collision. Compute legs. When obliquely viewed, both legs
* can come from a single vertex. Legs extend cut-off line when
* non-convex vertex.
*/
KInt2 leftLegDirection, rightLegDirection;
if (s < 0 && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that left vertex
* defines velocity obstacle.
*/
if (!obstacle1.convex_)
{
/* Ignore obstacle. */
continue;
}
obstacle2 = obstacle1;
KInt leg1 = RVOMath.sqrt(distSq1 - radiusSq);
leftLegDirection = KInt2.ToInt2(relativePosition1.IntX * leg1 - relativePosition1.IntY * radius_, relativePosition1.IntX * radius_ + relativePosition1.IntY * leg1) / distSq1;
rightLegDirection = KInt2.ToInt2(relativePosition1.IntX * leg1 + relativePosition1.IntY * radius_, -relativePosition1.IntX * radius_ + relativePosition1.IntY * leg1) / distSq1;
if (isover(leftLegDirection) || isover(rightLegDirection))
{
UnityEngine.Debug.LogError("!!!");
}
}
else if (s > 1 && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that
* right vertex defines velocity obstacle.
*/
if (!obstacle2.convex_)
{
/* Ignore obstacle. */
continue;
}
obstacle1 = obstacle2;
KInt leg2 = RVOMath.sqrt(distSq2 - radiusSq);
leftLegDirection = KInt2.ToInt2(relativePosition2.IntX * leg2 - relativePosition2.IntY * radius_, relativePosition2.IntX * radius_ + relativePosition2.IntY * leg2) / distSq2;
rightLegDirection = KInt2.ToInt2(relativePosition2.IntX * leg2 + relativePosition2.IntY * radius_, -relativePosition2.IntX * radius_ + relativePosition2.IntY * leg2) / distSq2;
if (isover(leftLegDirection) || isover(rightLegDirection))
{
Debug.LogError("!!!");
}
}
else
{
/* Usual situation. */
if (obstacle1.convex_)
{
KInt leg1 = RVOMath.sqrt(distSq1 - radiusSq);
leftLegDirection = KInt2.ToInt2(relativePosition1.IntX * leg1 - relativePosition1.IntY * radius_, relativePosition1.IntX * radius_ + relativePosition1.IntY * leg1) / distSq1;
if (isover(leftLegDirection))
{
Debug.LogError("!!!");
}
}
else
{
/* Left vertex non-convex; left leg extends cut-off line. */
leftLegDirection = -obstacle1.direction_;
if (isover(leftLegDirection))
{
Debug.LogError("!!!");
}
}
if (obstacle2.convex_)
{
KInt leg2 = RVOMath.sqrt(distSq2 - radiusSq);
rightLegDirection = KInt2.ToInt2(relativePosition2.IntX * leg2 + relativePosition2.IntY * radius_, -relativePosition2.IntX * radius_ + relativePosition2.IntY * leg2) / distSq2;
if (isover(rightLegDirection))
{
Debug.LogError("!!!");
}
}
else
{
/* Right vertex non-convex; right leg extends cut-off line. */
rightLegDirection = obstacle1.direction_;
if (isover(rightLegDirection))
{
Debug.LogError("!!!");
}
}
}
/*
* Legs can never point into neighboring edge when convex
* vertex, take cutoff-line of neighboring edge instead. If
* velocity projected on "foreign" leg, no constraint is added.
*/
Obstacle leftNeighbor = obstacle1.previous_;
bool isLeftLegForeign = false;
bool isRightLegForeign = false;
if (obstacle1.convex_ && RVOMath.det(leftLegDirection, -leftNeighbor.direction_) >= 0)
{
/* Left leg points into obstacle. */
leftLegDirection = -leftNeighbor.direction_;
if (isover(leftLegDirection))
{
Debug.LogError("!!!");
}
isLeftLegForeign = true;
}
if (obstacle2.convex_ && RVOMath.det(rightLegDirection, obstacle2.direction_) <= 0)
{
/* Right leg points into obstacle. */
rightLegDirection = obstacle2.direction_;
isRightLegForeign = true;
if (isover(rightLegDirection))
{
Debug.LogError("!!!");
}
}
/* Compute cut-off centers. */
KInt2 leftCutOff = (obstacle1.point_ - position_) / timeHorizonObst_;
KInt2 rightCutOff = (obstacle2.point_ - position_) / timeHorizonObst_;
KInt2 cutOffVector = rightCutOff - leftCutOff;
/* Project current velocity on velocity obstacle. */
/* Check if current velocity is projected on cutoff circles. */
KInt sqvalue = RVOMath.absSq(cutOffVector);
KInt t = KInt.ToInt(KInt.divscale / 2);
if (obstacle1 != obstacle2)
{
if (sqvalue == 0)
{
t = KInt.MaxValue;
}
else
{
t = RVOMath.Dot((velocity_ - leftCutOff), cutOffVector) / sqvalue;
}
}
KInt tLeft = RVOMath.Dot((velocity_ - leftCutOff), leftLegDirection);
KInt tRight = RVOMath.Dot((velocity_ - rightCutOff), rightLegDirection);
if ((t < 0 && tLeft < 0) || (obstacle1 == obstacle2 && tLeft < 0 && tRight < 0))
{
/* Project on left cut-off circle. */
KInt2 unitW = RVOMath.normalize((velocity_ - leftCutOff));
line.direction = KInt2.ToInt2(unitW.IntY, -unitW.IntX);
line.point = leftCutOff + radius_ * unitW / timeHorizonObst_;
orcaLines_.Add(line);
continue;
}
else if (t > 1 && tRight < 0)
{
/* Project on right cut-off circle. */
KInt2 unitW = RVOMath.normalize((velocity_ - rightCutOff));
line.direction = KInt2.ToInt2(unitW.IntY, -unitW.IntX);
line.point = rightCutOff + radius_ * unitW / timeHorizonObst_;
orcaLines_.Add(line);
continue;
}
/*
* Project on left leg, right leg, or cut-off line, whichever is
* closest to velocity.
*/
KInt distSqCutoff = (t < 0 || t > 1 || obstacle1 == obstacle2) ? KInt.MaxValue : RVOMath.absSq(velocity_ - (leftCutOff + t * cutOffVector));
KInt distSqLeft = tLeft < 0 ? KInt.MaxValue : RVOMath.absSq(velocity_ - (leftCutOff + tLeft * leftLegDirection));
KInt distSqRight = tRight < 0 ? KInt.MaxValue : RVOMath.absSq(velocity_ - (rightCutOff + tRight * rightLegDirection));
if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight)
{
/* Project on cut-off line. */
line.direction = -obstacle1.direction_;
line.point = leftCutOff + radius_ * KInt2.ToInt2(-line.direction.IntY, line.direction.IntX) / timeHorizonObst_;
orcaLines_.Add(line);
continue;
}
if (distSqLeft <= distSqRight)
{
/* Project on left leg. */
if (isLeftLegForeign)
{
continue;
}
line.direction = leftLegDirection;
line.point = leftCutOff + radius_ * KInt2.ToInt2(-line.direction.IntY, line.direction.IntX) / timeHorizonObst_;
orcaLines_.Add(line);
continue;
}
/* Project on right leg. */
if (isRightLegForeign)
{
continue;
}
line.direction = -rightLegDirection;
line.point = rightCutOff + radius_ * KInt2.ToInt2(-line.direction.IntY, line.direction.IntX) / timeHorizonObst_;
orcaLines_.Add(line);
}
int numObstLines = orcaLines_.Count;
//KInt invTimeHorizon = 1 / timeHorizon_;
/* Create agent ORCA lines. */
for (int i = 0; i < agentNeighbors_.Count; ++i)
{
Agent other = agentNeighbors_[i].Value;
KInt2 relativePosition = other.position_ - position_;
KInt2 relativeVelocity = velocity_ - other.velocity_;
KInt distSq = RVOMath.absSq(relativePosition);
KInt combinedRadius = radius_ + other.radius_;
KInt combinedRadiusSq = RVOMath.sqr(combinedRadius);
Line line = new Line();
KInt2 u;
if (distSq > combinedRadiusSq)
{
/* No collision. */
KInt2 w = relativeVelocity - relativePosition / timeHorizon_;
/* Vector from cutoff center to relative velocity. */
KInt wLengthSq = RVOMath.absSq(w);
KInt dotProduct1 = RVOMath.Dot(w, relativePosition);
if (dotProduct1 < 0 && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq)
{
/* Project on cut-off circle. */
KInt wLength = RVOMath.sqrt(wLengthSq);
if (wLength == 0)
{
continue;
}
KInt2 unitW = w / wLength;
line.direction = KInt2.ToInt2(unitW.IntY, -unitW.IntX);
u = (combinedRadius / timeHorizon_ - wLength) * unitW;
}
else
{
/* Project on legs. */
KInt leg = RVOMath.sqrt(distSq - combinedRadiusSq);
if (RVOMath.det(relativePosition, w) > 0)
{
/* Project on left leg. */
line.direction = KInt2.ToInt2(relativePosition.IntX * leg - relativePosition.IntY * combinedRadius, relativePosition.IntX * combinedRadius + relativePosition.IntY * leg) / distSq;
}
else
{
/* Project on right leg. */
line.direction = -KInt2.ToInt2(relativePosition.IntX * leg + relativePosition.IntY * combinedRadius, -relativePosition.IntX * combinedRadius + relativePosition.IntY * leg) / distSq;
}
KInt dotProduct2 = RVOMath.Dot(relativeVelocity, line.direction);
u = dotProduct2 * line.direction - relativeVelocity;
}
}
else
{
/* Collision. Project on cut-off circle of time timeStep. */
//KInt invTimeStep = 1 / Simulator.Instance.timeStep_;
/* Vector from cutoff center to relative velocity. */
KInt2 w = relativeVelocity - relativePosition / Simulator.Instance.timeStep_;
KInt wLength = RVOMath.abs(w);
if (wLength == 0)
{
continue;
}
KInt2 unitW = w / wLength;
line.direction = KInt2.ToInt2(unitW.IntY, -unitW.IntX);
u = (combinedRadius / Simulator.Instance.timeStep_ - wLength) * unitW;
}
line.point = velocity_ + u / 2;
orcaLines_.Add(line);
}
int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, ref newVelocity_);
if (lineFail < orcaLines_.Count)
{
linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, ref newVelocity_);
}
}
