APM:Libraries
|
#include <vector2.h>
Public Member Functions | |
constexpr | Vector2 () |
constexpr | Vector2 (const T x0, const T y0) |
void | operator() (const T x0, const T y0) |
bool | operator== (const Vector2< T > &v) const |
bool | operator!= (const Vector2< T > &v) const |
Vector2< T > | operator- (void) const |
Vector2< T > | operator+ (const Vector2< T > &v) const |
Vector2< T > | operator- (const Vector2< T > &v) const |
Vector2< T > | operator* (const T num) const |
Vector2< T > | operator/ (const T num) const |
Vector2< T > & | operator+= (const Vector2< T > &v) |
Vector2< T > & | operator-= (const Vector2< T > &v) |
Vector2< T > & | operator*= (const T num) |
Vector2< T > & | operator/= (const T num) |
T | operator* (const Vector2< T > &v) const |
T | operator% (const Vector2< T > &v) const |
float | angle (const Vector2< T > &v2) const |
bool | is_nan (void) const |
bool | is_inf (void) const |
bool | is_zero (void) const |
T & | operator[] (uint8_t i) |
const T & | operator[] (uint8_t i) const |
void | zero () |
T | length_squared () const |
float | length (void) const |
void | normalize () |
Vector2< T > | normalized () const |
void | reflect (const Vector2< T > &n) |
void | project (const Vector2< T > &v) |
Vector2< T > | projected (const Vector2< T > &v) |
Static Public Member Functions | |
static Vector2< T > | perpendicular (const Vector2< T > &pos_delta, const Vector2< T > &v1) |
static Vector2< T > | closest_point (const Vector2< T > &p, const Vector2< T > &v, const Vector2< T > &w) |
static float | closest_distance_between_radial_and_point (const Vector2< T > &w, const Vector2< T > &p) |
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 | circle_segment_intersection (const Vector2< T > &seg_start, const Vector2< T > &seg_end, const Vector2< T > &circle_center, float radius, Vector2< T > &intersection) |
Public Attributes | |
T | x |
T | y |
Definition at line 127 of file vector2.cpp.
Referenced by Vector2< int32_t >::circle_segment_intersection(), Vector2< int32_t >::operator()(), and AP_Landing_Deepstall::verify_breakout().
|
static |
Definition at line 180 of file vector2.cpp.
Referenced by AC_Avoid::adjust_velocity_circle_fence(), Vector2< int32_t >::circle_segment_intersection(), and Vector2< int32_t >::closest_distance_between_radial_and_point().
|
inlinestatic |
Definition at line 216 of file vector2.h.
Referenced by closest_approach_xy().
|
inlinestatic |
Definition at line 191 of file vector2.h.
Referenced by AC_Avoid::adjust_velocity_polygon(), and Vector2< int32_t >::closest_distance_between_radial_and_point().
bool Vector2< T >::is_inf | ( | void | ) | const |
Definition at line 72 of file vector2.cpp.
Referenced by AP_AHRS::calc_trig(), Vector2< int32_t >::circle_segment_intersection(), Vector2< int32_t >::operator()(), and AP_AHRS_DCM::yaw_error_compass().
bool Vector2< T >::is_nan | ( | void | ) | const |
Definition at line 66 of file vector2.cpp.
Referenced by AP_AHRS::calc_trig(), Vector2< int32_t >::circle_segment_intersection(), and Vector2< int32_t >::operator()().
|
inline |
Definition at line 105 of file vector2.h.
Referenced by AC_Avoid::adjust_velocity_beacon_fence(), AC_Avoid::adjust_velocity_polygon_fence(), AC_Avoid::adjust_velocity_proximity(), AC_Loiter::calc_desired_velocity(), Compass::consistent(), and AP_Beacon::get_next_boundary_point().
float Vector2< T >::length | ( | void | ) | const |
Definition at line 24 of file vector2.cpp.
Referenced by AC_Avoid::adjust_roll_pitch(), AC_Avoid::adjust_speed(), AC_Avoid::adjust_velocity_circle_fence(), AC_Avoid::adjust_velocity_polygon(), AC_AttitudeControl::ang_vel_limit(), Vector2< int32_t >::angle(), AC_Loiter::calc_desired_velocity(), NavEKF2_core::calcGpsGoodToAlign(), NavEKF3_core::calcGpsGoodToAlign(), AC_Fence::check_fence_circle(), Vector2< int32_t >::circle_segment_intersection(), Vector2< int32_t >::closest_distance_between_radial_and_point(), AC_PID_2D::get_i(), AC_PI_2D::get_i(), AC_PID_2D::get_i_shrink(), AC_PI_2D::get_i_shrink(), AP_Arming::gps_checks(), AP_AHRS::groundspeed(), Vector2< int32_t >::length_squared(), Vector2< int32_t >::normalize(), Vector2< int32_t >::normalized(), AP_Landing_Deepstall::predict_travel_distance(), AP_L1_Control::update_heading_hold(), AP_L1_Control::update_loiter(), AP_Avoidance::update_threat_level(), AP_L1_Control::update_waypoint(), NavEKF2_core::writeOptFlowMeas(), NavEKF3_core::writeOptFlowMeas(), and AP_AHRS_DCM::yaw_error_compass().
|
inline |
Definition at line 131 of file vector2.h.
Referenced by Vector2< int32_t >::closest_point().
|
inline |
Definition at line 140 of file vector2.h.
Referenced by AP_AHRS::calc_trig(), AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(), AP_Landing_Deepstall::update_steering(), AP_L1_Control::update_waypoint(), and AP_AHRS_DCM::yaw_error_compass().
Definition at line 146 of file vector2.h.
Referenced by Compass::consistent(), and AP_L1_Control::update_loiter().
Definition at line 121 of file vector2.cpp.
Referenced by Vector2< int32_t >::circle_segment_intersection(), and Vector2< int32_t >::operator()().
Definition at line 39 of file vector2.cpp.
Referenced by Vector2< int32_t >::circle_segment_intersection(), and Vector2< int32_t >::operator()().
|
inline |
Definition at line 91 of file vector2.cpp.
Referenced by Vector2< int32_t >::circle_segment_intersection(), and Vector2< int32_t >::operator()().
Definition at line 32 of file vector2.cpp.
Definition at line 45 of file vector2.cpp.
Referenced by Vector2< int32_t >::circle_segment_intersection(), and Vector2< int32_t >::operator()().
Definition at line 103 of file vector2.cpp.
Referenced by Vector2< int32_t >::circle_segment_intersection(), and Vector2< int32_t >::operator()().
Definition at line 78 of file vector2.cpp.
Referenced by Vector2< int32_t >::circle_segment_intersection(), and Vector2< int32_t >::operator()().
Definition at line 109 of file vector2.cpp.
Referenced by Vector2< int32_t >::circle_segment_intersection(), and Vector2< int32_t >::operator()().
Definition at line 97 of file vector2.cpp.
Definition at line 59 of file vector2.cpp.
Referenced by Vector2< int32_t >::circle_segment_intersection(), and Vector2< int32_t >::operator()().
Definition at line 85 of file vector2.cpp.
Referenced by Vector2< int32_t >::circle_segment_intersection(), and Vector2< int32_t >::operator()().
Definition at line 52 of file vector2.cpp.
Referenced by Vector2< int32_t >::circle_segment_intersection(), and Vector2< int32_t >::operator()().
Definition at line 115 of file vector2.cpp.
Referenced by Vector2< int32_t >::circle_segment_intersection(), and Vector2< int32_t >::operator()().
|
inline |
|
inline |
|
inlinestatic |
Definition at line 173 of file vector2.h.
Referenced by AP_Avoidance::perpendicular_xy().
Definition at line 160 of file vector2.h.
Referenced by Vector2< int32_t >::reflect().
|
static |
Definition at line 147 of file vector2.cpp.
Referenced by AC_Avoid::adjust_velocity_polygon(), Vector2< int32_t >::circle_segment_intersection(), and Vector2< int32_t >::closest_distance_between_radial_and_point().
|
inline |
Definition at line 125 of file vector2.h.
Referenced by AP_GPS::calc_blended_state(), AC_Loiter::clear_pilot_desired_acceleration(), AP_AHRS_NavEKF::get_variances(), NavEKF2::getLastPosNorthEastReset(), NavEKF3::getLastPosNorthEastReset(), AC_Loiter::init_target(), NavEKF2_core::InitialiseFilterBootstrap(), NavEKF3_core::InitialiseFilterBootstrap(), NavEKF2_core::InitialiseVariables(), NavEKF3_core::InitialiseVariables(), AC_PID_2D::reset_I(), AC_PI_2D::reset_I(), NavEKF2_core::SelectFlowFusion(), NavEKF3_core::SelectFlowFusion(), AP_OpticalFlow_Pixart::update(), and AP_OpticalFlow_Onboard::update().
T Vector2< T >::x |
Definition at line 37 of file vector2.h.
Referenced by OpticalFlow_backend::_applyYaw(), Linux::OpticalFlow_Onboard::_run_optflow(), GCS_MAVLINK::_set_mode_common(), AC_Avoid::adjust_roll_pitch(), AC_Avoid::adjust_speed(), AC_Avoid::adjust_velocity(), AC_Avoid::adjust_velocity_polygon(), AC_AttitudeControl::ang_vel_limit(), AP_GPS::calc_blended_state(), AC_Loiter::calc_desired_velocity(), AP_AHRS::calc_trig(), AC_PosControl::check_for_ekf_xy_reset(), Vector2< int32_t >::circle_segment_intersection(), NavEKF2_core::EstimateTerrainOffset(), NavEKF3_core::EstimateTerrainOffset(), NavEKF2_core::FuseAirspeed(), NavEKF3_core::FuseAirspeed(), NavEKF2_core::FuseOptFlow(), NavEKF3_core::FuseOptFlow(), NavEKF2_core::FuseSideslip(), NavEKF3_core::FuseSideslip(), NavEKF3_core::FuseVelPosNED(), AC_Circle::get_closest_point_on_circle(), AC_PID_2D::get_d(), AP_Beacon::get_next_boundary_point(), AC_Loiter::get_pilot_desired_acceleration(), AP_AHRS_NavEKF::get_relative_position_NE_home(), AP_AHRS_NavEKF::get_relative_position_NED_origin(), AC_PrecLand::get_target_position_cm(), Location_Class::get_vector_from_origin_NEU(), AC_WPNav::get_vector_NEU(), AP_Avoidance::get_vector_perpendicular(), Location_Class::get_vector_xy_from_origin_NE(), SoaringController::get_wind_corrected_drift(), NavEKF2_core::getLLH(), NavEKF3_core::getLLH(), NavEKF2_core::getPosNE(), NavEKF3_core::getPosNE(), NavEKF2_core::getWind(), NavEKF3_core::getWind(), AC_Fence::handle_msg(), AP_Proximity_Backend::init_boundary(), AC_Loiter::init_target(), NavEKF2_core::InitialiseVariables(), NavEKF3_core::InitialiseVariables(), AC_AttitudeControl::input_shaping_rate_predictor(), AC_PolyFence_loader::load_point_from_eeprom(), location_path_proportion(), DataFlash_Class::Log_Write_POS(), Vector2< int32_t >::operator!=(), Vector2< int32_t >::operator%(), Vector2< int32_t >::operator*(), Vector2< int32_t >::operator+(), Vector2< int32_t >::operator+=(), Vector2< int32_t >::operator-(), Vector2< int32_t >::operator-=(), Vector2< int32_t >::operator==(), Vector2< int32_t >::operator[](), Polygon_outside(), Linux::OpticalFlow_Onboard::push_gyro(), Linux::OpticalFlow_Onboard::push_gyro_bias(), AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(), SITL::CRRCSim::recv_fdm(), NavEKF2_core::ResetPosition(), NavEKF3_core::ResetPosition(), NavEKF2_core::ResetVelocity(), NavEKF3_core::ResetVelocity(), AP_AHRS_View::rotate_body_to_earth2D(), AP_AHRS::rotate_body_to_earth2D(), AP_AHRS_View::rotate_earth_to_body2D(), AP_AHRS::rotate_earth_to_body2D(), AC_PrecLand::run_estimator(), AC_PrecLand::run_output_prediction(), AC_PosControl::run_xy_controller(), AC_PolyFence_loader::save_point_to_eeprom(), NavEKF2_core::SelectVelPosFusion(), NavEKF3_core::SelectVelPosFusion(), AC_PID_2D::set_input(), AC_PI_2D::set_input(), AC_PID_2D::set_input_filter_d(), AC_PID_2D::set_integrator(), AC_PI_2D::set_integrator(), AC_Loiter::set_pilot_desired_acceleration(), NavEKF2_core::setAidingMode(), NavEKF3_core::setAidingMode(), NavEKF2_core::setWindMagStateLearningMode(), NavEKF3_core::setWindMagStateLearningMode(), AP_OpticalFlow_PX4Flow::timer(), AP_OpticalFlow_Pixart::update(), AP_OpticalFlow_SITL::update(), AP_Beacon_SITL::update(), AP_OpticalFlow_Onboard::update(), AP_Beacon::update_boundary_points(), AP_L1_Control::update_loiter(), AP_L1_Control::update_waypoint(), AP_Landing_Deepstall::verify_land(), NavEKF2_core::writeOptFlowMeas(), NavEKF3_core::writeOptFlowMeas(), and AP_AHRS_DCM::yaw_error_compass().
T Vector2< T >::y |
Definition at line 37 of file vector2.h.
Referenced by OpticalFlow_backend::_applyYaw(), Linux::OpticalFlow_Onboard::_run_optflow(), GCS_MAVLINK::_set_mode_common(), AC_Avoid::adjust_roll_pitch(), AC_Avoid::adjust_speed(), AC_Avoid::adjust_velocity(), AC_Avoid::adjust_velocity_polygon(), AC_AttitudeControl::ang_vel_limit(), AP_GPS::calc_blended_state(), AC_Loiter::calc_desired_velocity(), AP_AHRS::calc_trig(), AC_PosControl::check_for_ekf_xy_reset(), Vector2< int32_t >::circle_segment_intersection(), NavEKF2_core::EstimateTerrainOffset(), NavEKF3_core::EstimateTerrainOffset(), NavEKF2_core::FuseAirspeed(), NavEKF3_core::FuseAirspeed(), NavEKF2_core::FuseOptFlow(), NavEKF3_core::FuseOptFlow(), NavEKF2_core::FuseSideslip(), NavEKF3_core::FuseSideslip(), NavEKF3_core::FuseVelPosNED(), AC_Circle::get_closest_point_on_circle(), AC_PID_2D::get_d(), AP_Beacon::get_next_boundary_point(), AC_Loiter::get_pilot_desired_acceleration(), AP_AHRS_NavEKF::get_relative_position_NE_home(), AP_AHRS_NavEKF::get_relative_position_NED_origin(), AC_PrecLand::get_target_position_cm(), Location_Class::get_vector_from_origin_NEU(), AC_WPNav::get_vector_NEU(), AP_Avoidance::get_vector_perpendicular(), Location_Class::get_vector_xy_from_origin_NE(), SoaringController::get_wind_corrected_drift(), NavEKF2_core::getLLH(), NavEKF3_core::getLLH(), NavEKF2_core::getPosNE(), NavEKF3_core::getPosNE(), NavEKF2_core::getWind(), NavEKF3_core::getWind(), AC_Fence::handle_msg(), AP_Proximity_Backend::init_boundary(), AC_Loiter::init_target(), NavEKF2_core::InitialiseVariables(), NavEKF3_core::InitialiseVariables(), AC_AttitudeControl::input_shaping_rate_predictor(), AC_PolyFence_loader::load_point_from_eeprom(), location_path_proportion(), DataFlash_Class::Log_Write_POS(), Vector2< int32_t >::operator!=(), Vector2< int32_t >::operator%(), Vector2< int32_t >::operator*(), Vector2< int32_t >::operator+(), Vector2< int32_t >::operator+=(), Vector2< int32_t >::operator-(), Vector2< int32_t >::operator-=(), Vector2< int32_t >::operator==(), Polygon_outside(), Linux::OpticalFlow_Onboard::push_gyro(), Linux::OpticalFlow_Onboard::push_gyro_bias(), AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(), SITL::CRRCSim::recv_fdm(), NavEKF2_core::ResetPosition(), NavEKF3_core::ResetPosition(), NavEKF2_core::ResetVelocity(), NavEKF3_core::ResetVelocity(), AP_AHRS_View::rotate_body_to_earth2D(), AP_AHRS::rotate_body_to_earth2D(), AP_AHRS_View::rotate_earth_to_body2D(), AP_AHRS::rotate_earth_to_body2D(), AC_PrecLand::run_estimator(), AC_PrecLand::run_output_prediction(), AC_PosControl::run_xy_controller(), AC_PolyFence_loader::save_point_to_eeprom(), NavEKF2_core::SelectVelPosFusion(), NavEKF3_core::SelectVelPosFusion(), AC_PID_2D::set_input(), AC_PI_2D::set_input(), AC_PID_2D::set_input_filter_d(), AC_PID_2D::set_integrator(), AC_PI_2D::set_integrator(), AC_Loiter::set_pilot_desired_acceleration(), NavEKF2_core::setAidingMode(), NavEKF3_core::setAidingMode(), NavEKF2_core::setWindMagStateLearningMode(), NavEKF3_core::setWindMagStateLearningMode(), AP_OpticalFlow_PX4Flow::timer(), AP_OpticalFlow_Pixart::update(), AP_OpticalFlow_SITL::update(), AP_Beacon_SITL::update(), AP_OpticalFlow_Onboard::update(), AP_Beacon::update_boundary_points(), AP_L1_Control::update_loiter(), AP_L1_Control::update_waypoint(), AP_Landing_Deepstall::verify_land(), NavEKF2_core::writeOptFlowMeas(), NavEKF3_core::writeOptFlowMeas(), and AP_AHRS_DCM::yaw_error_compass().