public static Matrix ToXnaMatrix(this SensorRotationMatrix cameraMatrix)
{
float[,] m = new float[3, 3];
m[0, 0] = cameraMatrix.M11;
m[1, 0] = cameraMatrix.M12;
m[2, 0] = cameraMatrix.M13;
m[0, 1] = cameraMatrix.M21;
m[1, 1] = cameraMatrix.M22;
m[2, 1] = cameraMatrix.M23;
m[0, 2] = cameraMatrix.M31;
m[1, 2] = cameraMatrix.M32;
m[2, 2] = cameraMatrix.M33;
int r1 = 0;
int r2 = 2;
int r3 = 1;
int c1 = 1;
int c2 = 0;
int c3 = 2;
return(new Matrix(
-m[c1, r1], m[c2, r1], m[c3, r1], 0,
-m[c1, r2], m[c2, r2], m[c3, r2], 0,
-m[c1, r3], m[c2, r3], m[c3, r3], 0,
0, 0, 0, 1
));
}
/// <summary>
/// This is the event handler for ReadingChanged events.
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
//<SnippetReadingChangedCS>
async private void ReadingChanged(object sender, OrientationSensorReadingChangedEventArgs e)
{
await Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
OrientationSensorReading reading = e.Reading;
// Quaternion values
SensorQuaternion quaternion = reading.Quaternion; // get a reference to the object to avoid re-creating it for each access
ScenarioOutput_X.Text = String.Format("{0,8:0.00000}", quaternion.X);
ScenarioOutput_Y.Text = String.Format("{0,8:0.00000}", quaternion.Y);
ScenarioOutput_Z.Text = String.Format("{0,8:0.00000}", quaternion.Z);
ScenarioOutput_W.Text = String.Format("{0,8:0.00000}", quaternion.W);
// Rotation Matrix values
SensorRotationMatrix rotationMatrix = reading.RotationMatrix;
ScenarioOutput_M11.Text = String.Format("{0,8:0.00000}", rotationMatrix.M11);
ScenarioOutput_M12.Text = String.Format("{0,8:0.00000}", rotationMatrix.M12);
ScenarioOutput_M13.Text = String.Format("{0,8:0.00000}", rotationMatrix.M13);
ScenarioOutput_M21.Text = String.Format("{0,8:0.00000}", rotationMatrix.M21);
ScenarioOutput_M22.Text = String.Format("{0,8:0.00000}", rotationMatrix.M22);
ScenarioOutput_M23.Text = String.Format("{0,8:0.00000}", rotationMatrix.M23);
ScenarioOutput_M31.Text = String.Format("{0,8:0.00000}", rotationMatrix.M31);
ScenarioOutput_M32.Text = String.Format("{0,8:0.00000}", rotationMatrix.M32);
ScenarioOutput_M33.Text = String.Format("{0,8:0.00000}", rotationMatrix.M33);
});
}
/// <summary>
/// Invoked when a user clicks on the GetDataButton
/// </summary>
/// <param name="sender"></param>
/// <param name="args"></param>
private void GetData(object sender, object args)
{
OrientationSensorReading reading = Sensor.GetCurrentReading();
if (reading != null)
{
// Quaternion values
SensorQuaternion quaternion = reading.Quaternion; // get a reference to the object to avoid re-creating it for each access
ScenarioOutput_X.Text = String.Format("{0,8:0.00000}", quaternion.X);
ScenarioOutput_Y.Text = String.Format("{0,8:0.00000}", quaternion.Y);
ScenarioOutput_Z.Text = String.Format("{0,8:0.00000}", quaternion.Z);
ScenarioOutput_W.Text = String.Format("{0,8:0.00000}", quaternion.W);
// Rotation Matrix values
SensorRotationMatrix rotationMatrix = reading.RotationMatrix;
ScenarioOutput_M11.Text = String.Format("{0,8:0.00000}", rotationMatrix.M11);
ScenarioOutput_M12.Text = String.Format("{0,8:0.00000}", rotationMatrix.M12);
ScenarioOutput_M13.Text = String.Format("{0,8:0.00000}", rotationMatrix.M13);
ScenarioOutput_M21.Text = String.Format("{0,8:0.00000}", rotationMatrix.M21);
ScenarioOutput_M22.Text = String.Format("{0,8:0.00000}", rotationMatrix.M22);
ScenarioOutput_M23.Text = String.Format("{0,8:0.00000}", rotationMatrix.M23);
ScenarioOutput_M31.Text = String.Format("{0,8:0.00000}", rotationMatrix.M31);
ScenarioOutput_M32.Text = String.Format("{0,8:0.00000}", rotationMatrix.M32);
ScenarioOutput_M33.Text = String.Format("{0,8:0.00000}", rotationMatrix.M33);
}
}
/// <summary>
/// This is the dispatcher callback.
/// </summary>
/// <param name="sender"></param>
/// <param name="args"></param>
private void DisplayCurrentReading(object sender, object args)
{
OrientationSensorReading reading = _sensor.GetCurrentReading();
if (reading != null)
{
// Quaternion values
SensorQuaternion quaternion = reading.Quaternion; // get a reference to the object to avoid re-creating it for each access
ScenarioOutput_X.Text = String.Format("{0,8:0.00000}", quaternion.X);
ScenarioOutput_Y.Text = String.Format("{0,8:0.00000}", quaternion.Y);
ScenarioOutput_Z.Text = String.Format("{0,8:0.00000}", quaternion.Z);
ScenarioOutput_W.Text = String.Format("{0,8:0.00000}", quaternion.W);
// Rotation Matrix values
SensorRotationMatrix rotationMatrix = reading.RotationMatrix;
ScenarioOutput_M11.Text = String.Format("{0,8:0.00000}", rotationMatrix.M11);
ScenarioOutput_M12.Text = String.Format("{0,8:0.00000}", rotationMatrix.M12);
ScenarioOutput_M13.Text = String.Format("{0,8:0.00000}", rotationMatrix.M13);
ScenarioOutput_M21.Text = String.Format("{0,8:0.00000}", rotationMatrix.M21);
ScenarioOutput_M22.Text = String.Format("{0,8:0.00000}", rotationMatrix.M22);
ScenarioOutput_M23.Text = String.Format("{0,8:0.00000}", rotationMatrix.M23);
ScenarioOutput_M31.Text = String.Format("{0,8:0.00000}", rotationMatrix.M31);
ScenarioOutput_M32.Text = String.Format("{0,8:0.00000}", rotationMatrix.M32);
ScenarioOutput_M33.Text = String.Format("{0,8:0.00000}", rotationMatrix.M33);
// Yaw accuracy
switch (reading.YawAccuracy)
{
case MagnetometerAccuracy.Unknown:
ScenarioOutput_YawAccuracy.Text = "Unknown";
break;
case MagnetometerAccuracy.Unreliable:
ScenarioOutput_YawAccuracy.Text = "Unreliable";
break;
case MagnetometerAccuracy.Approximate:
ScenarioOutput_YawAccuracy.Text = "Approximate";
break;
case MagnetometerAccuracy.High:
ScenarioOutput_YawAccuracy.Text = "High";
break;
default:
ScenarioOutput_YawAccuracy.Text = "No data";
break;
}
}
}
/// <summary>
/// This is the event handler for ReadingChanged events.
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
async private void ReadingChanged(object sender, OrientationSensorReadingChangedEventArgs e)
{
await Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
OrientationSensorReading reading = e.Reading;
// Quaternion values
SensorQuaternion quaternion = reading.Quaternion; // get a reference to the object to avoid re-creating it for each access
ScenarioOutput_X.Text = String.Format("{0,8:0.00000}", quaternion.X);
ScenarioOutput_Y.Text = String.Format("{0,8:0.00000}", quaternion.Y);
ScenarioOutput_Z.Text = String.Format("{0,8:0.00000}", quaternion.Z);
ScenarioOutput_W.Text = String.Format("{0,8:0.00000}", quaternion.W);
// Rotation Matrix values
SensorRotationMatrix rotationMatrix = reading.RotationMatrix;
ScenarioOutput_M11.Text = String.Format("{0,8:0.00000}", rotationMatrix.M11);
ScenarioOutput_M12.Text = String.Format("{0,8:0.00000}", rotationMatrix.M12);
ScenarioOutput_M13.Text = String.Format("{0,8:0.00000}", rotationMatrix.M13);
ScenarioOutput_M21.Text = String.Format("{0,8:0.00000}", rotationMatrix.M21);
ScenarioOutput_M22.Text = String.Format("{0,8:0.00000}", rotationMatrix.M22);
ScenarioOutput_M23.Text = String.Format("{0,8:0.00000}", rotationMatrix.M23);
ScenarioOutput_M31.Text = String.Format("{0,8:0.00000}", rotationMatrix.M31);
ScenarioOutput_M32.Text = String.Format("{0,8:0.00000}", rotationMatrix.M32);
ScenarioOutput_M33.Text = String.Format("{0,8:0.00000}", rotationMatrix.M33);
// Yaw accuracy
switch (reading.YawAccuracy)
{
case MagnetometerAccuracy.Unknown:
ScenarioOutput_YawAccuracy.Text = "Unknown";
break;
case MagnetometerAccuracy.Unreliable:
ScenarioOutput_YawAccuracy.Text = "Unreliable";
break;
case MagnetometerAccuracy.Approximate:
ScenarioOutput_YawAccuracy.Text = "Approximate";
break;
case MagnetometerAccuracy.High:
ScenarioOutput_YawAccuracy.Text = "High";
break;
default:
ScenarioOutput_YawAccuracy.Text = "No data";
break;
}
});
}
public void ResetHome()
{
var incl = OrientationSensor.GetDefault();
OrientationSensorReading inc = incl.GetCurrentReading();
if (inc != null)
{
zeroSet = true;
zeroMatrix = inc.RotationMatrix;
}
}
public static void SetReadingText(TextBlock textBlock, OrientationSensorReading reading)
{
SensorQuaternion quaternion = reading.Quaternion; // get a reference to the object to avoid re-creating it for each access
string quaternionReport = string.Format("W: {0,5:0.00}, X: {1,5:0.00}, Y: {2,5:0.00}, Z: {3,5:0.00}",
quaternion.W, quaternion.X, quaternion.Y, quaternion.Z);
SensorRotationMatrix rotationMatrix = reading.RotationMatrix;
string rotationMatrixReport = string.Format(
"M11: {0,5:0.00}, M12: {1,5:0.00}, M13: {2,5:0.00}\n" +
"M21: {3,5:0.00}, M22: {4,5:0.00}, M23: {5,5:0.00}\n" +
"M31: {6,5:0.00}, M32: {7,5:0.00}, M33: {8,5:0.00}",
rotationMatrix.M11, rotationMatrix.M12, rotationMatrix.M13,
rotationMatrix.M21, rotationMatrix.M22, rotationMatrix.M23,
rotationMatrix.M31, rotationMatrix.M32, rotationMatrix.M33);
string yawAccuracyReport;
switch (reading.YawAccuracy)
{
case MagnetometerAccuracy.Unknown:
yawAccuracyReport = "unknown";
break;
case MagnetometerAccuracy.Unreliable:
yawAccuracyReport = "unreliable";
break;
case MagnetometerAccuracy.Approximate:
yawAccuracyReport = "approximate";
break;
case MagnetometerAccuracy.High:
yawAccuracyReport = "high";
break;
default:
yawAccuracyReport = "other";
break;
}
textBlock.Text = "Quaternion:\n" + quaternionReport + "\n\n" +
"Rotation Matrix:\n" + rotationMatrixReport + "\n\n" +
"Yaw Accuracy:\n" + yawAccuracyReport;
}
protected override void Update(GameTime gameTime)
{
if (!zeroSet)
{
ResetHome();
}
else
{
var incl = OrientationSensor.GetDefault();
var inc = incl.GetCurrentReading();
if (inc != null)
{
cameraMatrix = inc.RotationMatrix;
}
}
// shapes.Last().Position = shapes.First().Position + new Vector3(0.01f, 0.1f, 0);
shapes.Last().Rotation = shapes.Last().Rotation + new Vector3(0, 0, 0.01f);
base.Update(gameTime);
}
void ReadOrientationData()
{
if (orientationSensor != null)
{
OrientationSensorReading reading = orientationSensor.GetCurrentReading();
if (reading != null)
{
SensorRotationMatrix srm = reading.RotationMatrix;
Matrix3D rotationMatrix = new Matrix3D(
srm.M11, srm.M12, srm.M13, 0,
srm.M21, srm.M22, srm.M23, 0,
srm.M31, srm.M32, srm.M33, 0,
0, 0, 0, 0);
Matrix3D bodySpaceMatrix = pointMatrix * rotationMatrix;
point.Text = string.Format("Transform of (0.0, 10.0, 0.0) = ({0:F1}, {1:F1}, {2:F1})",
bodySpaceMatrix.OffsetX, bodySpaceMatrix.OffsetY, bodySpaceMatrix.OffsetZ);
}
}
}
void ShowOrientation(OrientationSensorReading orientationReading)
{
if (orientationReading == null)
{
return;
}
SensorRotationMatrix matrix = orientationReading.RotationMatrix;
if (matrix == null)
{
return;
}
m11Text.Text = matrix.M11.ToString("F3");
m12Text.Text = matrix.M12.ToString("F3");
m13Text.Text = matrix.M13.ToString("F3");
m21Text.Text = matrix.M21.ToString("F3");
m22Text.Text = matrix.M22.ToString("F3");
m23Text.Text = matrix.M23.ToString("F3");
m31Text.Text = matrix.M31.ToString("F3");
m32Text.Text = matrix.M32.ToString("F3");
m33Text.Text = matrix.M33.ToString("F3");
// Convert rotation matrix to axis and angle
double angle = Math.Acos((matrix.M11 + matrix.M22 + matrix.M33 - 1) / 2);
angleText.Text = (180 * angle / Math.PI).ToString("F0");
if (angle != 0)
{
double twoSine = 2 * Math.Sin(angle);
double x = (matrix.M23 - matrix.M32) / twoSine;
double y = (matrix.M31 - matrix.M13) / twoSine;
double z = (matrix.M12 - matrix.M21) / twoSine;
axisText.Text = String.Format("({0:F2} {1:F2} {2:F2})", x, y, z);
}
}
void ProcessNewOrientationReading(OrientationSensorReading orientationReading)
{
if (orientationReading == null)
{
return;
}
// Get the rotation matrix & convert to horizontal coordinates
SensorRotationMatrix m = orientationReading.RotationMatrix;
if (m == null)
{
return;
}
Matrix3D matrix3d = new Matrix3D(m.M11, m.M12, m.M13, 0,
m.M21, m.M22, m.M23, 0,
m.M31, m.M32, m.M33, 0,
0, 0, 0, 1);
if (!matrix3d.HasInverse)
{
return;
}
HorizontalCoordinate horzCoord = HorizontalCoordinate.FromMotionMatrix(matrix3d);
// Set the transform center on the Image element
imageTransform.CenterX = (imageWidth + maxDimension) *
(180 + horzCoord.Azimuth) / 360 - maxDimension / 2;
imageTransform.CenterY = (imageHeight + maxDimension) *
(90 - horzCoord.Altitude) / 180 - maxDimension / 2;
// Set the translation on the Border element
borderTransform.TranslateX = imageTransform.CenterX - pageWidth / 2;
borderTransform.TranslateY = imageTransform.CenterY - pageHeight / 2;
// Get rotation from Tilt
rotation = -horzCoord.Tilt;
UpdateImageTransforms();
}
public static string Ouput(this SensorRotationMatrix m) =>
$"m11 {m.M11} m12 {m.M12} m13 {m.M13} m21 {m.M21} m22 {m.M22} m23 {m.M23} m31 {m.M31} m32 {m.M32} m33 {m.M33}";
public static Matrix3D ToMatrix3D(this SensorRotationMatrix m) => new Matrix3D(m.M11, m.M21, m.M31, 0, m.M12, m.M22, m.M32, 0, m.M13, m.M23, m.M33, 0, 0, 0, 0, 1);
