From c7b47fc2c005752c95adc146238da316258cb476 Mon Sep 17 00:00:00 2001 From: Emma Broman Date: Tue, 14 Dec 2021 16:09:17 +0100 Subject: [PATCH] Slight improvement to AvoidCollision camera path Fixes "bouncing motion" that sometime happened when going to a position that is close to the original position --- src/navigation/pathcurves/avoidcollisioncurve.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/navigation/pathcurves/avoidcollisioncurve.cpp b/src/navigation/pathcurves/avoidcollisioncurve.cpp index 5c1c1d2623..460bca6bf5 100644 --- a/src/navigation/pathcurves/avoidcollisioncurve.cpp +++ b/src/navigation/pathcurves/avoidcollisioncurve.cpp @@ -80,8 +80,11 @@ AvoidCollisionCurve::AvoidCollisionCurve(const Waypoint& start, const Waypoint& const glm::dvec3 startToEnd = end.position() - start.position(); - if (glm::length(startToEnd) > 0.0) { - // Add point for moving out if the end state is in opposite direction + // Add point for moving out if the end state is far away and in opposite direction. + // This helps with avoiding fast rotation in the center of the path + const double maxRadius = std::max(startNodeRadius, endNodeRadius); + bool nodesAreDifferent = start.nodeIdentifier() != end.nodeIdentifier(); + if (glm::length(startToEnd) > 0.5 * maxRadius && nodesAreDifferent) { double cosAngleToTarget = glm::dot(normalize(-startViewDir), normalize(startToEnd)); bool targetInOppositeDirection = cosAngleToTarget > 0.7;