|
|
@ -30,6 +30,7 @@ |
|
|
|
#pragma once |
|
|
|
#pragma once |
|
|
|
|
|
|
|
|
|
|
|
#include <cmath> |
|
|
|
#include <cmath> |
|
|
|
|
|
|
|
#include <AP_Common/AP_Common.h> |
|
|
|
|
|
|
|
|
|
|
|
template <typename T> |
|
|
|
template <typename T> |
|
|
|
struct Vector2 |
|
|
|
struct Vector2 |
|
|
@ -100,13 +101,15 @@ struct Vector2 |
|
|
|
float angle(void) const; |
|
|
|
float angle(void) const; |
|
|
|
|
|
|
|
|
|
|
|
// check if any elements are NAN
|
|
|
|
// check if any elements are NAN
|
|
|
|
bool is_nan(void) const; |
|
|
|
bool is_nan(void) const WARN_IF_UNUSED; |
|
|
|
|
|
|
|
|
|
|
|
// check if any elements are infinity
|
|
|
|
// check if any elements are infinity
|
|
|
|
bool is_inf(void) const; |
|
|
|
bool is_inf(void) const WARN_IF_UNUSED; |
|
|
|
|
|
|
|
|
|
|
|
// check if all elements are zero
|
|
|
|
// check if all elements are zero
|
|
|
|
bool is_zero(void) const { return (fabsf(x) < FLT_EPSILON) && (fabsf(y) < FLT_EPSILON); } |
|
|
|
bool is_zero(void) const WARN_IF_UNUSED { |
|
|
|
|
|
|
|
return (fabsf(x) < FLT_EPSILON) && (fabsf(y) < FLT_EPSILON); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
// allow a vector2 to be used as an array, 0 indexed
|
|
|
|
// allow a vector2 to be used as an array, 0 indexed
|
|
|
|
T & operator[](uint8_t i) { |
|
|
|
T & operator[](uint8_t i) { |
|
|
@ -207,15 +210,15 @@ struct Vector2 |
|
|
|
// find the intersection between two line segments
|
|
|
|
// find the intersection between two line segments
|
|
|
|
// returns true if they intersect, false if they do not
|
|
|
|
// returns true if they intersect, false if they do not
|
|
|
|
// the point of intersection is returned in the intersection argument
|
|
|
|
// the point of intersection is returned in the intersection argument
|
|
|
|
static bool segment_intersection(const Vector2<T>& seg1_start, const Vector2<T>& seg1_end, const Vector2<T>& seg2_start, const Vector2<T>& seg2_end, Vector2<T>& intersection); |
|
|
|
static bool segment_intersection(const Vector2<T>& seg1_start, const Vector2<T>& seg1_end, const Vector2<T>& seg2_start, const Vector2<T>& seg2_end, Vector2<T>& intersection) WARN_IF_UNUSED; |
|
|
|
|
|
|
|
|
|
|
|
// find the intersection between a line segment and a circle
|
|
|
|
// find the intersection between a line segment and a circle
|
|
|
|
// returns true if they intersect and intersection argument is updated with intersection closest to seg_start
|
|
|
|
// returns true if they intersect and intersection argument is updated with intersection closest to seg_start
|
|
|
|
static bool circle_segment_intersection(const Vector2<T>& seg_start, const Vector2<T>& seg_end, const Vector2<T>& circle_center, float radius, Vector2<T>& intersection); |
|
|
|
static bool circle_segment_intersection(const Vector2<T>& seg_start, const Vector2<T>& seg_end, const Vector2<T>& circle_center, float radius, Vector2<T>& intersection) WARN_IF_UNUSED; |
|
|
|
|
|
|
|
|
|
|
|
static bool point_on_segment(const Vector2<T>& point, |
|
|
|
static bool point_on_segment(const Vector2<T>& point, |
|
|
|
const Vector2<T>& seg_start, |
|
|
|
const Vector2<T>& seg_start, |
|
|
|
const Vector2<T>& seg_end) { |
|
|
|
const Vector2<T>& seg_end) WARN_IF_UNUSED { |
|
|
|
const float expected_run = seg_end.x-seg_start.x; |
|
|
|
const float expected_run = seg_end.x-seg_start.x; |
|
|
|
const float intersection_run = point.x-seg_start.x; |
|
|
|
const float intersection_run = point.x-seg_start.x; |
|
|
|
// check slopes are identical:
|
|
|
|
// check slopes are identical:
|
|
|
|