46 lines
1.9 KiB
C++
46 lines
1.9 KiB
C++
#include "mg_obstacles/static_obstacle.hpp"
|
|
#include "mg_obstacles/sdf.hpp"
|
|
|
|
namespace mg {
|
|
|
|
double StaticObstacle::width = 0.1;
|
|
double StaticObstacle::height = 0.4;
|
|
|
|
StaticObstacle::StaticObstacle(const Json::Value& json) :
|
|
pos(json.get("x", 1.0).asDouble(), json.get("y", 1.0).asDouble()),
|
|
horizontal(json.get("horizontal", true).asBool()) { }
|
|
|
|
double StaticObstacle::distance(glm::dvec2 position) const {
|
|
const double width = StaticObstacle::width;
|
|
const double height = StaticObstacle::height;
|
|
if (!horizontal) {
|
|
return boxSdf(pos + 0.5 * glm::dvec2{ width, -height }, glm::dvec2{ width, height } * 0.5, position);
|
|
} else {
|
|
return boxSdf(pos + 0.5 * glm::dvec2{ height, -width }, glm::dvec2{ height, width } * 0.5, position);
|
|
}
|
|
}
|
|
|
|
double StaticObstacle::distanceBox(glm::dvec2 position, glm::dvec2 size, glm::dmat2 rot_mat) const {
|
|
const double width = StaticObstacle::width;
|
|
const double height = StaticObstacle::height;
|
|
if (!horizontal) {
|
|
return boxToBox(
|
|
pos + 0.5 * glm::dvec2{ width, -height }, glm::dvec2{ width, height }, position, size, rot_mat);
|
|
} else {
|
|
return boxToBox(
|
|
pos + 0.5 * glm::dvec2{ height, -width }, glm::dvec2{ height, width }, position, size, rot_mat);
|
|
}
|
|
}
|
|
|
|
bool StaticObstacle::contains(glm::dvec2 position, double radius) const {
|
|
const double width = StaticObstacle::width;
|
|
const double height = StaticObstacle::height;
|
|
if (!horizontal) {
|
|
return inBoxSdf(
|
|
pos + 0.5 * glm::dvec2{ width, -height }, glm::dvec2{ width, height } * 0.5, position, radius);
|
|
} else {
|
|
return inBoxSdf(
|
|
pos + 0.5 * glm::dvec2{ height, -width }, glm::dvec2{ height, width } * 0.5, position, radius);
|
|
}
|
|
}
|
|
} |