/// <summary>
/// Converts a <see cref="CoordinateSystem"/> in \psi basis to a <see cref="SpatialCoordinateSystem"/> in HoloLens basis.
/// </summary>
/// <param name="coordinateSystem">The <see cref="CoordinateSystem"/> in \psi basis.</param>
/// <returns>The <see cref="SpatialCoordinateSystem"/>.</returns>
public static SpatialCoordinateSystem TryConvertPsiCoordinateSystemToSpatialCoordinateSystem(this CoordinateSystem coordinateSystem)
{
var holoLensMatrix = coordinateSystem.RebaseToHoloLensSystemMatrix();
var translation = holoLensMatrix.Translation;
holoLensMatrix.Translation = Vector3.Zero;
var rotation = Quaternion.CreateFromRotationMatrix(holoLensMatrix);
var spatialAnchor = SpatialAnchor.TryCreateRelativeTo(MixedReality.WorldSpatialCoordinateSystem, translation, rotation);
return(spatialAnchor?.CoordinateSystem);
}
public static Quaternion LookIn(Vector3 dir, Vector3 up)
{
dir = Vector3.Normalize(dir);
var right = Vector3.Normalize(Vector3.Cross(up, dir));
up = Vector3.Cross(dir, right);
var matrix = new Matrix4x4(
right.X, right.Y, right.Z, 0f,
up.X, up.Y, up.Z, 0f,
dir.X, dir.Y, dir.Z, 0f,
0f, 0f, 0f, 1f);
return(Quaternion.CreateFromRotationMatrix(matrix));
}