| static private distSqPointLineSegment ( Vector2 vector1, Vector2 vector2, Vector2 vector3 ) : float | ||
| vector1 | Vector2 | The first endpoint of the line segment. |
| vector2 | Vector2 | The second endpoint of the line segment. * |
| vector3 | Vector2 | The point to which the squared distance is to * be calculated. |
| return | float | |
/**
* <summary>Inserts a static obstacle neighbor into the set of neighbors
* of this agent.</summary>
*
* <param name="obstacle">The number of the static obstacle to be
* inserted.</param>
* <param name="rangeSq">The squared range around this agent.</param>
*/
internal void insertObstacleNeighbor(Obstacle obstacle, float rangeSq)
{
Obstacle nextObstacle = obstacle.next_;
//if obstacle is below feet of agent or agent is above the head of obstacle, culling this obstacle
float minYPos = Mathf.Min(obstacle.curHeight_, nextObstacle.curHeight_);
float maxYPos = Mathf.Max(obstacle.curHeight_ + obstacle.ObsHeight_, nextObstacle.curHeight_ + nextObstacle.ObsHeight_);
if (curHeight_ > maxYPos || curHeight_ + agentHeight_ < minYPos)
{
return;
}
float distSq = RVOMath.distSqPointLineSegment(obstacle.point_, nextObstacle.point_, position_);
if (distSq < rangeSq)
{
obstacleNeighbors_.Add(new KeyValuePair <float, Obstacle>(distSq, obstacle));
int i = obstacleNeighbors_.Count - 1;
while (i != 0 && distSq < obstacleNeighbors_[i - 1].Key)
{
obstacleNeighbors_[i] = obstacleNeighbors_[i - 1];
--i;
}
obstacleNeighbors_[i] = new KeyValuePair <float, Obstacle>(distSq, obstacle);
}
}
/**
* <summary>Inserts a static obstacle neighbor into the set of neighbors
* of this agent.</summary>
*
* <param name="obstacle">The number of the static obstacle to be
* inserted.</param>
* <param name="rangeSq">The squared range around this agent.</param>
*/
internal void insertObstacleNeighbor(Obstacle obstacle, double rangeSq)
{
if ((type == AgentType.AirUnit || type == AgentType.GroundJumpUnit) && !obstacle.isMapBound)
{
return;
}
Obstacle nextObstacle = obstacle.next_;
double distSq = RVOMath.distSqPointLineSegment(obstacle.point_, nextObstacle.point_, position_);
if (distSq < rangeSq)
{
obstacleNeighbors_.Add(new KeyValuePair <double, Obstacle>(distSq, obstacle));
int i = obstacleNeighbors_.Count - 1;
while (i != 0 && distSq < obstacleNeighbors_[i - 1].Key)
{
obstacleNeighbors_[i] = obstacleNeighbors_[i - 1];
--i;
}
obstacleNeighbors_[i] = new KeyValuePair <double, Obstacle>(distSq, obstacle);
}
}
internal void insertObstacleNeighbor(Obstacle obstacle, float rangeSq)
{
Obstacle nextObstacle = obstacle.nextObstacle;
float distSq = RVOMath.distSqPointLineSegment(obstacle.point_, nextObstacle.point_, position_);
if (distSq < rangeSq)
{
obstacleNeighbors_.Add(new KeyValuePair <float, Obstacle>(distSq, obstacle));
int i = obstacleNeighbors_.Count - 1;
while (i != 0 && distSq < obstacleNeighbors_[i - 1].Key)
{
obstacleNeighbors_[i] = obstacleNeighbors_[i - 1];
--i;
}
obstacleNeighbors_[i] = new KeyValuePair <float, Obstacle>(distSq, obstacle);
}
}
private void acceleratedProgram(Vector2 velocity, ref Vector2 result)
{
//Debug.Log(velocity.ToString() + " " + result.ToString());
float speed = RVOMath.abs(velocity);
float maxSpeed = RVOMath.Min(speed + accelerated_, maxSpeed_);
float minSpeed = RVOMath.Max(speed - accelerated_, 0.0f);
Vector2 tempResult = result;
//Debug.Log(speed);
if (speed >= minSpeedToTurn_)
{
Vector2 leftVerticalVelocity = new Vector2(-velocity.y_, velocity.x_);
Vector2 rightVerticalVelocity = new Vector2(velocity.y_, -velocity.x_);
leftVerticalVelocity = RVOMath.normalize(leftVerticalVelocity) * speed * angularSpeed_;
rightVerticalVelocity = RVOMath.normalize(rightVerticalVelocity) * speed * angularSpeed_;
Vector2 leftMin = RVOMath.normalize(velocity + leftVerticalVelocity) * minSpeed;
Vector2 leftMax = RVOMath.normalize(velocity + leftVerticalVelocity) * maxSpeed;
Vector2 rightMin = RVOMath.normalize(velocity + rightVerticalVelocity) * minSpeed;
Vector2 rightMax = RVOMath.normalize(velocity + rightVerticalVelocity) * maxSpeed;
Vector2 leftVector2 = leftMax - leftMin;
Vector2 rightVector2 = rightMax - rightMin;
if (RVOMath.det(leftVector2, result) < 0.0f && RVOMath.det(rightVector2, result) > 0.0f) //middle
{
float resultSpeed = RVOMath.abs(result);
if (resultSpeed > maxSpeed)
{
tempResult = RVOMath.normalize(result) * maxSpeed;
}
if (resultSpeed < minSpeed)
{
tempResult = RVOMath.normalize(result) * minSpeed;
}
}
else
{
if (RVOMath.det(leftVector2, result) > 0.0f) //left
{
float r = ((result - leftMin) * (leftMax - leftMin)) / RVOMath.absSq(leftMax - leftMin);
if (id_ == 2)
{
Debug.Log(r);
}
if (r < 0.0f)
{
tempResult = RVOMath.normalize(leftVector2) * minSpeed;
}
if (r > 1.0f)
{
tempResult = RVOMath.normalize(leftVector2) * maxSpeed;
}
if (r >= 0.0f && r <= 1.0f)
{
float resultSpeed = RVOMath.absSq(result) - RVOMath.distSqPointLineSegment(leftMin, leftMax, result);
resultSpeed = RVOMath.sqrt(resultSpeed);
if (resultSpeed > maxSpeed)
{
tempResult = RVOMath.normalize(leftVector2) * maxSpeed;
}
if (resultSpeed < minSpeed)
{
tempResult = RVOMath.normalize(leftVector2) * minSpeed;
}
if (resultSpeed >= minSpeed && resultSpeed <= maxSpeed)
{
tempResult = RVOMath.normalize(leftVector2) * resultSpeed;
}
}
}
if (RVOMath.det(rightVector2, result) < 0.0f) //right
{
float r = ((result - rightMin) * (rightMax - rightMin)) / RVOMath.absSq(rightMax - rightMin);
if (r < 0.0f)
{
tempResult = RVOMath.normalize(rightVector2) * minSpeed;
}
if (r > 1.0f)
{
tempResult = RVOMath.normalize(rightVector2) * maxSpeed;
}
if (r >= 0.0f && r <= 1.0f)
{
float resultSpeed = RVOMath.absSq(result) - RVOMath.distSqPointLineSegment(rightMin, rightMax, result);
resultSpeed = RVOMath.sqrt(resultSpeed);
if (resultSpeed > maxSpeed)
{
tempResult = RVOMath.normalize(rightVector2) * maxSpeed;
}
if (resultSpeed < minSpeed)
{
tempResult = RVOMath.normalize(rightVector2) * minSpeed;
}
if (resultSpeed >= minSpeed && resultSpeed <= maxSpeed)
{
tempResult = RVOMath.normalize(rightVector2) * resultSpeed;
}
}
}
}
}
else
{
float resultSpeed = RVOMath.abs(result);
if (resultSpeed > maxSpeed)
{
if (RVOMath.abs(velocity) < 1e-6f)
{
tempResult = RVOMath.normalize(prefVelocity_) * maxSpeed;
}
else
{
tempResult = RVOMath.normalize(velocity) * maxSpeed;
}
}
if (resultSpeed < minSpeed)
{
tempResult = RVOMath.normalize(velocity) * minSpeed;
}
}
result = tempResult;
}