347 lines
11 KiB
C++
347 lines
11 KiB
C++
/*
|
||
* Vector.h
|
||
*
|
||
* Created on: 7 мая 2017 г.
|
||
* Author: titov
|
||
*/
|
||
|
||
#ifndef SOURCES_CONTROL_VECTOR_H_
|
||
#define SOURCES_CONTROL_VECTOR_H_
|
||
|
||
#include "../math/math_inc.hh"
|
||
|
||
namespace control {
|
||
|
||
typedef float Value;
|
||
|
||
struct IValueTransform {
|
||
virtual Value operator()(Value value) const = 0;
|
||
};
|
||
|
||
struct NaturalCoordinate {
|
||
float u;
|
||
float v;
|
||
float w;
|
||
|
||
NaturalCoordinate() noexcept : u(NAN), v(NAN), w(NAN) {}
|
||
NaturalCoordinate( float _u, float _v, float _w) noexcept : u(_u), v(_v), w(_w) {}
|
||
NaturalCoordinate( const NaturalCoordinate & rh ) noexcept : u(rh.u), v(rh.v), w(rh.w) {}
|
||
NaturalCoordinate & operator=( const NaturalCoordinate & rh ) { u = rh.u, v = rh.v, w = rh.w; return *this; }
|
||
NaturalCoordinate( const volatile NaturalCoordinate & rh ) noexcept : u(rh.u), v(rh.v), w(rh.w) {}
|
||
NaturalCoordinate & operator=( const volatile NaturalCoordinate & rh ) { u = rh.u, v = rh.v, w = rh.w; return *this; }
|
||
|
||
bool operator==( const NaturalCoordinate & rh ) const { return u == rh.u and v == rh.v and w == rh.w; }
|
||
bool operator!=( const NaturalCoordinate & rh ) const { return not rh.operator ==(*this); }
|
||
|
||
operator bool() const {
|
||
|
||
using namespace std;
|
||
return not( isnan(u) or isnan(v) or isnan(w) );
|
||
|
||
}
|
||
|
||
};
|
||
|
||
struct StandingVector {
|
||
float alfa;
|
||
float beta;
|
||
|
||
StandingVector() noexcept : alfa(NAN), beta(NAN) {}
|
||
StandingVector(float _alfa, float _beta) noexcept : alfa(_alfa), beta(_beta) {}
|
||
StandingVector( const volatile StandingVector & rh ) noexcept : alfa(rh.alfa), beta(rh.beta) {}
|
||
StandingVector( const StandingVector & rh ) noexcept : alfa(rh.alfa), beta(rh.beta) {}
|
||
StandingVector & operator=( const volatile StandingVector & rh ) noexcept { alfa = rh.alfa, beta = rh.beta; return *this; }
|
||
StandingVector & operator=( const StandingVector & rh ) noexcept { alfa = rh.alfa, beta = rh.beta; return *this; }
|
||
|
||
float sqrlen() const { return alfa * alfa + beta * beta; }
|
||
float len() const { return std::sqrt( sqrlen() ); }
|
||
StandingVector & operator*=(float mul) { alfa *= mul; beta *= mul; return *this; }
|
||
StandingVector operator*(float mul) {
|
||
return StandingVector( alfa * mul, beta * mul );
|
||
}
|
||
|
||
bool operator==( const StandingVector & rh ) const { return alfa == rh.alfa and beta == rh.beta; }
|
||
bool operator!=( const StandingVector & rh ) const { return not rh.operator ==(*this); }
|
||
|
||
|
||
};
|
||
|
||
struct IStandingVectorFactorTransform {
|
||
virtual StandingVector operator()( StandingVector value, Value factor ) const = 0;
|
||
};
|
||
|
||
struct RotatingVector {
|
||
float q;
|
||
float d;
|
||
|
||
RotatingVector() noexcept : q(NAN), d(NAN) {}
|
||
RotatingVector(float _q, float _d) : q(_q), d(_d) {}
|
||
RotatingVector( const volatile RotatingVector & rh ) : q(rh.q), d(rh.d) {}
|
||
RotatingVector & operator=( const volatile RotatingVector & rh ) { q = rh.q, d = rh.d; return *this;}
|
||
RotatingVector( const RotatingVector & rh ) : q(rh.q), d(rh.d) {}
|
||
RotatingVector & operator=( const RotatingVector & rh ) { q = rh.q, d = rh.d; return *this;}
|
||
|
||
float sqrlen() const { return q * q + d * d; }
|
||
float len() const { return std::sqrt( sqrlen() ); }
|
||
RotatingVector & operator*=(float mul) { q *= mul; d *= mul; return *this; }
|
||
RotatingVector operator*(float mul) { return RotatingVector( q * mul, d * mul ); }
|
||
|
||
RotatingVector operator-( const RotatingVector & rv ) {
|
||
return RotatingVector( q - rv.q, d - rv.d );
|
||
}
|
||
|
||
RotatingVector operator+( const RotatingVector & rv ) {
|
||
return RotatingVector( q + rv.q, d + rv.d );
|
||
}
|
||
|
||
RotatingVector & operator+=( const RotatingVector & rv ) {
|
||
q += rv.q, d += rv.d;
|
||
return *this;
|
||
}
|
||
|
||
bool operator==( const RotatingVector & rh ) const { return q == rh.q and d == rh.d; }
|
||
bool operator!=( const RotatingVector & rh ) const { return not rh.operator ==(*this); }
|
||
};
|
||
|
||
//!Функция нормализации значения заданного тремя векторами сдвинутиными на 120 гр.
|
||
/*!Функция нормализации значения заданного тремя векторами сдвинутиными на 120 гр.
|
||
* Вычисляет новые значения исходя из предположения что сумма всех значений равна нулю.
|
||
*
|
||
*/
|
||
NaturalCoordinate normalize( const NaturalCoordinate & input );
|
||
|
||
//!Функция вычисляет значение нулевой последовательности.
|
||
float calc_offset(const NaturalCoordinate & input);
|
||
|
||
//!Функция быстрого вычисления длинны вектора.
|
||
float flength(const StandingVector & vector);
|
||
//!Функция быстрого вычисления длинны вектора.
|
||
float flength(const RotatingVector & vector);
|
||
|
||
StandingVector tf_clark( const NaturalCoordinate & );
|
||
RotatingVector tf_park( const StandingVector &, float );
|
||
|
||
NaturalCoordinate itf_clark( const StandingVector & );
|
||
StandingVector itf_park( const RotatingVector &, float );
|
||
|
||
//!Функция ограничивает амплитуду вектора.
|
||
/*!Функция ограничивает амплитуду вектора по быстрому алгоритму.
|
||
* Направление вектора не изменяется.
|
||
* Возможное отклонение не превышает 5%.
|
||
* \param[in,out] vector Вектор подлежащий ограничению
|
||
* \param[in] limit Максимальная амплитуда вектора.
|
||
* \return Признак того что вектор был ограничен.
|
||
*/
|
||
bool flimitation( RotatingVector & vector, float limit );
|
||
//!\copydoc flimitation( RotatingVector & vector, float limit )
|
||
bool flimitation( StandingVector & vector, float limit );
|
||
|
||
//!Функция ограничивает амплитуду вектора.
|
||
/*!Функция ограничивает амплитуду вектора.
|
||
* Направление вектора не изменяется.
|
||
* \param[in,out] vector Вектор подлежащий ограничению
|
||
* \param[in] limit Максимальная амплитуда вектора.
|
||
* \return Признак того что вектор был ограничен.
|
||
*/
|
||
bool limitation( RotatingVector & vector, float limit );
|
||
//!\copydoc flimitation( RotatingVector & vector, float limit )
|
||
bool limitation( StandingVector & vector, float limit );
|
||
|
||
template<typename Type>
|
||
struct PhaseSpacePoint {
|
||
|
||
typedef Type Value;
|
||
|
||
Type value;
|
||
Type d_value;
|
||
|
||
PhaseSpacePoint() noexcept : value(), d_value() {}
|
||
PhaseSpacePoint( Type val, Type d_val ) noexcept : value(val), d_value(d_val) {}
|
||
PhaseSpacePoint( const PhaseSpacePoint & rh ) noexcept : value(rh.value), d_value(rh.d_value) {}
|
||
PhaseSpacePoint operator*( const Type & mul ) { return PhaseSpacePoint( value * mul, d_value * mul ); }
|
||
PhaseSpacePoint & operator=( const PhaseSpacePoint & rh ) { value = rh.value, d_value = rh.d_value; return *this;}
|
||
|
||
bool operator==( const PhaseSpacePoint & rh ) const { return value == rh.value and d_value == rh.d_value; }
|
||
bool operator!=( const PhaseSpacePoint & rh ) const { return not rh.operator ==(*this); }
|
||
};
|
||
|
||
typedef PhaseSpacePoint<float> PhaseSpaceValue;
|
||
|
||
bool limitation( PhaseSpaceValue & value, float limit );
|
||
|
||
float next( const PhaseSpaceValue & val, float time );
|
||
template<typename Type, typename Time = float>
|
||
Type next( const PhaseSpacePoint<Type> & val, Time time );
|
||
|
||
bool limitation( PhaseSpaceValue & value, float upper_limit, float lower_limit );
|
||
|
||
}
|
||
|
||
|
||
inline float control::flength(const StandingVector & vector) {
|
||
const float amax = 0.960433870f;
|
||
const float bmin = 0.397824735f;
|
||
|
||
const float qa = std::fabs(vector.alfa);
|
||
const float qb = std::fabs(vector.beta);
|
||
|
||
if(qa > qb)
|
||
return qa * amax + qb * bmin;
|
||
else
|
||
return qa * bmin + qb * amax;
|
||
}
|
||
|
||
inline float control::flength(const RotatingVector & vector) {
|
||
const float amax = 0.960433870f;
|
||
const float bmin = 0.397824735f;
|
||
|
||
const float qa = std::fabs(vector.q);
|
||
const float qb = std::fabs(vector.d);
|
||
|
||
if(qa > qb)
|
||
return qa * amax + qb * bmin;
|
||
else
|
||
return qa * bmin + qb * amax;
|
||
}
|
||
|
||
inline control::NaturalCoordinate control::normalize(const NaturalCoordinate & input) {
|
||
return NaturalCoordinate(
|
||
( input.u * 2 - input.v - input.w ) * 0.333333333f,
|
||
( input.v * 2 - input.w - input.u ) * 0.333333333f,
|
||
( input.w * 2 - input.u - input.v ) * 0.333333333f
|
||
);
|
||
}
|
||
|
||
inline float control::calc_offset(const NaturalCoordinate & input) {
|
||
return ( input.u + input.v + input.w ) * 0.333333333f;
|
||
}
|
||
|
||
inline control::StandingVector control::tf_clark( const NaturalCoordinate & value ) {
|
||
|
||
return StandingVector(
|
||
value.u,
|
||
( value.u + 2 * value.v ) * 0.577350269f
|
||
);
|
||
|
||
}
|
||
|
||
inline control::RotatingVector control::tf_park( const StandingVector & value, float iota ) {
|
||
|
||
float sin_val;
|
||
float cos_val;
|
||
|
||
math::function::sincos(iota, &sin_val, &cos_val);
|
||
|
||
return RotatingVector(
|
||
- value.alfa * sin_val + value.beta * cos_val,
|
||
value.alfa * cos_val + value.beta * sin_val
|
||
);
|
||
|
||
}
|
||
|
||
inline control::NaturalCoordinate control::itf_clark( const StandingVector & value ) {
|
||
|
||
return NaturalCoordinate(
|
||
value.alfa,
|
||
(math::constants::sqrt3 * value.beta - value.alfa) * 0.5f,
|
||
- value.alfa - (math::constants::sqrt3 * value.beta - value.alfa) * 0.5f
|
||
);
|
||
|
||
}
|
||
|
||
inline control::StandingVector control::itf_park( const RotatingVector & value, float iota ) {
|
||
|
||
float sin_val;
|
||
float cos_val;
|
||
|
||
math::function::sincos(iota, &sin_val, &cos_val);
|
||
|
||
return StandingVector(
|
||
-value.q * sin_val + value.d * cos_val,
|
||
value.q * cos_val + value.d * sin_val
|
||
);
|
||
|
||
}
|
||
|
||
inline float control::next( const PhaseSpaceValue & val, float time ) {
|
||
return val.value + val.d_value * time;
|
||
}
|
||
|
||
template<typename Type, typename Time>
|
||
inline Type control::next( const PhaseSpacePoint<Type> & val, Time time ) {
|
||
return val.value + val.d_value * time;
|
||
}
|
||
|
||
inline bool control::flimitation( RotatingVector & vector, float limit ) {
|
||
|
||
return limitation( vector, limit );
|
||
|
||
}
|
||
|
||
inline bool control::flimitation( StandingVector & vector, float limit ) {
|
||
|
||
return limitation( reinterpret_cast<RotatingVector &>(vector), limit );
|
||
|
||
}
|
||
|
||
inline bool control::limitation( RotatingVector & vector, float limit ) {
|
||
bool is_limited;
|
||
|
||
float a_sqrt = vector.sqrlen();
|
||
float inv_sqrt = math::function::q_rsqrt( a_sqrt );
|
||
|
||
if( is_limited = ( a_sqrt > limit*limit ) )
|
||
vector = vector * ( limit * inv_sqrt );
|
||
|
||
return is_limited;
|
||
}
|
||
|
||
|
||
inline bool control::limitation( StandingVector & vector, float limit ) {
|
||
|
||
return limitation( reinterpret_cast<RotatingVector &>(vector), limit );
|
||
|
||
}
|
||
|
||
inline bool control::limitation( PhaseSpaceValue & value, float limit ) {
|
||
|
||
bool is_limited = false;
|
||
|
||
using namespace std;
|
||
|
||
if( value.value > limit ) {
|
||
value = PhaseSpaceValue( limit, 0.0f );
|
||
is_limited = true;
|
||
}
|
||
|
||
if( value.value < - limit ) {
|
||
value = PhaseSpaceValue( - limit, 0.0f );
|
||
is_limited = true;
|
||
}
|
||
|
||
return is_limited;
|
||
|
||
}
|
||
|
||
inline bool control::limitation( PhaseSpaceValue & value, float upper_limit, float lower_limit ) {
|
||
|
||
bool is_limited = false;
|
||
|
||
using namespace std;
|
||
|
||
if( value.value > upper_limit ) {
|
||
value = PhaseSpaceValue( upper_limit, 0.0f );
|
||
is_limited = true;
|
||
}
|
||
|
||
if( value.value < lower_limit ) {
|
||
value = PhaseSpaceValue( lower_limit, 0.0f );
|
||
is_limited = true;
|
||
}
|
||
|
||
return is_limited;
|
||
|
||
}
|
||
|
||
#endif /* SOURCES_CONTROL_VECTOR_H_ */
|