mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AP_Prox: Add plane intersection code to closest_point_from_segment_to_obstacle
This commit is contained in:
parent
206a414553
commit
cb911a01e2
@ -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)
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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)
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user