p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

fixes std::numeric_limits<float>::epsilon error

This commit is contained in:
Jacopo Bartiromo 2020-09-01 17:08:28 +02:00 committed by bernat
parent 318681e276
commit 14900e3bb7
3 changed files with 12 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
LibCarla/source/carla/trafficmanager/CollisionStage.cpp
View File

@ -143,7 +143,7 @@ LocationVector CollisionStage::GetBoundary(const ActorId actor_id) {
float bbox_y = dimensions.y;
const cg::Vector3D x_boundary_vector = heading_vector * (bbox_x + forward_extension);
const auto perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f).MakeUnitVector();
const auto perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f).MakeSafeUnitVector(EPSILON);
const cg::Vector3D y_boundary_vector = perpendicular_vector * (bbox_y + forward_extension);
// Four corners of the vehicle in top view clockwise order (left-handed system).
@ -199,7 +199,7 @@ LocationVector CollisionStage::GetGeodesicBoundary(const ActorId actor_id) {
const cg::Vector3D heading_vector = current_point->GetForwardVector();
const cg::Location location = current_point->GetLocation();
cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f);
perpendicular_vector = perpendicular_vector.MakeUnitVector();
perpendicular_vector = perpendicular_vector.MakeSafeUnitVector(EPSILON);
// Direction determined for the left-handed system.
const cg::Vector3D scaled_perpendicular = perpendicular_vector * width;
left_boundary.push_back(location + cg::Location(scaled_perpendicular));
@ -304,15 +304,14 @@ std::pair<bool, float> CollisionStage::NegotiateCollision(const ActorId referenc
// Ego and other vehicle heading.
const cg::Vector3D reference_heading = simulation_state.GetHeading(reference_vehicle_id);
// Vector from ego position to position of the other vehicle.
const float vector_magnitude_epsilon = 2.0f * std::numeric_limits<float>::epsilon();
cg::Vector3D reference_to_other = other_location - reference_location;
reference_to_other = reference_to_other.MakeSafeUnitVector(vector_magnitude_epsilon);
reference_to_other = reference_to_other.MakeSafeUnitVector(EPSILON);
// Other vehicle heading.
const cg::Vector3D other_heading = simulation_state.GetHeading(other_actor_id);
// Vector from other vehicle position to ego position.
cg::Vector3D other_to_reference = reference_location - other_location;
other_to_reference = other_to_reference.MakeSafeUnitVector(vector_magnitude_epsilon);
other_to_reference = other_to_reference.MakeSafeUnitVector(EPSILON);
float reference_vehicle_length = simulation_state.GetDimensions(reference_vehicle_id).x * SQUARE_ROOT_OF_TWO;
float other_vehicle_length = simulation_state.GetDimensions(other_actor_id).x * SQUARE_ROOT_OF_TWO;

2
LibCarla/source/carla/trafficmanager/Constants.h
View File

@ -78,6 +78,7 @@ static const float MAX_LOCKING_EXTENSION = 10.0f;
static const float WALKER_TIME_EXTENSION = 1.5f;
static const float SQUARE_ROOT_OF_TWO = 1.414f;
static const float VERTICAL_OVERLAP_THRESHOLD = 4.0f;
static const float EPSILON = 2.0f * std::numeric_limits<float>::epsilon();
} // namespace Collision
namespace FrameMemory {
@ -126,7 +127,6 @@ static const uint64_t BUFFER_STEP_THROUGH = 10;
static const float INV_BUFFER_STEP_THROUGH = 0.1f;
} // namespace TrackTraffic
} // namespace constants
} // namespace traffic_manager
} // namespace carla

12
LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp
View File

@ -5,16 +5,19 @@
// For a copy, see <https://opensource.org/licenses/MIT>.
#include "carla/trafficmanager/LocalizationUtils.h"
#include "carla/trafficmanager/Constants.h"
namespace carla {
namespace traffic_manager {
using constants::Collision::EPSILON;
float DeviationCrossProduct(const cg::Location &reference_location,
const cg::Vector3D &heading_vector,
const cg::Location &target_location) {
cg::Vector3D next_vector = target_location - reference_location;
float vector_magnitude_epsilon = 2.0f * std::numeric_limits<float>::epsilon();
next_vector = next_vector.MakeSafeUnitVector(vector_magnitude_epsilon);
next_vector = next_vector.MakeSafeUnitVector(EPSILON);
const float cross_z = heading_vector.x * next_vector.y - heading_vector.y * next_vector.x;
return cross_z;
}
@ -23,11 +26,10 @@ float DeviationDotProduct(const cg::Location &reference_location,
const cg::Vector3D &heading_vector,
const cg::Location &target_location) {
cg::Vector3D next_vector = target_location - reference_location;
float vector_magnitude_epsilon = 2.0f * std::numeric_limits<float>::epsilon();
next_vector.z = 0.0f;
next_vector = next_vector.MakeSafeUnitVector(vector_magnitude_epsilon);
next_vector = next_vector.MakeSafeUnitVector(EPSILON);
cg::Vector3D heading_vector_flat(heading_vector.x, heading_vector.y, 0);
heading_vector_flat = heading_vector_flat.MakeSafeUnitVector(vector_magnitude_epsilon);
heading_vector_flat = heading_vector_flat.MakeSafeUnitVector(EPSILON);
const float dot_product = cg::Math::Dot(next_vector, heading_vector_flat);
return dot_product;
}