def __init__(self, referenceGPS):
self.localZECEF = self._gpsToECEF(referenceGPS)
localUnitZECEF = self.localZECEF / np.linalg.norm(self.localZECEF)
unitToPole = calcs.unit(northPoleECEF - self.localZECEF)
self.localUnitYECEF = unitToPole - localUnitZECEF * np.dot(
unitToPole, localUnitZECEF)
self.localUnitYECEF = calcs.unit(self.localUnitYECEF)
self.localUnitXECEF = np.cross(self.localUnitYECEF, localUnitZECEF)
self.localUnitZECEF = calcs.unit(self.localZECEF)
def testEntryVelocity(self, entryVelocity):
"""
Is the given absolute velocity OK? Illegal velocities are ones which are headed "into" the shape and would cause an immediate collision.
"""
relativeVelocity = calcs.unit(entryVelocity - self.velocity)
cosEntryAngle = np.dot(relativeVelocity, self.normal)
return cosEntryAngle <= self.upperCosLimit and cosEntryAngle >= self.lowerCosLimit
def genStandardHardScenario():
boundaryPoints = np.array([(-COURSE_DIM / 2.0, -COURSE_DIM / 2.0),
(-COURSE_DIM / 2.0, COURSE_DIM / 2.0),
(COURSE_DIM / 2.0, COURSE_DIM / 2.0),
(COURSE_DIM / 2.0, -COURSE_DIM / 2.0)],
np.double)
startPoint = boundaryPoints[0] * 0.8
waypoints = []
waypoints.append(boundaryPoints[2] * 0.8)
waypoints.append(boundaryPoints[1] * 0.8)
waypoints.append(boundaryPoints[3] * 0.8)
startVelocity = calcs.unit(waypoints[0] -
startPoint) * DEFAULT_VEHICLE.maxSpeed
scenario = Scenario(boundaryPoints=boundaryPoints,
noFlyZones=[],
dynamicNoFlyZones=[],
roads=[],
startPoint=startPoint,
startVelocity=startVelocity,
wayPoints=waypoints)
generator = ObstacleGenerator(DEFAULT_PARAMS, scenario, DEFAULT_VEHICLE)
generator.setGenerationInfo(COURSE_DIM / 10.0, 1.0, 0.0)
generator.blockEstimatedPath(10)
generator.setGenerationInfo(COURSE_DIM / 15.0, 0.0, 0.15)
generator.blockEstimatedPath(5)
return scenario
def __init__(self, p1, p2):
# Point 1
self.p1 = p1
# Point 2
self.p2 = p2
diff = self.p2 - self.p1
magSquared = calcs.lengthSquared(diff)
if magSquared > 0.0:
# Unit normal of the line. Paths only intersect this line if opposing the normal.
# Normal is chosen so that, if polygons are wound counter-clockwise,
# normals point outwards.
self.n = calcs.unit(calcs.CWNorm(diff))
# Points from point 1 to point 2 with length inverse to the distance between the _points.
self.invTan = diff / magSquared
else:
# If this happens, the line segment is not suitable for checking intersections. However, you can still interpolate over it.
self.n = None
self.invTan = None
if __name__ == "__main__":
params = DEFAULT_PARAMS
vehicle = DEFAULT_VEHICLE
boundaryPoints = np.array([(-COURSE_DIM / 2.0, -COURSE_DIM / 2.0),
(-COURSE_DIM / 2.0, COURSE_DIM / 2.0),
(COURSE_DIM / 2.0, COURSE_DIM / 2.0),
(COURSE_DIM / 2.0, -COURSE_DIM / 2.0)],
np.double)
startPoint = boundaryPoints[0] * 0.8
waypoints = []
waypoints.append(boundaryPoints[2] * 0.8)
waypoints.append(boundaryPoints[1] * 0.8)
waypoints.append(boundaryPoints[3] * 0.8)
startVelocity = calcs.unit(waypoints[0] -
startPoint) * DEFAULT_VEHICLE.maxSpeed
scenario = Scenario(boundaryPoints=boundaryPoints,
noFlyZones=[],
dynamicNoFlyZones=[],
roads=[],
startPoint=startPoint,
startVelocity=startVelocity,
wayPoints=waypoints)
for i in range(100):
bestPath = solve(params, scenario, vehicle)
generator = ObstacleGenerator(params, scenario, vehicle)
generator.setGenerationInfo(COURSE_DIM / 10.0, 1.0, 0.0)
generator.block(bestPath.pathSegments)
interface.save("../scenarios/generated.json", params, scenario,