mirror of https://github.com/ArduPilot/ardupilot
AP_Math: added tofloat() and todouble() methods to Vector2 and Vector3
This commit is contained in:
parent
cf149a9d18
commit
3315ec5acc
|
@ -204,10 +204,10 @@ template <typename T>
|
|||
bool Vector2<T>::circle_segment_intersection(const Vector2<T>& seg_start, const Vector2<T>& seg_end, const Vector2<T>& circle_center, float radius, Vector2<T>& intersection)
|
||||
{
|
||||
// calculate segment start and end as offsets from circle's center
|
||||
const Vector2f seg_start_local = seg_start - circle_center;
|
||||
const Vector2<T> seg_start_local = seg_start - circle_center;
|
||||
|
||||
// calculate vector from start to end
|
||||
const Vector2f seg_end_minus_start = seg_end - seg_start;
|
||||
const Vector2<T> seg_end_minus_start = seg_end - seg_start;
|
||||
|
||||
const float a = sq(seg_end_minus_start.x) + sq(seg_end_minus_start.y);
|
||||
const float b = 2 * ((seg_end_minus_start.x * seg_start_local.x) + (seg_end_minus_start.y * seg_start_local.y));
|
||||
|
@ -448,8 +448,21 @@ void Vector2<T>::rotate(float angle_rad)
|
|||
y = ry;
|
||||
}
|
||||
|
||||
// only define for float
|
||||
template <typename T>
|
||||
Vector2<double> Vector2<T>::todouble(void) const
|
||||
{
|
||||
return Vector2d{x,y};
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
Vector2<float> Vector2<T>::tofloat(void) const
|
||||
{
|
||||
return Vector2f{float(x),float(y)};
|
||||
}
|
||||
|
||||
// define for float and double
|
||||
template class Vector2<float>;
|
||||
template class Vector2<double>;
|
||||
|
||||
// define some ops for int and long
|
||||
template bool Vector2<long>::operator ==(const Vector2<long> &v) const;
|
||||
|
|
|
@ -162,6 +162,12 @@ struct Vector2
|
|||
// rotate vector by angle in radians
|
||||
void rotate(float angle_rad);
|
||||
|
||||
/*
|
||||
conversion to/from double
|
||||
*/
|
||||
Vector2<float> tofloat() const;
|
||||
Vector2<double> todouble() const;
|
||||
|
||||
// given a position p1 and a velocity v1 produce a vector
|
||||
// perpendicular to v1 maximising distance from p1
|
||||
static Vector2<T> perpendicular(const Vector2<T> &pos_delta, const Vector2<T> &v1);
|
||||
|
@ -269,3 +275,4 @@ typedef Vector2<uint16_t> Vector2ui;
|
|||
typedef Vector2<int32_t> Vector2l;
|
||||
typedef Vector2<uint32_t> Vector2ul;
|
||||
typedef Vector2<float> Vector2f;
|
||||
typedef Vector2<double> Vector2d;
|
||||
|
|
|
@ -265,6 +265,12 @@ public:
|
|||
// extrapolate position given bearing and pitch (in degrees) and distance
|
||||
void offset_bearing(float bearing, float pitch, float distance);
|
||||
|
||||
/*
|
||||
conversion to/from double
|
||||
*/
|
||||
Vector3<float> tofloat() const;
|
||||
Vector3<double> todouble() const;
|
||||
|
||||
// given a position p1 and a velocity v1 produce a vector
|
||||
// perpendicular to v1 maximising distance from p1. If p1 is the
|
||||
// zero vector the return from the function will always be the
|
||||
|
|
Loading…
Reference in New Issue