def testEllipseSemiMinorAxis(self):
dt = 0.01
externalVelocity = np.array([1., 1., 1.])
position = np.array([2., 2., 2.])
externalPosition = np.array([3., 3., 3.])
externalTarget = np.array([4., 4., 4.])
returnedSemiMinorAxis = ellipseSemiMinorAxis(dt, externalVelocity, position, externalPosition, externalTarget)
expectedSemiMinorAxis = sqrt(2.01**2 - 0.01**2) * sqrt(3.) /2.
errorMessage = "Wrong value for semi-minor axis."
self.assertAlmostEqual(returnedSemiMinorAxis, expectedSemiMinorAxis, 7, errorMessage)
pass
def computePedPedRepulsiveForce(currentPedestrian, pedestrians):
''' Returns the repulsive force between all other pedestrians and the current one'''
externalVelocities, externalPositions, externalTargets, normalizedAlphaBetaVectors = extractExternalVariablesForPedestrians(currentPedestrian, pedestrians)
semiMinorAxes = np.array([ellipseSemiMinorAxis(dt, eV, currentPedestrian.position, eP, eT) for eV, eP, eT in zip(externalVelocities, externalPositions, externalTargets)])
return amplitude * sum([exp(-semiMinorAxes[i]/sigma) * normalizedAlphaBetaVectors[i] for i in range(len(semiMinorAxes))])