bool PlanningProblem::hasCollision(const Station &st, const Obstacle &ob) { b2Transform st_Transform; st_Transform.Set(st.getPosition().to2D().toB2vec2(), st.getPosition().Teta()); return b2TestOverlap(ob.shape, 0, agent->shape, 1, ob.predictedTransform(0), st_Transform); }