private void Update() { RTQuaternion rotationDelta; RTQuaternion rotationPower; RTVector3 rotationUnitVector; if (EnableMag || EnableAccel) { // calculate rotation delta rotationDelta = mStateQ.Conjugate() * mMeasuredQPose; rotationDelta.Normalize(); // take it to the power (0 to 1) to give the desired amount of correction double theta = Math.Acos(rotationDelta.Scalar); double sinPowerTheta = Math.Sin(theta * SlerpPower); double cosPowerTheta = Math.Cos(theta * SlerpPower); rotationUnitVector = new RTVector3(rotationDelta.X, rotationDelta.Y, rotationDelta.Z); rotationUnitVector.Normalize(); rotationPower = new RTQuaternion(cosPowerTheta, sinPowerTheta * rotationUnitVector.X, sinPowerTheta * rotationUnitVector.Y, sinPowerTheta * rotationUnitVector.Z); rotationPower.Normalize(); // multiple this by predicted value to get result mStateQ *= rotationPower; mStateQ.Normalize(); } }
public void AccelToEuler(out RTVector3 rollPitchYaw) { RTVector3 normAccel = this; normAccel.Normalize(); rollPitchYaw = new RTVector3(Math.Atan2(normAccel.mY, normAccel.mZ), -Math.Atan2(normAccel.mX, Math.Sqrt(normAccel.mY * normAccel.mY + normAccel.mZ * normAccel.mZ)), 0); }
public void AccelToQuaternion(out RTQuaternion qPose) { RTVector3 normAccel = this; RTVector3 vec; RTVector3 z = new RTVector3(0, 0, 1.0); qPose = new RTQuaternion(); normAccel.Normalize(); double angle = Math.Acos(RTVector3.DotProduct(z, normAccel)); RTVector3.CrossProduct(normAccel, z, out vec); vec.Normalize(); qPose.FromAngleVector(angle, vec); }