/*
* Detirmines if the velocity vector is on the left or right side of the VO if its inside the VO.
* Returns -1 if the velocity vector is inside the VO and left of the centerline
* Returns 1 if the velocity vector is inside the VO and right of the centerline
* returns 0 if the velocity vector is outside the VO
*/
private int pointRelativeCenterline(Vector p, VelocityObstacle VO)
{
Vector pToVertex = p - VO.vertex;
double pAngle = angleToXAxis(pToVertex);
double lAngle = angleToXAxis(VO.leftEdge);
double rAngle = angleToXAxis(VO.rightEdge);
double centerlineAngle = angleToXAxis(VO.centerline);
//Fix the angles so that they can be compared
if (lAngle > rAngle)
{
if (pAngle < 0)
{
lAngle -= Math.PI * 2;
}
else
{
rAngle += Math.PI * 2;
}
if (centerlineAngle < lAngle)
{
centerlineAngle += Math.PI * 2;
}
else if (centerlineAngle > rAngle)
{
centerlineAngle -= Math.PI * 2;
}
}
if (lAngle < pAngle && pAngle < rAngle)
{
if (pAngle < centerlineAngle)
{
return(-1);
}
return(1);
}
return(0);
}
/*
* Calculates the permissible velocities for the robot. Permissible velocities is outside the VO and should be as
* close to the prefered velocity as possible. They consists partly of the intersections between the edges of VO,
* and partly of the velocity vector projected on the edges of VO.
*/
private void calculatePermissibleVelocities()
{
//Calculates the permissible velocities in the intersections between the edges of VO
for (int i = 0; i < obstacleList.Count - 1; i++)
{
VelocityObstacle VO1 = obstacleList[i];
for (int j = i + 1; j < obstacleList.Count; j++)
{
VelocityObstacle VO2 = obstacleList[j];
Vector possibleVelocity = findRayIntersection(VO1.vertex, VO1.leftEdge, VO2.vertex, VO2.leftEdge);
if (possibleVelocity != zero)
{
if (pointOutsideVO(possibleVelocity))
{
permissibleVelocities.Add(possibleVelocity);
}
}
possibleVelocity = findRayIntersection(VO1.vertex, VO1.leftEdge, VO2.vertex, VO2.rightEdge);
if (possibleVelocity != zero)
{
if (pointOutsideVO(possibleVelocity))
{
permissibleVelocities.Add(possibleVelocity);
}
}
possibleVelocity = findRayIntersection(VO1.vertex, VO1.rightEdge, VO2.vertex, VO2.leftEdge);
if (possibleVelocity != zero)
{
if (pointOutsideVO(possibleVelocity))
{
permissibleVelocities.Add(possibleVelocity);
}
}
possibleVelocity = findRayIntersection(VO1.vertex, VO1.rightEdge, VO2.vertex, VO2.rightEdge);
if (possibleVelocity != zero)
{
if (pointOutsideVO(possibleVelocity))
{
permissibleVelocities.Add(possibleVelocity);
}
}
}
}
//Calculates the permissible velocities that is the velocity vector projected on the edges of VO
foreach (VelocityObstacle VO in obstacleList)
{
Vector pointOnEdge = projectPointOnRay(VO.vertex, VO.leftEdge, velocityPoint);
if (pointOnEdge != zero)
{
if (pointOutsideVO(pointOnEdge))
{
permissibleVelocities.Add(pointOnEdge);
}
}
pointOnEdge = projectPointOnRay(VO.vertex, VO.rightEdge, velocityPoint);
if (pointOnEdge != zero)
{
if (pointOutsideVO(pointOnEdge))
{
permissibleVelocities.Add(pointOnEdge);
}
}
}
}
/*
* Calculates a forbidden area, the velocity obstacle VO. If the velocity vector is inside
* this, the robot will collide with a neighbor robot. The VO is shaped like a cone with its vertex
* at the robot position and the 2 edges tangent to the collision radius of the neighbor robot.
* The cone is defined by 2 vectors representing the 2 edges and a vector point representing its vertex.
*/
private void calculateVO()
{
double collisionRadius = 2 * Program.robotRadius * radiusScaling;
obstacleList = new List <VelocityObstacle>();
foreach (Robot neighbor in neighborRobot)
{
Vector neighborHeading = new Vector(neighbor.getHeading().X, neighbor.getHeading().Y);
Vector neighborVelocity = neighborHeading * neighbor.getSpeed() * velocityScaling;
Vector neighborVector = new Vector(neighbor.getPosition().X, neighbor.getPosition().Y);
Vector vectorToNeighbor = neighborVector - robotPos;
double radiusRelativeDist = collisionRadius / vectorToNeighbor.Length;
//The VOangle is half of the angle of the cone that defines the VO.
double VOangle;
//Arcsinus is not defined for values larger than 1. Though if the robot drives inside the neighbor
//robot's collision radius the radiusRelativeDist gets larger than 1. In that case the VOangle is set to 90 degrees.
if (radiusRelativeDist <= 1)
{
VOangle = Math.Asin(radiusRelativeDist);
}
else
{
VOangle = Math.PI / 2;
}
double sinAngle = Math.Sin(VOangle);
double cosAngle = Math.Cos(VOangle);
Matrix rotationMatrixClockwise = new Matrix(cosAngle, sinAngle, -sinAngle, cosAngle, 0, 0);
Matrix rotationMatrixCounterClockwise = new Matrix(cosAngle, -sinAngle, sinAngle, cosAngle, 0, 0);
Vector rightEdge = vectorToNeighbor * rotationMatrixClockwise;
Vector leftEdge = vectorToNeighbor * rotationMatrixCounterClockwise;
Vector coneVertex = robotPos + vertexOffsetScaling * (neighborVelocity + velocity) / (2 * speedScaling);
VelocityObstacle tempVO = new VelocityObstacle(rightEdge, leftEdge, vectorToNeighbor, coneVertex);
/*
* An additional method which is used to enlarge the VO additionally. If the velocity vector is inside VO and left
* of the centerline, the VO is enlarged to the right and if the velocity vector is inside VO and right of the
* centerline, the VO is enlarged to the left. This makes it easier for the robot to chose the correct side to
* drive pass the neighbor. It is further explained in (J. Snape, J. van den Berg, S. J. Guy, D. Manocha (2011) "The hybrid
* reciprocal velocity obstacle") where it is called "The hybrid reciprocal velocity obstacle". It is disabled here since
* it didn't work according to plan but should work better with some adjustments.
*/
/*
* int test = pointRelativeCenterline(velocityPoint, temp);
* //test = -1 means that the velocity vector is inside VO and on the left side of the centerline
* if (test == -1)
* {
* tempVO.vertex = findLineIntersection(robotPos + neighborVelocity, temp.rightEdge, temp.vertex, temp.leftEdge);
* }
* //test = -1 means that the velocity vector is inside VO and on the right side of the centerline
* else if (test == 1)
* {
* tempVO.vertex = findLineIntersection(robotPos + neighborVelocity, temp.leftEdge, temp.vertex, temp.rightEdge);
* }*/
obstacleList.Add(tempVO);
}
}
