mirror/OpenSpace
1
0
Fork 0
mirror of https://github.com/OpenSpace/OpenSpace.git synced 2026-05-01 08:19:51 -05:00
Code Issues Packages Projects Releases Wiki Activity

Fix problems in avoid collision path when start and end position are the same

Browse Source
This commit is contained in:
Emma Broman
2021-11-17 09:39:03 +01:00
parent d7565f65ee
commit 73895e4c62
1 changed files with 21 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/navigation/pathcurves/avoidcollisioncurve.cpp
+21 -12
View File
@@ -44,6 +44,8 @@ namespace {
constexpr const double AvoidCollisionDistanceRadiusMultiplier = 3.0;
constexpr const double CollisionBufferSizeRadiusMultiplier = 1.0;
constexpr const int MaxAvoidCollisionSteps = 10;
constexpr const double Epsilon = 1e-5;
} // namespace
namespace openspace::interaction {
@@ -76,20 +78,23 @@ AvoidCollisionCurve::AvoidCollisionCurve(const Waypoint& start, const Waypoint&
_points.push_back(newPos);
}
// Add point for moving out if the end state is in opposite direction
glm::dvec3 startToEnd = end.position() - start.position();
double cosAngleToTarget = glm::dot(normalize(-startViewDir), normalize(startToEnd));
bool targetInOppositeDirection = cosAngleToTarget > 0.7;
const glm::dvec3 startToEnd = end.position() - start.position();
if (targetInOppositeDirection) {
const glm::dquat midleRot = glm::slerp(start.rotation(), end.rotation(), 0.5);
const glm::dvec3 middleViewDir = ghoul::viewDirection(midleRot);
const double stepOutDistance = 0.4 * glm::length(startToEnd);
if (glm::length(startToEnd) > 0.0) {
// Add point for moving out if the end state is in opposite direction
double cosAngleToTarget = glm::dot(normalize(-startViewDir), normalize(startToEnd));
bool targetInOppositeDirection = cosAngleToTarget > 0.7;
glm::dvec3 newPos = start.position() + 0.2 * startToEnd -
stepOutDistance * glm::normalize(middleViewDir);
if (targetInOppositeDirection) {
const glm::dquat midleRot = glm::slerp(start.rotation(), end.rotation(), 0.5);
const glm::dvec3 middleViewDir = ghoul::viewDirection(midleRot);
const double stepOutDistance = 0.4 * glm::length(startToEnd);
_points.push_back(newPos);
glm::dvec3 newPos = start.position() + 0.2 * startToEnd -
stepOutDistance * glm::normalize(middleViewDir);
_points.push_back(newPos);
}
}
// Add an extra point to approach target
@@ -119,11 +124,15 @@ void AvoidCollisionCurve::removeCollisions(int step) {
return;
}
const int nSegments = static_cast<int>( _points.size() - 3);
const int nSegments = static_cast<int>(_points.size() - 3);
for (int i = 0; i < nSegments; ++i) {
const glm::dvec3 lineStart = _points[i + 1];
const glm::dvec3 lineEnd = _points[i + 2];
if (glm::distance(lineEnd, lineStart) - Epsilon < 0.0) {
continue; // Start and end position are the same. Go to next segment
}
for (SceneGraphNode* node : _relevantNodes) {
// Do collision check in relative coordinates, to avoid huge numbers
const glm::dmat4 modelTransform = node->modelTransform();