def computeCarCarRepulsiveForce(currentCar, cars): ''' Returns the repulsive force between all other cars and the current one, discarding the form factor [Anvari et al.]''' externalVelocities, externalPositions, externalTargets, externalIds, normalizedAlphaBetaVectors = extractExternalVariablesForCars(currentCar, cars) radiusesOfEllipses = np.array([radiusOfEllipse(currentCar.position, currentCar.target, eP, currentCar.length, currentCar.width) for eP in externalPositions]) radiusesOfEllipsesFromExternalCars = np.array([radiusOfEllipse(eP, eT, currentCar.position, cars[eC].length, cars[eC].width) for eP, eT, eC in zip(externalPositions, externalTargets, externalIds)]) distanceBetweenCentersOfCars = [np.linalg.norm(eC - currentCar.position) for eC in externalPositions] sumOfRadiuses = radiusesOfEllipses + radiusesOfEllipsesFromExternalCars - distanceBetweenCentersOfCars return amplitudeAlphaU * sum([exp(sumOfRadiuses[i]/BAlphaU) * normalizedAlphaBetaVectors[i] for i in range(len(sumOfRadiuses))])
def computePedCarRepulsiveForce(currentEntity, cars): externalVelocities, externalPositions, externalTargets, externalIds, normalizedAlphaBetaVectors = extractExternalVariablesForCars(currentEntity, cars) radiusesOfEllipsesFromExternalCars = np.array([radiusOfEllipse(eP, eT, currentEntity.position, cars[eC].length, cars[eC].width) for eP, eT, eC in zip(externalPositions, externalTargets, externalIds)]) distanceBetweenPedestrianAndCars = np.array([np.linalg.norm(eC - currentEntity.position) for eC in externalPositions]) radiusOfPedestrian = 1. sumOfRadiuses = radiusOfPedestrian + radiusesOfEllipsesFromExternalCars - distanceBetweenPedestrianAndCars return amplitudeAlphaU * sum([exp(sumOfRadiuses[i]/BAlphaU) * normalizedAlphaBetaVectors[i] for i in range(len(sumOfRadiuses))])