1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
|
#include "link.h"
#include <glm/gtx/transform.hpp>
#include <location.h>
#include <maths.h>
#include <ray.h>
#include <tuple>
Link::Link(End endA, End endB, float len) : ends {{std::move(endA), std::move(endB)}}, length {len} { }
LinkCurve::LinkCurve(GlobalPosition3D centre, RelativeDistance radius, Arc arc) :
centreBase {centre}, radius {radius}, arc {std::move(arc)}
{
}
bool
operator<(const GlobalPosition3D & left, const GlobalPosition3D & right)
{
// NOLINTNEXTLINE(hicpp-use-nullptr,modernize-use-nullptr)
return std::tie(left.x, left.y, left.z) < std::tie(right.x, right.y, right.z);
}
bool
operator<(const Node & left, const Node & right)
{
return left.pos < right.pos;
}
Location
LinkStraight::positionAt(RelativeDistance dist, unsigned char start) const
{
const auto endNodes = std::make_pair(ends[start].node.get(), ends[1 - start].node.get());
const auto diff = ::difference(endNodes.second->pos, endNodes.first->pos);
const auto directionVector = glm::normalize(diff);
return Location {
.pos = endNodes.first->pos + (vehiclePositionOffset() + directionVector * dist),
.rot = {vector_pitch(directionVector), vector_yaw(directionVector), 0},
};
}
bool
LinkStraight::intersectRay(const Ray<GlobalPosition3D> & ray) const
{
static constexpr auto PROXIMITY = 1'000;
return ray.passesCloseToEdges(std::array {ends.front().node->pos, ends.back().node->pos}, PROXIMITY);
}
std::vector<GlobalPosition3D>
LinkStraight::getBase(RelativeDistance width) const
{
const auto start = ends.front().node->pos;
const auto end = ends.back().node->pos;
const auto direction = (vector_normal(normalize(::difference(start, end).xy())) * width / 2.F) || 0.F;
return {
start - direction,
start + direction,
end - direction,
end + direction,
};
}
Location
LinkCurve::positionAt(float dist, unsigned char start) const
{
static constexpr std::array DIR_OFFSET {half_pi, -half_pi};
const auto frac = dist / length;
const auto endNodes = std::make_pair(ends[start].node.get(), ends[1 - start].node.get());
const auto arcEndAngles = std::make_pair(arc[start], arc[1 - start]);
const auto ang = glm::mix(arcEndAngles.first, arcEndAngles.second, frac);
const auto relPos = (sincos(ang) || 0.F) * radius;
const auto relClimb = vehiclePositionOffset()
+ RelativePosition3D {0, 0,
static_cast<RelativeDistance>(endNodes.first->pos.z - centreBase.z)
+ (static_cast<RelativeDistance>(endNodes.second->pos.z - endNodes.first->pos.z) * frac)};
const auto pitch {vector_pitch(difference(endNodes.second->pos, endNodes.first->pos) / length)};
return Location {
.pos = GlobalPosition3D(relPos + relClimb) + centreBase,
.rot = {pitch, normalize(ang + DIR_OFFSET[start]), 0},
};
}
bool
LinkCurve::intersectRay(const Ray<GlobalPosition3D> & ray) const
{
const auto e0p = ends[0].node->pos.z;
const auto e1p = ends[1].node->pos.z;
const auto slength = round_frac(length / 2.F, 5.F);
const auto segs = std::round(15.F * slength / std::pow(radius, 0.7F));
const auto step {glm::vec<2, RelativeDistance> {arc.length(), e1p - e0p} / segs};
auto segCount = static_cast<std::size_t>(std::lround(segs)) + 1;
std::vector<GlobalPosition3D> points;
points.reserve(segCount);
for (std::remove_const_t<decltype(step)> swing = {arc.first, centreBase.z - e0p}; segCount;
swing += step, --segCount) {
points.emplace_back(centreBase + ((sincos(swing.x) * radius) || swing.y));
}
return ray.passesCloseToEdges(points, 1.F);
}
std::vector<GlobalPosition3D>
LinkCurve::getBase(RelativeDistance width) const
{
const auto start = ends.front().node->pos;
const auto end = ends.back().node->pos;
const auto segs = std::ceil(std::sqrt(radius) * 0.02F * arc.length());
const auto step = glm::vec<2, RelativeDistance> {arc.length(), end.z - start.z} / segs;
auto segCount = static_cast<size_t>(segs) + 1;
std::vector<GlobalPosition3D> out;
out.reserve(segCount);
for (RelativePosition2D swing = {arc.first, centreBase.z - start.z}; segCount != 0U; swing += step, --segCount) {
const auto direction = sincos(swing.x);
const auto linkCentre = centreBase + ((direction * radius) || swing.y);
const auto toEdge = (direction * width / 2.F) || 0.F;
out.emplace_back(linkCentre + toEdge);
out.emplace_back(linkCentre - toEdge);
}
return out;
}
|
