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;
	}
Example #2
0
string DebugUtil::toString(ofQuaternion quat)
{
    
    ofVec3f vec = quat.getEuler();
    return "alpha:" + ofToString(vec.x) + ", beta:" + ofToString(vec.y) + ", gamma:" + ofToString(vec.z);
}