void KalmanEuler_<T>::update(const ofQuaternion& q) { // warp to appropriate dimension ofVec3f euler = q.getEuler(); for( int i = 0; i < 3; i++ ) { float rev = floorf((eulerPrev[i] + 180) / 360.f) * 360; euler[i] += rev; if( euler[i] < -90 + rev && eulerPrev[i] > 90 + rev ) euler[i] += 360; else if( euler[i] > 90 + rev && eulerPrev[i] < -90 + rev ) euler[i] -= 360; } KalmanPosition_<T>::update(euler); eulerPrev = euler; }
string DebugUtil::toString(ofQuaternion quat) { ofVec3f vec = quat.getEuler(); return "alpha:" + ofToString(vec.x) + ", beta:" + ofToString(vec.y) + ", gamma:" + ofToString(vec.z); }