Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: allow selection of inline version of vector operations for performance.

Browse Source
This commit is contained in:
Andy Piper 2021-11-30 21:45:21 +00:00 committed by Andrew Tridgell
parent fbdeea2d03
commit fbaa2e7b09
2 changed files with 78 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Math/SCurve.cpp
View File

@ -545,7 +545,7 @@ void SCurve::move_to_pos_vel_accel(float dt, Vector3f &pos, Vector3f &vel, Vecto
void SCurve::move_from_time_pos_vel_accel(float time_now, Vector3f &pos, Vector3f &vel, Vector3f &accel)
{
float scurve_P1 = 0.0f;
float scurve_V1, scurve_A1, scurve_J1;
float scurve_V1 = 0.0f, scurve_A1 = 0.0f, scurve_J1 = 0.0f;
get_jerk_accel_vel_pos_at_time(time_now, scurve_J1, scurve_A1, scurve_V1, scurve_P1);
pos += delta_unit * scurve_P1;
vel += delta_unit * scurve_V1;

77
libraries/AP_Math/vector3.h
View File

@ -315,6 +315,83 @@ public:
static bool segment_plane_intersect(const Vector3<T>& seg_start, const Vector3<T>& seg_end, const Vector3<T>& plane_normal, const Vector3<T>& plane_point);
};
// The creation of temporary vector objects as return types creates a significant overhead in certain hot
// code paths. This allows callers to select the inline versions where profiling shows a significant benefit
#if defined(AP_INLINE_VECTOR_OPS) && !defined(HAL_DEBUG_BUILD)
// vector cross product
template <typename T>
inline Vector3<T> Vector3<T>::operator %(const Vector3<T> &v) const
{
return Vector3<T>(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x);
}
// dot product
template <typename T>
inline T Vector3<T>::operator *(const Vector3<T> &v) const
{
return x*v.x + y*v.y + z*v.z;
}
template <typename T>
inline Vector3<T> &Vector3<T>::operator *=(const T num)
{
x*=num; y*=num; z*=num;
return *this;
}
template <typename T>
inline Vector3<T> &Vector3<T>::operator /=(const T num)
{
x /= num; y /= num; z /= num;
return *this;
}
template <typename T>
inline Vector3<T> &Vector3<T>::operator -=(const Vector3<T> &v)
{
x -= v.x; y -= v.y; z -= v.z;
return *this;
}
template <typename T>
inline Vector3<T> &Vector3<T>::operator +=(const Vector3<T> &v)
{
x+=v.x; y+=v.y; z+=v.z;
return *this;
}
template <typename T>
inline Vector3<T> Vector3<T>::operator /(const T num) const
{
return Vector3<T>(x/num, y/num, z/num);
}
template <typename T>
inline Vector3<T> Vector3<T>::operator *(const T num) const
{
return Vector3<T>(x*num, y*num, z*num);
}
template <typename T>
inline Vector3<T> Vector3<T>::operator -(const Vector3<T> &v) const
{
return Vector3<T>(x-v.x, y-v.y, z-v.z);
}
template <typename T>
inline Vector3<T> Vector3<T>::operator +(const Vector3<T> &v) const
{
return Vector3<T>(x+v.x, y+v.y, z+v.z);
}
template <typename T>
inline Vector3<T> Vector3<T>::operator -(void) const
{
return Vector3<T>(-x,-y,-z);
}
#endif
typedef Vector3<int16_t> Vector3i;
typedef Vector3<uint16_t> Vector3ui;
typedef Vector3<int32_t> Vector3l;