def computeCollisionTime(predictedTrajectory1, predictedTrajectory2,
    collisionDistanceThreshold, timeHorizon):
    """Computes the first instant 
    at which two predicted trajectories are within some distance threshold
    Computes all the times including timeHorizon
    
    User has to check the first variable collision to know about a collision"""
    t = 1
    p1 = predictedTrajectory1.predictPosition(t)
    p2 = predictedTrajectory2.predictPosition(t)
    collision = (p1 - p2).norm2() <= collisionDistanceThreshold
    while t < timeHorizon and not collision:
        t += 1
        p1 = predictedTrajectory1.predictPosition(t)
        p2 = predictedTrajectory2.predictPosition(t)
        collision = (p1 - p2).norm2() <= collisionDistanceThreshold
    return collision, t, p1, p2
