void object::refresh(){
fx = bd->GetFixtureList();
b2Transform trans = bd->GetTransform();
trans.q = b2Rot(0);
fx->GetShape()->ComputeAABB(&aabb,trans,0);
}
directional_sensor(phys_sensor_defn const& defn):
m_body(defn.body),
m_pos(defn.pos),
m_angle(defn.orientation),
m_range(defn.range)
{
m_dir = b2Mul(b2Rot(m_angle), b2Vec2(0.f, 1.f));
}
void Robot::postStep()
{
// Substract covered distance.
b2Vec2 newPosition = robotBody->GetPosition();
float newAngle = robotBody->GetAngle();
distanceSetpoint -= b2Dot(newPosition - position, b2Rot((newAngle + angle)/2).GetXAxis());
angleSetpoint -= newAngle - angle;
position = newPosition;
angle = newAngle;
}
fan_sensor(phys_sensor_defn const& defn):
m_angle(defn.orientation),
m_count(0)
{
const int SegmentsPerQuadrant = 8;
auto fan_data = boost::any_cast<fan_sensor_data>(defn.type_specific);
const size_t num_segments = (int)std::ceil(SegmentsPerQuadrant * fan_data.field_of_view / (b2_pi / 2));
const size_t max_segments_per_poly = b2_maxPolygonVertices - 2;
const size_t num_polys = num_segments / max_segments_per_poly + (num_segments % max_segments_per_poly > 0 ? 1 : 0);
size_t segment_idx = 0;
for(size_t i = 0; i < num_polys; ++i)
{
std::vector< b2Vec2 > points;
points.emplace_back(b2Vec2{ 0.f, 0.f });
b2Vec2 base(0.f, defn.range);
base = b2Mul(b2Rot(-fan_data.field_of_view / 2), base);
points.emplace_back(b2Mul(b2Rot(fan_data.field_of_view * segment_idx / num_segments), base));
auto end = std::min(segment_idx + max_segments_per_poly, num_segments);
for(; segment_idx < end; ++segment_idx)
{
points.emplace_back(b2Mul(b2Rot(fan_data.field_of_view * (segment_idx + 1) / num_segments), base));
}
b2FixtureDef fd;
b2PolygonShape poly;
transform_points(points, b2Transform(defn.pos, b2Rot(defn.orientation)));
poly.Set(points.data(), points.size());
fd.shape = &poly;
fd.isSensor = true;
//fd.filter
auto fix = defn.body->CreateFixture(&fd);
set_fixture_data(fix, entity_fix_data{
entity_fix_data::Type::Sensor,
entity_fix_data::val_t{ static_cast<contact_based_sensor*>(this) }
});
}
}
int Shape::computeAABB(lua_State *L) const
{
float x = Physics::scaleDown((float)luaL_checknumber(L, 1));
float y = Physics::scaleDown((float)luaL_checknumber(L, 2));
float r = (float)luaL_checknumber(L, 3);
int childIndex = (int) luaL_optinteger(L, 4, 1) - 1; // Convert from 1-based index
b2Transform transform(b2Vec2(x, y), b2Rot(r));
b2AABB box;
shape->ComputeAABB(&box, transform, childIndex);
box = Physics::scaleUp(box);
lua_pushnumber(L, box.lowerBound.x);
lua_pushnumber(L, box.lowerBound.y);
lua_pushnumber(L, box.upperBound.x);
lua_pushnumber(L, box.upperBound.y);
return 4;
}
void renderEdge(b2Body* eb){
for(b2Fixture* f = eb->GetFixtureList(); f; f = f->GetNext()){
b2AABB aabb;
b2Transform trans = eb->GetTransform();
trans.q = b2Rot(0);
f->GetShape()->ComputeAABB(&aabb,trans,0);
sf::VertexArray line(sf::TrianglesStrip);
line.append(sf::Vertex(sf::Vector2f(aabb.lowerBound.x * SCALE, aabb.lowerBound.y * SCALE),sf::Color::Green));
line.append(sf::Vertex(sf::Vector2f(aabb.upperBound.x * SCALE, aabb.upperBound.y * SCALE),sf::Color::Green));
line.append(sf::Vertex(sf::Vector2f(aabb.lowerBound.x * SCALE+1, aabb.lowerBound.y * SCALE+1),sf::Color::Green));
line.append(sf::Vertex(sf::Vector2f(aabb.upperBound.x * SCALE+1, aabb.upperBound.y * SCALE+1),sf::Color::Green));
win.draw(line);
}
}
// this function returns -1 if two objects has collision
// And otherwise returns the distance
float PlanningProblem::distToObstacle(Station A, const Obstacle &ob, b2Vec2& A_point, b2Vec2& ob_point)
{
b2DistanceProxy state_proxy, ob_proxy;
state_proxy.Set(agent->shape, 0);
ob_proxy.Set(ob.shape, 1);
b2DistanceInput dist_in;
dist_in.proxyA = state_proxy;
dist_in.proxyB = ob_proxy;
dist_in.transformA = b2Transform(A.getPosition().toB2vec2(),
b2Rot(A.getPosition().Teta()));
dist_in.transformB = ob.transform;
b2SimplexCache dist_cache;
dist_cache.count = 0;
b2DistanceOutput dis_out;
b2Distance(&dis_out, &dist_cache, &dist_in);
A_point = dis_out.pointA;
ob_point = dis_out.pointB;
if(hasCollision(A, ob)) {
return -1;
}
return dis_out.distance;
}
int Shape::rayCast(lua_State *L) const
{
float p1x = Physics::scaleDown((float)luaL_checknumber(L, 1));
float p1y = Physics::scaleDown((float)luaL_checknumber(L, 2));
float p2x = Physics::scaleDown((float)luaL_checknumber(L, 3));
float p2y = Physics::scaleDown((float)luaL_checknumber(L, 4));
float maxFraction = (float)luaL_checknumber(L, 5);
float x = Physics::scaleDown((float)luaL_checknumber(L, 6));
float y = Physics::scaleDown((float)luaL_checknumber(L, 7));
float r = (float)luaL_checknumber(L, 8);
int childIndex = (int) luaL_optinteger(L, 9, 1) - 1; // Convert from 1-based index
b2RayCastInput input;
input.p1.Set(p1x, p1y);
input.p2.Set(p2x, p2y);
input.maxFraction = maxFraction;
b2Transform transform(b2Vec2(x, y), b2Rot(r));
b2RayCastOutput output;
if (!shape->RayCast(&output, input, transform, childIndex))
return 0; // No hit.
lua_pushnumber(L, output.normal.x);
lua_pushnumber(L, output.normal.y);
lua_pushnumber(L, output.fraction);
return 3;
}
void Personaje::leermovimiento(int direccion, int id_jugador){
if (this->nro_jugador == id_jugador && seleccionado[id_jugador]){
if (direccion == 3 && body->GetLinearVelocity().x < 0.7){ // para la derecha
dir_imagen = "TPTaller/imagenes/gusanitoderecha.png";
orientacion=1;
this->movio = 1;
this->mover(b2Vec2(2,0));
return;
}
if (direccion == 1 && body->GetLinearVelocity().x > -0.7 ){ // para la izquierda
dir_imagen = "TPTaller/imagenes/gusanitoizquierda.png";
orientacion=-1;
this->movio = 1;
this->mover(b2Vec2(-2,0));
return;
}
b2Vec2 posicion = this->getPosition();
b2CircleShape* over = new b2CircleShape();
over->m_radius = 0.0001;
b2Transform transformOver = b2Transform(posicion+b2Vec2(0,alto/2), b2Rot(0) );
b2World* mundo = body->GetWorld();
bool resultado;
for (b2Body* body_actual = mundo->GetBodyList()->GetNext(); body_actual; body_actual = body_actual->GetNext()){
if (body_actual->GetPosition() == body->GetPosition()) continue;
b2Transform transformada_actual = body_actual->GetTransform();
b2Fixture* fix = body_actual->GetFixtureList();
b2Shape* shape_actual = fix->GetShape();
resultado = b2TestOverlap(shape_actual,0, over , 0, transformada_actual, transformOver);
if (resultado) break;
}
if(over) delete over; over = NULL;
if((direccion == 2 && resultado) || ((direccion == 2) && (body->GetContactList() != NULL))){ // para arriba
this->mover(b2Vec2(0,-3));
this->salto = 1;
return;
}
if ( (direccion == 4 && body->GetLinearVelocity().x <0.7 ) && (body->GetContactList() != NULL) ){ // para arriba a la derecha
this->mover(b2Vec2(2,-3));
this->salto = 1;
// if(body->GetLinearVelocity().y == 0)
// this->mover(b2Vec2(2,-3));
// else
// this->mover(b2Vec2(1,-1));
// return;
}
if ( (direccion == 5 && body->GetLinearVelocity().x >-0.7 ) && (body->GetContactList() != NULL)){ // para arriba a la izq
this->mover(b2Vec2(-2,-3));
this->salto = 1;
// if(body->GetLinearVelocity().y == 0)
// this->mover(b2Vec2(-2,-3));
// else
// this->mover(b2Vec2(-1,-1));
// return;
}
if((direccion == 5) && (body->GetContactList() != NULL)){ // para arriba a la izq
this->mover(b2Vec2(-2,-3));
this->salto = 1;
}
}
}
bool Shape::testPoint(float x, float y, float r, float px, float py) const
{
b2Vec2 point(px, py);
b2Transform transform(Physics::scaleDown(b2Vec2(x, y)), b2Rot(r));
return shape->TestPoint(transform, Physics::scaleDown(point));
}
// http://www.iforce2d.net/b2dtut/constant-speed
// http://clubelek.insa-lyon.fr/joomla/fr/base_de_connaissances/informatique/asservissement_et_pilotage_de_robot_auto.php
void Robot::preStep()
{
float speed = b2Dot(robotBody->GetLinearVelocity(), b2Rot(angle).GetXAxis());
float angularSpeed = robotBody->GetAngularVelocity();
float dist = sensor->sense();
//if (dist != 0)
// printf("rf %f\n", dist);
float dist2 = turret->sense();
if (dist2 != 0)
printf("rf %f\n", dist2);
// Motion control for distances.
if(distanceControl != ControlFreerun) {
if(distanceControl == ControlSetpoint) { // Compute the desired speed.
if(fabs(distanceSetpoint) < distanceMaxSpeed * distanceMaxSpeed / 2 / distanceMaxAccel) { // Braking margin.
distanceSetSpeed = sqrt(fabs(distanceSetpoint * 2 * distanceMaxAccel)); // TODO il devrait y avoir la vitesse actuelle
}
else { // Speed up or plateau.
distanceSetSpeed = b2Min(distanceMaxSpeed,
(float)fabs(speed) + B2_TIMESTEP * distanceMaxAccel);
}
distanceSetSpeed *= sgn(distanceSetpoint);
}
// PID.
float newError = distanceSetSpeed - speed;
float errorDeriv = fabs(newError - distanceError); // fabs ?
distanceError = newError;
distanceErrorInteg += distanceError;
distanceErrorInteg = b2Clamp(distanceErrorInteg, -1.f, 1.f);
float impulse = distanceError * .1 + distanceErrorInteg * .1 + errorDeriv * .0;
robotBody->ApplyLinearImpulse(impulse * b2Rot(angle).GetXAxis(),
robotBody->GetWorldCenter());
if(this == simulator->robot)
simulator->mainWindow->debugPlot(impulse, speed);
}
// Motion control for angles.
if(angleControl != ControlFreerun) {
if(angleControl == ControlSetpoint) { // Compute the desired speed.
if(fabs(angleSetpoint) < angleMaxSpeed * angleMaxSpeed / 2 / angleMaxAccel) { // Braking margin.
angleSetSpeed = sqrt(fabs(angleSetpoint * 2 * angleMaxAccel));
}
else { // Speed up or plateau.
angleSetSpeed = b2Min(angleMaxSpeed,
(float)fabs(angularSpeed) + B2_TIMESTEP * angleMaxAccel);
}
angleSetSpeed *= sgn(angleSetpoint);
}
// PID.
float newError = angleSetSpeed - angularSpeed;
float errorDeriv = fabs(newError - angleError);
angleError = newError;
angleErrorInteg += angleError;
float impulse = angleError * .4 + angleErrorInteg * 0.2 - errorDeriv * .05;
robotBody->ApplyTorque(impulse);
}
// FIXME résister aux déplacements pas dans le sens des roues.
//simulator->robotBody->GetLinearVelocityFromLocalPoint()
//float impulse = body->GetMass() * velChange; //disregard time factor
}
void b2d_shape_test_point(int id, double sx, double sy, double sa, double px, double py)
{
get_shape(b2dshape, id);
b2dshape->shape->TestPoint(b2Transform(b2Vec2(sx, sy), b2Rot(sa)), b2Vec2(px, py));
}
void pathfinding_system::advance_pathfinding_sessions(logic_step& step) {
auto& cosmos = step.cosm;
const auto& settings = cosmos.significant.meta.settings.pathfinding;
/* prepare epsilons to be used later, just to make the notation more clear */
const float epsilon_distance_visible_point_sq = settings.epsilon_distance_visible_point * settings.epsilon_distance_visible_point;
/* we'll need a reference to physics system for raycasting */
physics_system& physics = cosmos.systems_temporary.get<physics_system>();
auto& renderer = augs::renderer::get_current();
auto& lines = augs::renderer::get_current().logic_lines;
for (const auto& it : cosmos.get(processing_subjects::WITH_PATHFINDING)) {
/* get necessary components */
auto& pathfinding = it.get<components::pathfinding>();
const auto& transform = it.logic_transform() + pathfinding.eye_offset;
auto& body = it.get<components::physics>();
if (!body.is_constructed()) {
continue;
}
/* check if we request pathfinding at the moment */
if (!pathfinding.session_stack.empty()) {
/* get visibility information */
auto& vision = step.transient.calculated_visibility[it];
std::vector<components::pathfinding::pathfinding_session::navigation_vertex> undiscovered_visible;
/* proceed only if the session is not degenerate */
if (!vision.edges.empty() && !vision.discontinuities.empty()) {
if (pathfinding.force_touch_sensors) {
for (auto& vertex_hit : vision.vertex_hits) {
messages::visibility_information_response::discontinuity new_discontinuity;
new_discontinuity.edge_index = vertex_hit.first;
new_discontinuity.points.first = vertex_hit.second;
new_discontinuity.is_boundary = false;
/* rest is not worth filling so proceed */
if (vertex_hit.second.y >= transform.pos.y) {
vision.discontinuities.push_back(new_discontinuity);
}
}
vision.vertex_hits.clear();
}
//vision.ignore_discontinuities_shorter_than = pathfinding.session().temporary_ignore_discontinuities_shorter_than;
/* save all fully visible vertices as discovered */
for (const auto& visible_vertex : vision.vertex_hits) {
bool this_visible_vertex_is_already_memorised = false;
//auto components::pathfinding::pathfinding_session::* location =
// pathfinding.force_touch_sensors ?
// &components::pathfinding::pathfinding_session::undiscovered_vertices :
//&components::pathfinding::pathfinding_session::discovered_vertices;
for (auto& memorised : pathfinding.session().discovered_vertices) {
/* if a similiar discovered vertex exists */
if (memorised.location.compare(visible_vertex.second, settings.epsilon_distance_the_same_vertex)) {
this_visible_vertex_is_already_memorised = true;
/* overwrite the location just in case */
memorised.location = visible_vertex.second;
break;
}
}
/* save if unique */
if (!this_visible_vertex_is_already_memorised) {
components::pathfinding::pathfinding_session::navigation_vertex vert;
vert.location = visible_vertex.second;
pathfinding.session().discovered_vertices.push_back(vert);
}
}
/* save all new discontinuities from visibility */
for (const auto& disc : vision.discontinuities) {
if (disc.is_boundary) continue;
bool this_discontinuity_is_already_memorised = false;
bool this_discontinuity_is_already_discovered = false;
components::pathfinding::pathfinding_session::navigation_vertex vert;
for (auto& memorised_undiscovered : pathfinding.session().undiscovered_vertices) {
/* if a discontinuity with the same closer vertex already exists */
if (memorised_undiscovered.location.compare(disc.points.first, settings.epsilon_distance_the_same_vertex)) {
this_discontinuity_is_already_memorised = true;
vert = memorised_undiscovered;
//memorised_undiscovered.location = disc.points.first;
break;
}
}
for (auto& memorised_discovered : pathfinding.session().discovered_vertices) {
/* if a discontinuity with the same closer vertex already exists */
if (memorised_discovered.location.compare(disc.points.first, settings.epsilon_distance_the_same_vertex)) {
this_discontinuity_is_already_discovered = true;
memorised_discovered.location = disc.points.first;
break;
}
}
vert.location = disc.points.first;
/* if it is unique, push it */
if (!this_discontinuity_is_already_memorised && !this_discontinuity_is_already_discovered) {
/* get the associated edge to prepare a relevant sensor */
auto associated_edge = vision.edges[disc.edge_index];
/* get the direction the sensor will be going to */
vec2 sensor_direction;
bool degenerate = false;
/* if the first vertex of the edge matches the location */
if (associated_edge.first.compare(vert.location))
sensor_direction = associated_edge.first - associated_edge.second;
/* if it is the second one */
else if (associated_edge.second.compare(vert.location))
sensor_direction = associated_edge.second - associated_edge.first;
/* should never happen, degenerate edge */
else {
degenerate = true;
}
if (!degenerate) {
/* rotate a bit to prevent non-reachable sensors */
float rotation = pathfinding.rotate_navpoints;
if (disc.winding == disc.LEFT) rotation = -rotation;
sensor_direction.rotate(rotation, vec2(0, 0));
//sensor_direction = transform.pos - vert.location;
sensor_direction.normalize();
vert.sensor = vert.location + sensor_direction * pathfinding.target_offset;
/* if this sensor overlaps anything, discard it */
std::vector<vec2> sensor_polygon = {
sensor_direction * 10 + vert.location - sensor_direction.perpendicular_cw() * 4,
sensor_direction * 10 + vert.location - sensor_direction.perpendicular_cw() * 4 + sensor_direction * pathfinding.target_offset,
sensor_direction * 10 + vert.location + sensor_direction.perpendicular_cw() * 4 + sensor_direction * pathfinding.target_offset,
sensor_direction * 10 + vert.location + sensor_direction.perpendicular_cw() * 4
};
auto out = physics.query_polygon(sensor_polygon, pathfinding.filter, it);
if (out.bodies.empty()) {
vert.sensor = physics.push_away_from_walls(vert.sensor, pathfinding.target_offset, 50, pathfinding.filter, it);
pathfinding.session().undiscovered_vertices.push_back(vert);
}
}
}
if (!this_discontinuity_is_already_discovered)
undiscovered_visible.push_back(vert);
}
}
/* mark as visible vertices such that:
a) there is a memorised discovered vertex that is epsilon-close to it
b) sensor's distance from the the body is less than distance_navpoint_hit
*/
/* prepare body polygon to test for overlaps */
b2PolygonShape body_poly;
auto verts = get_world_vertices(it);
body_poly.Set(verts.data(), verts.size());
/* for every undiscovered navigation point */
auto& undiscs = pathfinding.session().undiscovered_vertices;
undiscs.erase(std::remove_if(undiscs.begin(), undiscs.end(), [&body, &pathfinding, &body_poly, &settings](const components::pathfinding::pathfinding_session::navigation_vertex& nav){
/* if we want to force the entity to touch the sensors, we can't discard undiscovered vertices only by
saying that there exists a discovered vertex (which is discovered only because it is fully visible)
*/
//if (!pathfinding.force_touch_sensors) {
/* find epsilon-close discovered vertices */
for (auto& memorised_discovered : pathfinding.session().discovered_vertices)
/* if a similiar discovered vertex exists */
if (memorised_discovered.location.compare(nav.location, settings.epsilon_distance_the_same_vertex))
return true;
//}
if (pathfinding.mark_touched_as_discovered) {
/* prepare edge shape for sensor to test for overlaps */
b2EdgeShape sensor_edge;
sensor_edge.Set(nav.location * PIXELS_TO_METERSf, nav.sensor * PIXELS_TO_METERSf);
/* prepare null transform, both bodies are already in the same frame of reference */
b2Transform null_transform(b2Vec2(0.f, 0.f), b2Rot(0.f));
/* if shortest distance between body and sensor fits in distance_navpoint_hit */
if (b2TestOverlap(&sensor_edge, 0, &body_poly, 0, null_transform, null_transform, pathfinding.distance_navpoint_hit * PIXELS_TO_METERSf)) {
/* save this sensor in discovered ones and return true to remove it from the undiscovered */
pathfinding.session().discovered_vertices.push_back(nav);
return true;
}
}
return false;
}), undiscs.end());
/* now for the actual pathfinding routine */
/* helpful lambda */
auto& is_point_visible = [&physics, epsilon_distance_visible_point_sq, &pathfinding, it](vec2 from, vec2 point, b2Filter& filter){
bool visibility_condition_fulfilled = true;
//if (pathfinding.target_visibility_condition)
// visibility_condition_fulfilled = pathfinding.target_visibility_condition(it, from, point);
if (visibility_condition_fulfilled) {
auto line_of_sight = physics.ray_cast_px(from, point, filter);
return (!line_of_sight.hit || (line_of_sight.intersection - point).length_sq() < epsilon_distance_visible_point_sq);
}
else return false;
};
/* we are sure here that session stack has at least 1 session
we drop secondary sessions whose targets are visible
*/
if (pathfinding.enable_session_rollbacks && pathfinding.session_stack.size() >= 2) {
for (auto old_session = pathfinding.session_stack.begin(); old_session != pathfinding.session_stack.end(); ++old_session) {
/* check if there's a line of sight to any of the old targets
if there's a line of sight to "navigate_to" it will be visible as target to the newer session
and we either way also handle the current session's target so nothing is missing here
*/
/* if we're exploring, we have no target in the first session */
if (pathfinding.is_exploring && old_session == pathfinding.session_stack.begin())
continue;
if (body.test_point((*old_session).target) ||
is_point_visible(transform.pos, (*old_session).target, pathfinding.filter)) {
/* if there is, roll back to this session */
pathfinding.session() = (*old_session);
/* if it is the first session, we don't want to erase it since we still need to reach the target */
if (old_session == pathfinding.session_stack.begin())
++old_session;
/* drop unnecessary sessions */
pathfinding.session_stack.erase(old_session, pathfinding.session_stack.end());
break;
}
}
}
/* if we're exploring, we have no target in the first session */
if (!pathfinding.is_exploring && pathfinding.session_stack.size() == 1) {
/* if the target is inside body, it's already found */
if (body.test_point(pathfinding.session().target)) {
/* done, target found */
pathfinding.stop_and_clear_pathfinding();
continue;
}
/* check if there's a line of sight */
if (is_point_visible(transform.pos, pathfinding.session().target, pathfinding.filter)) {
/* if there is, navigate directly to target */
pathfinding.session().discovered_vertices.clear();
pathfinding.session().undiscovered_vertices.clear();
pathfinding.session().navigate_to = pathfinding.session().target;
continue;
}
}
/* if it is the last session but there's no line of sight,
or it is not the last session but it was not dropped from the loop which means there's no line of sight to target,
pick the best navigation candidate
if we're exploring, pick only visible undiscovered vertices not to get stuck between two nodes
*/
auto& vertices = //(pathfinding.is_exploring && pathfinding.session_stack.size() == 1 && !undiscovered_visible.empty()) ?
//undiscovered_visible :
pathfinding.session().undiscovered_vertices;
/* save only for queries within the function "exists_among_undiscovered_visible" */
pathfinding.session().undiscovered_visible = undiscovered_visible;
if (settings.draw_undiscovered) {
for (auto& disc : vertices)
lines.draw(disc.location, disc.sensor, rgba(0, 127, 255, 255));
for (auto& disc : pathfinding.session().discovered_vertices)
//if(disc.sensor.non_zero())
lines.draw(disc.location, disc.location + vec2(0, pathfinding.target_offset), rgba(0, 255, 0, 255));
}
if (!vertices.empty()) {
bool persistent_navpoint_found = false;
components::pathfinding::pathfinding_session::navigation_vertex current_target;
if (pathfinding.force_persistent_navpoints) {
if (pathfinding.session().persistent_navpoint_set) {
for (auto& v : vertices) {
if (v.sensor.compare(pathfinding.session().persistent_navpoint.sensor, settings.epsilon_distance_the_same_vertex)) {
persistent_navpoint_found = true;
break;
}
}
}
}
if (persistent_navpoint_found) {
current_target = pathfinding.session().persistent_navpoint;
}
else {
vec2 unit_vel = body.velocity();
unit_vel.normalize();
auto local_minimum_predicate = [&pathfinding, &transform, unit_vel](const components::pathfinding::pathfinding_session::navigation_vertex& a,
const components::pathfinding::pathfinding_session::navigation_vertex& b) {
/* if we're exploring, we have no target in the first session */
if (pathfinding.is_exploring && pathfinding.session_stack.size() == 1) {
if (pathfinding.favor_velocity_parallellness) {
float parallellness_a = 0.f;
float parallellness_b = 0.f;
if (pathfinding.custom_exploration_hint.enabled) {
vec2 compared_dir = (pathfinding.custom_exploration_hint.target - pathfinding.custom_exploration_hint.origin).normalize();
parallellness_a = (a.location - pathfinding.custom_exploration_hint.origin).normalize().dot(compared_dir);
parallellness_b = (b.location - pathfinding.custom_exploration_hint.origin).normalize().dot(compared_dir);
}
else {
parallellness_a = (a.location - transform.pos).normalize().dot(unit_vel);
parallellness_b = (b.location - transform.pos).normalize().dot(unit_vel);
}
return parallellness_a > parallellness_b;
}
else if (pathfinding.custom_exploration_hint.enabled)
return (a.location - pathfinding.custom_exploration_hint.origin).length_sq() < (b.location - pathfinding.custom_exploration_hint.origin).length_sq();
else return (a.location - transform.pos).length_sq() < (b.location - transform.pos).length_sq();
}
auto dist_a = (a.location - pathfinding.session().target).length_sq() + (a.location - transform.pos).length_sq();
auto dist_b = (b.location - pathfinding.session().target).length_sq() + (b.location - transform.pos).length_sq();
return dist_a < dist_b;
};
bool first_priority_navpoint_found = false;
//if (pathfinding.first_priority_navpoint_check) {
// std::vector<components::pathfinding::pathfinding_session::navigation_vertex> first_priority_candidates;
//
// for (auto& v : vertices) {
// try {
// /* arguments: subject, transform, navpoint
// returns true or false
// */
// if (pathfinding.first_priority_navpoint_check(it, transform.pos, v.sensor)) {
// first_priority_candidates.push_back(v);
// }
// }
// catch (std::exception compilation_error) {
// LOG(compilation_error.what());
// }
// }
//
// if (!first_priority_candidates.empty()) {
// /* find discontinuity that is closest to the target */
// current_target = (*std::min_element(first_priority_candidates.begin(), first_priority_candidates.end(), local_minimum_predicate));
//
// first_priority_navpoint_found = true;
// }
//}
if (!first_priority_navpoint_found)
/* find discontinuity that is closest to the target */
current_target = (*std::min_element(vertices.begin(), vertices.end(), local_minimum_predicate));
if (pathfinding.force_persistent_navpoints) {
pathfinding.session().persistent_navpoint_set = true;
pathfinding.session().persistent_navpoint = current_target;
}
}
/* extract the closer vertex, condition to faciliate debug */
if (current_target.sensor != pathfinding.session().navigate_to)
pathfinding.session().navigate_to = current_target.sensor;
if (settings.draw_undiscovered) {
lines.draw(transform.pos, current_target.sensor, rgba(255, 255, 0, 255));
lines.draw(transform.pos, pathfinding.session().target, rgba(255, 0, 0, 255));
}
bool rays_hit = false;
/* extract all transformed vertices of the subject's original model, false means we want pixels */
auto& subject_verts = get_world_vertices(it, false);
subject_verts.clear();
subject_verts.push_back(transform.pos);
for (auto& subject_vert : subject_verts) {
if (
//is_point_visible(subject_vert, current_target.location, pathfinding.filter) ||
is_point_visible(subject_vert, current_target.sensor, pathfinding.filter)
) {
/* assume for now that the rays DID hit the navpoint */
rays_hit = true;
/* now see if the navpoint can be seen through marked non-walkable areas
prepare raycast data
*/
b2RayCastOutput output;
b2RayCastInput input;
input.maxFraction = 1.0;
for (auto& marked : vision.marked_holes) {
/* prepare raycast subject */
b2EdgeShape marked_hole;
marked_hole.Set(marked.first, marked.second);
input.p1 = subject_vert;
input.p2 = current_target.location;
/* we don't need to transform edge or ray since they are in the same space
but we have to prepare dummy b2Transform as argument for b2EdgeShape::RayCast
*/
b2Transform null_transform(b2Vec2(0.f, 0.f), b2Rot(0.f));
if (marked_hole.RayCast(&output, input, null_transform, 0)) {
rays_hit = false;
break;
}
input.p2 = current_target.sensor;
if (marked_hole.RayCast(&output, input, null_transform, 0)) {
rays_hit = false;
break;
}
}
}
}
/* if we can see it, navigate there */
if (body.test_point(current_target.location) ||
body.test_point(current_target.sensor) ||
rays_hit
) {
}
/* else start new navigation session */
else {
if (pathfinding.enable_backtracking) {
vec2 new_target = pathfinding.session().navigate_to;
pathfinding.session_stack.push_back(components::pathfinding::pathfinding_session());
pathfinding.session().target = new_target;
pathfinding.session().temporary_ignore_discontinuities_shorter_than = pathfinding.starting_ignore_discontinuities_shorter_than;
}
}
}
else {
/* something went wrong, let's begin again */
//if (pathfinding.session_stack.size() == 1) {
pathfinding.session().discovered_vertices.clear();
pathfinding.session().undiscovered_vertices.clear();
pathfinding.session().persistent_navpoint_set = false;
pathfinding.session().undiscovered_visible.clear();
//pathfinding.session().temporary_ignore_discontinuities_shorter_than /= 1.5f;
//}
//pathfinding.session_stack.resize(1);
}
}
}
}
