83 lines
2.8 KiB
C++
83 lines
2.8 KiB
C++
#pragma once
|
|
#include "handlers/dwm_core.hpp"
|
|
|
|
#include "glm/gtx/norm.hpp"
|
|
#include "glm/gtx/rotate_vector.hpp"
|
|
#include "glm/gtx/vector_angle.hpp"
|
|
#include <glm/gtc/constants.hpp>
|
|
|
|
namespace mg {
|
|
|
|
template <>
|
|
inline bool DWM<MgNavigationServer::LookAt>::target_check() {
|
|
|
|
const double a = glm::abs(theta - target_ornt);
|
|
const double b = (a > glm::pi<double>()) ? glm::two_pi<double>() - a : a;
|
|
|
|
return b > goal->tolerance;
|
|
}
|
|
|
|
template <>
|
|
inline void DWM<MgNavigationServer::LookAt>::target_init() {
|
|
|
|
target_pos = glm::vec2(goal->x, goal->y);
|
|
|
|
const double angle = glm::angle(glm::dvec2(1, 0), glm::normalize(target_pos - pos));
|
|
const double cross = glm::cross(glm::dvec3(1, 0, 0), glm::dvec3(target_pos - pos, 0)).z;
|
|
const double sign = (cross < 0) ? -1 : 1;
|
|
|
|
target_ornt = angle * sign;
|
|
}
|
|
|
|
template <>
|
|
inline void DWM<MgNavigationServer::LookAt>::target_update() {
|
|
|
|
const double angle = glm::angle(glm::dvec2(1, 0), glm::normalize(target_pos - pos));
|
|
const double cross = glm::cross(glm::dvec3(1, 0, 0), glm::dvec3(target_pos - pos, 0)).z;
|
|
const double sign = (cross < 0) ? -1 : 1;
|
|
|
|
target_ornt = angle * sign;
|
|
}
|
|
|
|
template <>
|
|
inline bool DWM<MgNavigationServer::LookAt>::condition_check() {
|
|
return false;
|
|
}
|
|
|
|
template <>
|
|
inline double DWM<MgNavigationServer::LookAt>::calc_score(int vl, int vr) {
|
|
constexpr double delta = 0.5;
|
|
const auto [v, w] = to_vel(step * vl, step * vr);
|
|
const double n_theta = theta + w * delta;
|
|
|
|
double dist_old = glm::abs(target_ornt - theta);
|
|
double dist_new = glm::abs(target_ornt - n_theta);
|
|
|
|
dist_old = (dist_old > glm::pi<double>()) ? glm::two_pi<double>() - dist_old : dist_old;
|
|
dist_new = (dist_new > glm::pi<double>()) ? glm::two_pi<double>() - dist_new : dist_new;
|
|
|
|
const double score = goal->ornt_mult * (dist_old - dist_new);
|
|
|
|
return score;
|
|
}
|
|
|
|
template <>
|
|
inline void DWM<MgNavigationServer::LookAt>::populate_candidates(std::vector<int>& vl,
|
|
std::vector<int>& vr,
|
|
int dimx,
|
|
int) {
|
|
vl.clear();
|
|
vr.clear();
|
|
|
|
for (int i = -dimx / 2; i <= dimx / 2; i++) {
|
|
auto [v, w] = to_vel(step * (cvl + i), step * (cvr + i));
|
|
|
|
if (step * abs(cvl - i) <= goal->max_wheel_speed && step * abs(cvr + i) <= goal->max_wheel_speed
|
|
&& glm::abs(w) < goal->max_angular) {
|
|
vl.push_back(cvl - i);
|
|
vr.push_back(cvr + i);
|
|
}
|
|
}
|
|
}
|
|
|
|
} |