private static void PrintTrackingState(
Antilatency.Alt.Tracking.ITrackingCotask cotask,
Antilatency.Math.floatP3Q placement)
{
while (!cotask.isTaskFinished())
{
var state = cotask.getExtrapolatedState(placement, 0.06f);
if (state.stability.stage ==
Antilatency.Alt.Tracking.Stage.InertialDataInitialization)
{
continue;
}
Console.WriteLine(
"{0,-12} : {1,-11:G5} {2,-11:G5} {3,-11:G5} : " +
"{4,-11:G4} {5,-11:G5} {6,-11:G5} {7:G5}",
state.stability.stage,
state.pose.position.x,
state.pose.position.y,
state.pose.position.z,
state.pose.rotation.x,
state.pose.rotation.y,
state.pose.rotation.z,
state.pose.rotation.w);
// Do not print too often; 5 FPS is enough.
Thread.Sleep(200);
}
}
private static void PrintPlacementInfo(Antilatency.Math.floatP3Q placement)
{
Console.WriteLine("Placement:");
Console.WriteLine(" offset: {0:G5} {1:G5} {2:G5}",
placement.position.x,
placement.position.y,
placement.position.z);
Console.WriteLine(" rotation: {0:G5} {1:G5} {2:G5} {3:G5}",
placement.rotation.x,
placement.rotation.y,
placement.rotation.z,
placement.rotation.w);
Console.WriteLine();
}
private static Antilatency.Math.floatP3Q CreatePlacement(
Antilatency.StorageClient.ILibrary storageClientLibrary,
Antilatency.Alt.Tracking.ILibrary trackingLibrary)
{
using var storage = storageClientLibrary.getLocalStorage();
string placementCode = storage.read("placement", "default");
if (string.IsNullOrEmpty(placementCode))
{
var identityPlacement = new Antilatency.Math.floatP3Q();
identityPlacement.rotation.w = 1;
Console.WriteLine("Failed to get placement code, using identity placement");
return(identityPlacement);
}
return(trackingLibrary.createPlacement(placementCode));
}
