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);
}
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();
}
}
