public static bool CheckIfDirectionShouldBeInverted(DirectionClosestTo settings, TrackerReference trackers, CalibrationProfile profile, Vector3 worldDirection)
{
switch (settings.type)
{
case DirectionClosestType.SingleDirection:
Vector3 dir = Direction.GetDirection(settings.singleDirection, trackers, profile);
if (Vector3.Distance(dir, worldDirection) > Vector3.Distance(dir, worldDirection * -1))
{
return(true);
}
break;
case DirectionClosestType.Cross:
Vector3 crossDir1 = Direction.GetDirection(settings.crossDirection1, trackers, profile);
Vector3 crossDir2 = Direction.GetDirection(settings.crossDirection2, trackers, profile);
Vector3 cross = Vector3.Cross(crossDir1, crossDir2);
if (Vector3.Distance(cross, worldDirection) > Vector3.Distance(cross, worldDirection * -1))
{
return(true);
}
break;
default:
Debug.LogError("Implement your shit");
break;
}
return(false);
}
public static Vector3 GetDirection(DirectionClosestTo settings, TrackerReference trackers, CalibrationProfile profile)
{
switch (settings.type)
{
case DirectionClosestType.SingleDirection:
return(Direction.GetDirection(settings.singleDirection, trackers, profile));
case DirectionClosestType.Cross:
Vector3 crossDir1 = Direction.GetDirection(settings.crossDirection1, trackers, profile);
Vector3 crossDir2 = Direction.GetDirection(settings.crossDirection2, trackers, profile);
return(Vector3.Cross(crossDir1, crossDir2));
default:
Debug.LogError("Implement your shit");
break;
}
return(Vector3.zero);
}
//public override void Setup(CalibrationProfile profile, TrackerReference trackers)
//{
// base.Setup(profile, trackers);
//}
protected override void End()
{
foreach (AxisData data in axisData)
{
switch (data.dataType)
{
case AlignType.GetAxis:
Vector3 axisDirection = DirectionClosestTo.GetDirection(data.axisDirectionToGet, trackers, profile);
TransformValues?getAxisTracker = trackers.GetTracker(data.getAxisTracker);
if (getAxisTracker == null)
{
Debug.LogError("Not all trackers are connected");
continue;
}
Matrix4x4 getAxisTrackerMatrix = Matrix4x4.TRS(getAxisTracker.Value.position, getAxisTracker.Value.rotation, Vector3.one);
getAxisTrackerMatrix = getAxisTrackerMatrix.inverse;
profile.AddTrackerDirection(data.getAxisTracker, data.axisToGet, getAxisTrackerMatrix.MultiplyVector(axisDirection));
break;
case AlignType.AverageTwoAxis:
TransformValues?averageTracker1 = trackers.GetTracker(data.averageTracker1);
TransformValues?averageTracker2 = trackers.GetTracker(data.averageTracker2);
if (averageTracker1 == null || averageTracker2 == null)
{
Debug.LogError("Not all trackers are connected");
continue;
}
if (!profile.trackerDirections.ContainsKey(data.averageTracker1) || !profile.trackerDirections.ContainsKey(data.averageTracker2) ||
profile.trackerDirections[data.averageTracker1].GetAxis(data.averageAxis1) == null || profile.trackerDirections[data.averageTracker2].GetAxis(data.averageAxis2) == null)
{
Debug.LogError("Not all trackers have required directions");
continue;
}
Vector3 averageTracker1Dir = (Vector3)profile.trackerDirections[data.averageTracker1].GetAxis(data.averageAxis1) * (data.averageAxisInvert1 ? -1 : 1);
Vector3 averageTracker2Dir = (Vector3)profile.trackerDirections[data.averageTracker2].GetAxis(data.averageAxis2) * (data.averageAxisInvert2 ? -1 : 1);
Matrix4x4 averageTracker1Matrix = Matrix4x4.TRS(averageTracker1.Value.position, averageTracker1.Value.rotation, Vector3.one);
Matrix4x4 averageTracker2Matrix = Matrix4x4.TRS(averageTracker2.Value.position, averageTracker2.Value.rotation, Vector3.one);
averageTracker1Dir = averageTracker1Matrix.MultiplyVector(averageTracker1Dir);
averageTracker2Dir = averageTracker2Matrix.MultiplyVector(averageTracker2Dir);
Vector3 averageTrackerDir = (averageTracker1Dir + averageTracker2Dir) / 2f;
profile.AddTrackerDirection(data.averageTracker1, data.averageAxis1, averageTracker1Matrix.inverse.MultiplyVector(averageTrackerDir * (data.averageAxisInvert1 ? -1 : 1)));
profile.AddTrackerDirection(data.averageTracker2, data.averageAxis2, averageTracker2Matrix.inverse.MultiplyVector(averageTrackerDir * (data.averageAxisInvert2 ? -1 : 1)));
break;
case AlignType.CalculateAxis:
TransformValues?calculateTrackerTransform = trackers.GetTracker(data.calculateTracker);
if (calculateTrackerTransform == null)
{
Debug.LogError("Not all trackers are connected");
continue;
}
Axis[] axisToUse = { };
switch (data.axisToCalculate)
{
case Axis.X:
axisToUse = new[] { Axis.Y, Axis.Z };
break;
case Axis.Y:
axisToUse = new[] { Axis.X, Axis.Z };
break;
case Axis.Z:
axisToUse = new[] { Axis.X, Axis.Y };
break;
}
if (!profile.trackerDirections.ContainsKey(data.calculateTracker) ||
profile.trackerDirections[data.calculateTracker].GetAxis(axisToUse[0]) == null || profile.trackerDirections[data.calculateTracker].GetAxis(axisToUse[1]) == null)
{
Debug.LogError("Tracker doesn't have required directions");
continue;
}
Matrix4x4 calucalteTrackerMatrix = Matrix4x4.TRS(calculateTrackerTransform.Value.position, calculateTrackerTransform.Value.rotation, Vector3.one);
Vector3 calculatedAxis = Vector3.Cross(
profile.trackerDirections[data.calculateTracker].GetAxis(axisToUse[0]).Value,
profile.trackerDirections[data.calculateTracker].GetAxis(axisToUse[1]).Value);
if (DirectionClosestTo.CheckIfDirectionShouldBeInverted(data.calculateDirectionClosestTo, trackers, profile, calucalteTrackerMatrix.MultiplyVector(calculatedAxis)))
{
calculatedAxis *= -1;
}
profile.AddTrackerDirection(data.calculateTracker, data.axisToCalculate, calculatedAxis);
break;
case AlignType.GetMeasurement:
TransformValues?measurementTrackerTransform1 = trackers.GetTracker(data.measurementTracker1);
TransformValues?measurementTrackerTransform2 = trackers.GetTracker(data.measurementTracker2);
if (data.measurementLocal1)
{
if (profile.trackerOffsets?[data.measurementOffset1].position != null)
{
measurementTrackerTransform1 = trackers.GetTrackerWithOffset(data.measurementTracker1, profile.trackerOffsets[data.measurementOffset1].Position, Quaternion.identity);
}
}
if (data.measurementLocal2)
{
if (profile.trackerOffsets?[data.measurementOffset2].position != null)
{
measurementTrackerTransform2 = trackers.GetTrackerWithOffset(data.measurementTracker2, profile.trackerOffsets[data.measurementOffset2].Position, Quaternion.identity);
}
}
if (measurementTrackerTransform1 == null || measurementTrackerTransform2 == null)
{
Debug.LogError("Something went wrong");
continue;
}
profile.AddBodyMeasurement(data.measurement, Vector3.Distance(measurementTrackerTransform1.Value.position, measurementTrackerTransform2.Value.position));
break;
default:
Debug.LogError("Selected align setting type is not implemented");
break;
}
}
foreach (OffsetData data in offsetData)
{
switch (data.type)
{
case OffsetType.AverageOffsetsOnPlane:
if (!profile.trackerOffsets.ContainsKey(data.offsetTracker1) || !profile.trackerOffsets.ContainsKey(data.offsetTracker2) ||
profile.trackerOffsets[data.offsetTracker1].position == null || profile.trackerOffsets[data.offsetTracker2].position == null)
{
Debug.LogError("Not all trackers have required offsets");
}
TrackerOffset offsetTracker1 = profile.trackerOffsets[data.offsetTracker1];
TrackerOffset offsetTracker2 = profile.trackerOffsets[data.offsetTracker2];
TransformValues?offsetTrackerTransform1 = trackers.GetTrackerWithOffset(data.offsetFromTracker1, offsetTracker1.Position, Quaternion.identity);
TransformValues?offsetTrackerTransform2 = trackers.GetTrackerWithOffset(data.offsetFromTracker2, offsetTracker2.Position, Quaternion.identity);
if (offsetTrackerTransform1 == null || offsetTrackerTransform2 == null)
{
Debug.LogError("Not all trackers have required directions");
continue;
}
break;
default:
Debug.LogError("Implement your shit");
break;
}
}
}
