30 #ifndef MCL_3DL_FILTER_H 31 #define MCL_3DL_FILTER_H 54 inline Filter(
const enum type_t type,
const float time_const,
const float out0,
const bool angle =
false)
61 k_[3] = -1 / (1.0 + 2 * time_const);
63 k_[1] = (1.0 - 2 * time_const) * k_[3];
65 x_ = (1 - k_[2]) * out0 / k_[3];
68 k_[3] = -1 / (1.0 + 2 * time_const);
69 k_[2] = -k_[3] * 2 * time_const;
70 k_[1] = (1.0 - 2 * time_const) * k_[3];
71 k_[0] = 2 * time_const * (-k_[1] + 1.0);
72 x_ = (1 - k_[2]) * out0 / k_[3];
76 inline void set(
const float out0)
78 x_ = (1 - k_[2]) * out0 / k_[3];
83 assert(std::isfinite(in));
87 in = out_ + remainder(in - out_, M_PI * 2.0);
89 x_ = k_[0] * in + k_[1] *
x_;
90 out_ = k_[2] * in + k_[3] *
x_;
92 assert(std::isfinite(out_));
95 inline float get()
const 102 #endif // MCL_3DL_FILTER_H
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
Filter(const enum type_t type, const float time_const, const float out0, const bool angle=false)