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_ */
|