static waypoint_follower::lane rule_stopline(const waypoint_follower::lane& msg, double acceleration, size_t fixed_cnt)
{
waypoint_follower::lane computations = msg;
std::vector<size_t> indexes = search_stopline_index(msg);
if (indexes.empty())
return computations;
for (const size_t& i : indexes)
computations = apply_acceleration(computations, acceleration, i, 1, 0);
std::reverse(computations.waypoints.begin(), computations.waypoints.end());
std::vector<size_t> reverse_indexes;
for (const size_t& i : indexes)
reverse_indexes.push_back(msg.waypoints.size() - i - 1);
std::reverse(reverse_indexes.begin(), reverse_indexes.end());
for (const size_t& i : reverse_indexes)
computations = apply_acceleration(computations, acceleration, i, fixed_cnt, 0);
std::reverse(computations.waypoints.begin(), computations.waypoints.end());
return computations;
}
static waypoint_follower::lane rule_crossroad(const waypoint_follower::lane& msg, double acceleration)
{
waypoint_follower::lane computations = msg;
bool crossroad;
std::vector<size_t> start_indexes;
std::vector<size_t> end_indexes;
for (size_t i = 0; i < msg.waypoints.size(); ++i) {
map_file::DTLane dtlane;
dtlane.r = msg.waypoints[i].dtlane.r;
if (i == 0) {
crossroad = is_crossroad(dtlane);
continue;
}
if (!crossroad && is_crossroad(dtlane)) {
start_indexes.push_back(i);
crossroad = true;
continue;
}
if (crossroad && !is_crossroad(dtlane)) {
end_indexes.push_back(i - 1);
crossroad = false;
}
}
for (const size_t& i : end_indexes)
computations = apply_acceleration(computations, acceleration, i, 1,
computations.waypoints[i].twist.twist.linear.x);
std::reverse(computations.waypoints.begin(), computations.waypoints.end());
std::vector<size_t> reverse_start_indexes;
for (const size_t& i : start_indexes)
reverse_start_indexes.push_back(msg.waypoints.size() - i - 1);
std::reverse(reverse_start_indexes.begin(), reverse_start_indexes.end());
for (const size_t& i : reverse_start_indexes)
computations = apply_acceleration(computations, acceleration, i, 1,
computations.waypoints[i].twist.twist.linear.x);
std::reverse(computations.waypoints.begin(), computations.waypoints.end());
return computations;
}
void Step(float dt) {
if (impulse[0]) force[0]+=impulse[0];
if (impulse[1]) force[1]+=impulse[1];
if (impulse[2]) force[2]+=impulse[2];
float weight;
weight = mass * gravity[1];
impulse[0] = impulse[0] * 0.9f; if (abs(impulse[0])<0.001f) impulse[0] = 0.0f;
impulse[1] = impulse[1] * 0.9f; if (abs(impulse[1])<0.001f) impulse[1] = 0.0f;
impulse[2] = impulse[2] * 0.9f; if (abs(impulse[2])<0.001f) impulse[2] = 0.0f;
if (!active) {
return;
}
apply_force(&accel[0], &force[0], mass);
if (active) accel[0] += gravity[0];
if (active) accel[1] += gravity[1];
if (active) accel[2] += gravity[2];
apply_acceleration(&vel[0], &accel[0], dt);
vel[0]*= friction;
vel[1]*= friction;
vel[2]*= friction;
apply_velocity(&pos[0], &vel[0], dt);
momentum[0] = mass * vel[0];
momentum[1] = mass * vel[1];
momentum[2] = mass * vel[2];
}
