/* * 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); } }