mirror of
https://github.com/OpenSpace/OpenSpace.git
synced 2026-03-14 01:10:11 -05:00
224 lines
10 KiB
C++
224 lines
10 KiB
C++
/*****************************************************************************************
|
|
* *
|
|
* OpenSpace *
|
|
* *
|
|
* Copyright (c) 2014-2019 *
|
|
* *
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy of this *
|
|
* software and associated documentation files (the "Software"), to deal in the Software *
|
|
* without restriction, including without limitation the rights to use, copy, modify, *
|
|
* merge, publish, distribute, sublicense, and/or sell copies of the Software, and to *
|
|
* permit persons to whom the Software is furnished to do so, subject to the following *
|
|
* conditions: *
|
|
* *
|
|
* The above copyright notice and this permission notice shall be included in all copies *
|
|
* or substantial portions of the Software. *
|
|
* *
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, *
|
|
* INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A *
|
|
* PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT *
|
|
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF *
|
|
* CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE *
|
|
* OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. *
|
|
****************************************************************************************/
|
|
|
|
#include <modules/autonavigation/avoidcollisioncurve.h>
|
|
|
|
#include <modules/autonavigation/autonavigationmodule.h>
|
|
#include <modules/autonavigation/helperfunctions.h>
|
|
#include <modules/autonavigation/waypoint.h>
|
|
#include <openspace/engine/globals.h>
|
|
#include <openspace/engine/moduleengine.h>
|
|
#include <openspace/query/query.h>
|
|
#include <openspace/scene/scenegraphnode.h>
|
|
#include <ghoul/logging/logmanager.h>
|
|
#include <glm/gtx/projection.hpp>
|
|
#include <algorithm>
|
|
#include <vector>
|
|
|
|
namespace {
|
|
constexpr const char* _loggerCat = "AvoidCollisionCurve";
|
|
const double Epsilon = 1E-7;
|
|
|
|
const double CloseToNodeThresholdFactor = 5.0;
|
|
const double AvoidCollisionDistanceFactor = 3.0;
|
|
const double CollisionBufferSizeFactor = 1.0;
|
|
const int MaxAvoidCollisionSteps = 10;
|
|
|
|
} // namespace
|
|
|
|
namespace openspace::autonavigation {
|
|
|
|
AvoidCollisionCurve::AvoidCollisionCurve(const Waypoint& start, const Waypoint& end) {
|
|
AutoNavigationModule* module = global::moduleEngine.module<AutoNavigationModule>();
|
|
_relevantNodes = module->AutoNavigationHandler().relevantNodes();
|
|
|
|
glm::dvec3 startNodeCenter = start.node()->worldPosition();
|
|
glm::dvec3 endNodeCenter = end.node()->worldPosition();
|
|
double startNodeRadius = start.nodeDetails.validBoundingSphere;
|
|
double endNodeRadius = end.nodeDetails.validBoundingSphere;
|
|
glm::dvec3 startViewDirection = glm::normalize(
|
|
start.rotation() * glm::dvec3(0.0, 0.0, -1.0)
|
|
);
|
|
glm::dvec3 endViewDirection = glm::normalize(
|
|
end.rotation() * glm::dvec3(0.0, 0.0, -1.0)
|
|
);
|
|
|
|
// Add control points for a catmull-rom spline, first and last will not be intersected
|
|
_points.push_back(start.position());
|
|
_points.push_back(start.position());
|
|
|
|
// Add an extra point to first go backwards if starting close to planet
|
|
glm::dvec3 nodeToStart = start.position() - startNodeCenter;
|
|
double distanceToStartNode = glm::length(nodeToStart);
|
|
|
|
if (distanceToStartNode < CloseToNodeThresholdFactor * startNodeRadius) {
|
|
double distance = startNodeRadius;
|
|
glm::dvec3 newPos = start.position() + distance * glm::normalize(nodeToStart);
|
|
_points.push_back(newPos);
|
|
}
|
|
|
|
// Add point for moving out if the end state is in opposite direction
|
|
//TODO: determine if its best to compare to end orientation or position
|
|
glm::dvec3 startToEnd = end.position() - start.position();
|
|
double cosAngleToTarget = glm::dot(normalize(-startViewDirection), normalize(startToEnd));
|
|
bool targetInOppositeDirection = cosAngleToTarget > 0.7;
|
|
|
|
// TODO: reduce magical numbers / create constants
|
|
if (targetInOppositeDirection) {
|
|
glm::dquat midleRotation = glm::slerp(start.rotation(), end.rotation(), 0.5);
|
|
glm::dvec3 middleViewDirection = midleRotation * glm::dvec3(0.0, 0.0, -1.0);
|
|
double stepOutdistance = 0.4 * glm::length(startToEnd);
|
|
|
|
glm::dvec3 newPosition = start.position() + 0.2 * startToEnd -
|
|
stepOutdistance * glm::normalize(middleViewDirection);
|
|
|
|
_points.push_back(newPosition);
|
|
}
|
|
|
|
// Add an extra point to approach target
|
|
glm::dvec3 nodeToEnd = end.position() - endNodeCenter;
|
|
double distanceToEndNode = glm::length(nodeToEnd);
|
|
|
|
if (distanceToEndNode < CloseToNodeThresholdFactor * endNodeRadius) {
|
|
double distance = endNodeRadius;
|
|
glm::dvec3 newPos = end.position() + distance * glm::normalize(nodeToEnd);
|
|
_points.push_back(newPos);
|
|
}
|
|
|
|
_points.push_back(end.position());
|
|
_points.push_back(end.position());
|
|
|
|
// Create extra points to avoid collision
|
|
removeCollisions();
|
|
|
|
_nrSegments = _points.size() - 3;
|
|
|
|
initParameterIntervals();
|
|
}
|
|
|
|
// Interpolate a list of control points and knot times
|
|
glm::dvec3 AvoidCollisionCurve::interpolate(double u) {
|
|
if (u < Epsilon)
|
|
return _points[1];
|
|
if (u > (1.0 - Epsilon))
|
|
return *(_points.end() - 2);
|
|
|
|
std::vector<double>::iterator segmentEndIt =
|
|
std::lower_bound(_parameterIntervals.begin(), _parameterIntervals.end(), u);
|
|
unsigned int index = (segmentEndIt - 1) - _parameterIntervals.begin();
|
|
|
|
double segmentStart = _parameterIntervals[index];
|
|
double segmentDuration = (_parameterIntervals[index + 1] - _parameterIntervals[index]);
|
|
double uSegment = (u - segmentStart) / segmentDuration;
|
|
|
|
return interpolation::catmullRom(
|
|
uSegment,
|
|
_points[index],
|
|
_points[index + 1],
|
|
_points[index + 2],
|
|
_points[index + 3],
|
|
1.0
|
|
);
|
|
}
|
|
|
|
// Try to reduce the risk of collision by approximating the curve with linear segments.
|
|
// If a collision happens, create a new point for the path to go through, in an attempt to
|
|
// avoid that collision
|
|
void AvoidCollisionCurve::removeCollisions(int step) {
|
|
int nrSegments = _points.size() - 3;
|
|
|
|
if (step > MaxAvoidCollisionSteps)
|
|
return; // TODO: handle better / present warning if early out
|
|
|
|
for (int i = 0; i < nrSegments; ++i) {
|
|
const glm::dvec3 lineStart = _points[i + 1];
|
|
const glm::dvec3 lineEnd = _points[i + 2];
|
|
|
|
for (SceneGraphNode* node : _relevantNodes) {
|
|
// do collision check in relative coordinates, to avoid huge numbers
|
|
const glm::dmat4 modelTransform = node->modelTransform();
|
|
glm::dvec3 p1 = glm::inverse(modelTransform) * glm::dvec4(lineStart, 1.0);
|
|
glm::dvec3 p2 = glm::inverse(modelTransform) * glm::dvec4(lineEnd, 1.0);
|
|
|
|
// sphere to check for collision
|
|
double radius = node->boundingSphere();
|
|
glm::dvec3 center = glm::dvec3(0.0, 0.0, 0.0);
|
|
|
|
// add a buffer, so we don't pass too close to the node
|
|
// dont't add it if any point is inside the buffer
|
|
double buffer = CollisionBufferSizeFactor * node->boundingSphere();
|
|
bool p1IsInside = helpers::isPointInsideSphere(p1, center, radius + buffer);
|
|
bool p2IsInside = helpers::isPointInsideSphere(p2, center, radius + buffer);
|
|
if (!p1IsInside && !p2IsInside) {
|
|
radius += buffer;
|
|
}
|
|
|
|
glm::dvec3 intersectionPointModelCoords;
|
|
bool collision = helpers::lineSphereIntersection(
|
|
p1,
|
|
p2,
|
|
center,
|
|
radius,
|
|
intersectionPointModelCoords
|
|
);
|
|
|
|
if (collision) {
|
|
//LINFO(fmt::format("Collision with node: {}!", node->identifier()));
|
|
glm::dvec3 collisionPoint = modelTransform *
|
|
glm::dvec4(intersectionPointModelCoords, 1.0);
|
|
|
|
// before collision response, make sure none of the points are inside the node
|
|
bool isStartInsideNode = helpers::isPointInsideSphere(p1, center, radius);
|
|
bool isEndInsideNode = helpers::isPointInsideSphere(p2, center, radius);
|
|
if (isStartInsideNode || isEndInsideNode) {
|
|
LWARNING(fmt::format(
|
|
"Something went wrong! "
|
|
"At least one point in the path is inside node: {}",
|
|
node->identifier()
|
|
));
|
|
break;
|
|
}
|
|
|
|
// to avoid collision, take a step in an orhtogonal direction of the
|
|
// collision point and add a new point
|
|
glm::dvec3 lineDirection = glm::normalize(lineEnd - lineStart);
|
|
glm::dvec3 nodeCenter = node->worldPosition();
|
|
glm::dvec3 collisionPointToCenter = nodeCenter - collisionPoint;
|
|
glm::dvec3 parallell = glm::proj(collisionPointToCenter, lineDirection);
|
|
glm::dvec3 orthogonal = collisionPointToCenter - parallell;
|
|
|
|
double avoidCollisionDistance = AvoidCollisionDistanceFactor * radius;
|
|
glm::dvec3 extraKnot = collisionPoint -
|
|
avoidCollisionDistance * glm::normalize(orthogonal);
|
|
_points.insert(_points.begin() + i + 2, extraKnot);
|
|
|
|
removeCollisions(++step);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
} // namespace openspace::autonavigation
|