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; }