AP_Prox: Add plane intersection code to closest_point_from_segment_to_obstacle

This commit is contained in:
Rishabh 2021-05-27 00:32:53 +05:30 committed by Randy Mackay
parent 206a414553
commit cb911a01e2
5 changed files with 25 additions and 14 deletions

View File

@ -398,12 +398,12 @@ bool AP_Proximity::get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle)
// returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
// used in GPS based Simple Avoidance
float AP_Proximity::distance_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const
bool AP_Proximity::closest_point_from_segment_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const
{
if (!valid_instance(primary_instance)) {
return FLT_MAX;
return false;
}
return drivers[primary_instance]->distance_to_obstacle(obstacle_num, seg_start, seg_end, closest_point);
return drivers[primary_instance]->closest_point_from_segment_to_obstacle(obstacle_num, seg_start, seg_end, closest_point);
}
// get distance and angle to closest object (used for pre-arm check)

View File

@ -114,7 +114,7 @@ public:
// returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
// returns FLT_MAX if it's an invalid instance.
float distance_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const;
bool closest_point_from_segment_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const;
// get distance and angle to closest object (used for pre-arm check)
// returns true on success, false if no valid readings

View File

@ -60,7 +60,7 @@ public:
// returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
// used in GPS based Simple Avoidance
float distance_to_obstacle(const uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const { return boundary.distance_to_obstacle(obstacle_num , seg_start, seg_end, closest_point); }
bool closest_point_from_segment_to_obstacle(const uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const { return boundary.closest_point_from_segment_to_obstacle(obstacle_num , seg_start, seg_end, closest_point); }
// get distance and angle to closest object (used for pre-arm check)
// returns true on success, false if no valid readings

View File

@ -270,21 +270,32 @@ bool AP_Proximity_Boundary_3D::get_obstacle(uint8_t obstacle_num, Vector3f& vec_
// Appropriate layer and sector are found from the passed obstacle_num
// This function then draws a line between this sector, and sector + 1 at the given layer
// Then returns the closest point on this line from the segment that was passed, in body-frame.
// Used by GPS based Simple Avoidance - for "brake mode"
// FLT_MAX is returned if the obstacle_num provided does not produce a valid obstacle
float AP_Proximity_Boundary_3D::distance_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const
{
Face face;
// Addionally a 3-D plane is constructed using the closest point found above as normal, and a point on the line segment in the boundary.
// True is returned when the passed line segment intersects this plane.
// This helps us know if the passed line segment was in the direction of the boundary, or going in a different direction.
// Used by GPS based Simple Avoidance - for "brake mode"
// False is returned if the obstacle_num provided does not produce a valid obstacle
bool AP_Proximity_Boundary_3D::closest_point_from_segment_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const
{
Face face;
if (!convert_obstacle_num_to_face(obstacle_num, face)) {
// not a valid a face
return FLT_MAX;
return false;
}
const uint8_t sector_end = face.sector;
const uint8_t sector_start = get_next_sector(face.sector);
const Vector3f start = _boundary_points[face.layer][sector_start];
const Vector3f end = _boundary_points[face.layer][sector_end];
return Vector3f::segment_to_segment_dist(seg_start, seg_end, start, end, closest_point);
// closest point between passed line segment and boundary
Vector3f::segment_to_segment_closest_point(seg_start, seg_end, start, end, closest_point);
if (closest_point == start) {
// draw a plane using the closest point as normal vector, and a point on the boundary
// return false if the passed segment does not intersect the plane
return Vector3f::segment_plane_intersect(seg_start, seg_end, closest_point, end);
}
return Vector3f::segment_plane_intersect(seg_start, seg_end, closest_point, start);
}
// get distance and angle to closest object (used for pre-arm check)

View File

@ -97,8 +97,8 @@ public:
bool get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_boundary) const;
// Returns a body frame vector (in cm) nearest to obstacle, in betwen seg_start and seg_end
// FLT_MAX is returned if the obstacle_num provided does not produce a valid obstacle
float distance_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const;
// True is returned if the segment intersects a plane formed by considering the "closest point" as normal vector to the plane.
bool closest_point_from_segment_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const;
// get distance and angle to closest object (used for pre-arm check)
// returns true on success, false if no valid readings