std::tuple<Segment, bool> ew::VectorTerrainWorld::getFirstColliding(const Segment &motion)
{
Segment closest;
bool collisions = false;
float closestDistanceSquared = -1;
// Find closest terrain collision
segmentTree.query(motion, [&](Segment const& segment){
if(segment.intersects(motion))
{
if(segment.delta().cross(motion.delta()) <= 0)
{
Vec2D collisionPoint = segment.intersectionPoint(motion);
float distanceSquared = (collisionPoint - motion.a).lengthSquared();
if(closestDistanceSquared < 0 || distanceSquared < closestDistanceSquared)
{
closestDistanceSquared = distanceSquared;
closest = segment;
collisions = true;
}
}
}
return false;
});
return std::make_tuple(closest, collisions);
}
void WindowEvaluator::obstacle_robot(vector<Window>& windows, Point origin,
Segment target, Point bot_pos) {
auto n = (bot_pos - origin).normalized();
auto t = n.perpCCW();
auto r = Robot_Radius + Ball_Radius;
Segment seg{bot_pos - n * Robot_Radius + t * r,
bot_pos - n * Robot_Radius - t * r};
if (debug) {
system->drawLine(seg, QColor{"Red"}, "Debug");
}
auto end = target.delta().magsq();
array<double, 2> extent = {0, end};
for (int i = 0; i < 2; i++) {
Line edge{origin, seg.pt[i]};
auto d = edge.delta().magsq();
Point intersect;
if (edge.intersects(Line(target), &intersect) &&
(intersect - origin).dot(edge.delta()) > d) {
auto f = (intersect - target.pt[0]).dot(target.delta());
if (f < 0)
extent[i] = 0;
else if (f > end)
extent[i] = end;
else
extent[i] = f;
} else {
return;
}
}
obstacle_range(windows, extent[0], extent[1]);
}
int ew::VectorTerrainWorld::getCollideCount(const Segment &motion)
{
int result = 0;
segmentTree.query(motion, [&](Segment const& segment){
if(segment.intersects(motion))
{
if(segment.delta().cross(motion.delta()) <= 0)
{
result += 1;
}
}
return false;
});
return result;
}
SegmentTree::SegmentList ew::VectorTerrainWorld::getColliding(const Segment &motion)
{
SegmentTree::SegmentList result;
segmentTree.query(motion, [&](Segment const& segment){
if(segment.intersects(motion))
{
if(segment.delta().cross(motion.delta()) <= 0)
{
result.push_back(segment);
}
}
return false;
});
return result;
}
Vec2D Segment::intersectionPoint(Segment const& other) const
{
Vec2D selfDelta = delta();
Vec2D otherDelta = other.delta();
if(selfDelta.cross(otherDelta) != 0)
{
float t = other.a.subtract(a).cross(otherDelta) / selfDelta.cross(otherDelta);
float u = other.a.subtract(a).cross(selfDelta) / selfDelta.cross(otherDelta);
if(t >= 0 && t <= 1 && u >= 0 && u <= 1)
{
return a.add(selfDelta.scale(t));
}
}
return Vec2D(0, 0);
}
bool Segment::intersects(Segment const& other) const
{
Vec2D selfDelta = delta();
Vec2D otherDelta = other.delta();
if(selfDelta.cross(otherDelta) != 0)
{
float t = other.a.subtract(a).cross(otherDelta) / selfDelta.cross(otherDelta);
float u = other.a.subtract(a).cross(selfDelta) / selfDelta.cross(otherDelta);
if(t >= 0 && t <= 1 && u >= 0 && u <= 1)
{
return true;
}
}
return false;
}
void Polygon::init(const Segment& seg, float r, float length) {
Point dir;
if (length) {
dir = seg.delta() / length;
} else {
// Degenerate segment, so direction doesn't matter as long as it's a
// unit vector.
dir = Point(1, 0);
}
Point v = dir * r;
Point u = v.perpCCW();
vertices.resize(4);
vertices[0] = seg.pt[0] - u - v;
vertices[1] = seg.pt[0] + u - v;
vertices[2] = seg.pt[1] + u + v;
vertices[3] = seg.pt[1] - u + v;
}
WindowingResult WindowEvaluator::eval_pt_to_seg(Point origin, Segment target) {
auto end = target.delta().magsq();
// if target is a zero-length segment, there are no windows
if (end == 0) return make_pair(vector<Window>{}, boost::none);
if (debug) {
system->drawLine(target, QColor{"Blue"}, "Debug");
}
vector<Window> windows = {Window{0, end}};
// apply the obstacles
vector<Robot*> bots(system->self.size() + system->opp.size());
auto filter_predicate = [&](const Robot* bot) -> bool {
return bot != nullptr && bot->visible &&
find(excluded_robots.begin(), excluded_robots.end(), bot) ==
excluded_robots.end();
};
auto end_it = copy_if(system->self.begin(), system->self.end(),
bots.begin(), filter_predicate);
end_it = copy_if(system->opp.begin(), system->opp.end(), end_it,
filter_predicate);
bots.resize(distance(bots.begin(), end_it));
vector<Point> bot_locations;
for_each(bots.begin(), bots.end(), [&bot_locations](Robot* bot) {
bot_locations.push_back(bot->pos);
});
bot_locations.insert(bot_locations.end(),
hypothetical_robot_locations.begin(),
hypothetical_robot_locations.end());
for (auto& pos : bot_locations) {
auto d = (pos - origin).mag();
// whether or not we can ship over this bot
auto chip_overable = chip_enabled &&
(d < max_chip_range - Robot_Radius) &&
(d > min_chip_range + Robot_Radius);
if (!chip_overable) {
obstacle_robot(windows, origin, target, pos);
}
}
auto p0 = target.pt[0];
auto delta = target.delta() / end;
for (auto& w : windows) {
w.segment = Segment{p0 + delta * w.t0, p0 + delta * w.t1};
w.a0 = RadiansToDegrees((w.segment.pt[0] - origin).angle());
w.a1 = RadiansToDegrees((w.segment.pt[1] - origin).angle());
fill_shot_success(w, origin);
}
boost::optional<Window> best{
!windows.empty(), *max_element(windows.begin(), windows.end(),
[](Window& a, Window& b) -> bool {
return a.segment.delta().magsq() <
b.segment.delta().magsq();
})};
if (debug) {
if (best) {
system->drawLine(Segment{origin, best->segment.center()},
QColor{"Green"}, "Debug");
}
for (Window& window : windows) {
system->drawLine(window.segment, QColor{"Green"}, "Debug");
system->drawText(QString::number(window.shot_success),
window.segment.center() + Point(0, 0.1),
QColor{"Green"}, "Debug");
}
}
return make_pair(windows, best);
}