private void VisualizeTrackerOffsets()
{
for (int i = 0; i < profile.trackerOffsets.Keys.Count; i++)
{
if (trackerOffsetVisuals.Count < i + 1)
{
trackerOffsetVisuals.Add(GameObject.CreatePrimitive(PrimitiveType.Sphere));
trackerOffsetVisuals[i].GetComponent <MeshRenderer>().material.color = Color.black;
trackerOffsetVisuals[i].transform.localScale = new Vector3(0.05f, 0.05f, 0.05f);
trackerOffsetVisuals[i].transform.SetParent(transform);
}
OffsetsToTrackers offsetType = profile.trackerOffsets.Keys.ToArray()[i];
VRTrackerType type = CalibrationProfile.GetMatchingTrackerFromOffset(offsetType) ?? VRTrackerType.Other;
TrackerOffset offset = profile.trackerOffsets[offsetType];
TransformValues?trackerWithOffset = trackers.GetTrackerWithOffset(type, offset.Position, Quaternion.identity);
if (trackerWithOffset == null)
{
if (trackerOffsetVisuals[i].activeSelf)
{
trackerOffsetVisuals[i].SetActive(false);
}
continue;
}
if (!trackerOffsetVisuals[i].activeSelf)
{
trackerOffsetVisuals[i].SetActive(true);
}
trackerOffsetVisuals[i]?.transform.SetPositionAndRotation(
trackerWithOffset.Value.position,
trackerWithOffset.Value.rotation);
}
}
private void UpdateTrackerTargets()
{
foreach (VRTrackerType tracker in trackers.RequiredTrackers)
{
TransformValues?trackerTransform = trackers.GetTracker(tracker);
Transform obj = null;
switch (tracker)
{
case VRTrackerType.Head:
obj = head;
if (profile.trackerOffsets.ContainsKey(OffsetsToTrackers.HeadTrackerToHead))
{
TrackerOffset offset = profile.trackerOffsets[OffsetsToTrackers.HeadTrackerToHead];
trackerTransform = trackers.GetTrackerWithOffset(
VRTrackerType.Head,
offset.Position,
offset.Rotation);
}
break;
case VRTrackerType.Waist:
obj = hip;
if (profile.trackerOffsets.ContainsKey(OffsetsToTrackers.HipTrackerToHip))
{
TrackerOffset offset = profile.trackerOffsets[OffsetsToTrackers.HipTrackerToHip];
trackerTransform = trackers.GetTrackerWithOffset(
VRTrackerType.Waist,
offset.Position,
offset.Rotation);
}
break;
case VRTrackerType.LeftHand:
obj = leftHand;
if (profile.trackerOffsets.ContainsKey(OffsetsToTrackers.LeftHandTrackerToWrist))
{
TrackerOffset offset = profile.trackerOffsets[OffsetsToTrackers.LeftHandTrackerToWrist];
trackerTransform = trackers.GetTrackerWithOffset(
VRTrackerType.LeftHand,
offset.Position,
offset.Rotation);
}
break;
case VRTrackerType.RightHand:
obj = rightHand;
if (profile.trackerOffsets.ContainsKey(OffsetsToTrackers.RightHandTrackerToWrist))
{
TrackerOffset offset = profile.trackerOffsets[OffsetsToTrackers.RightHandTrackerToWrist];
trackerTransform = trackers.GetTrackerWithOffset(
VRTrackerType.RightHand,
offset.Position,
offset.Rotation);
}
break;
case VRTrackerType.LeftFoot:
obj = leftFoot;
if (profile.trackerOffsets.ContainsKey(OffsetsToTrackers.LeftFootTrackerToAnkle))
{
TrackerOffset offset = profile.trackerOffsets[OffsetsToTrackers.LeftFootTrackerToAnkle];
trackerTransform = trackers.GetTrackerWithOffset(
VRTrackerType.LeftFoot,
offset.Position,
offset.Rotation);
}
break;
case VRTrackerType.RightFoot:
obj = rightFoot;
if (profile.trackerOffsets.ContainsKey(OffsetsToTrackers.RightFootTrackerToAnkle))
{
TrackerOffset offset = profile.trackerOffsets[OffsetsToTrackers.RightFootTrackerToAnkle];
trackerTransform = trackers.GetTrackerWithOffset(
VRTrackerType.RightFoot,
offset.Position,
offset.Rotation);
}
break;
}
if (trackerTransform == null || obj == null)
{
continue;
}
filters[tracker].UpdateFilter(trackerTransform.Value.position, trackerTransform.Value.rotation);
obj.position = useKalmanOnTrackerData ? filters[tracker].Position : trackerTransform.Value.position;
obj.rotation = useKalmanOnTrackerData ? filters[tracker].Rotation : trackerTransform.Value.rotation;
switch (tracker)
{
case VRTrackerType.LeftFoot:
case VRTrackerType.RightFoot:
float _footHeight = 0.1346572f;
if (obj.position.y < _footHeight)
{
obj.position = new Vector3(obj.position.x, _footHeight, obj.position.z);
}
break;
}
}
}
//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;
}
}
}
private void VisualizeDirection()
{
for (int i = 0; i < profile.trackerDirections.Keys.Count; i++)
{
if (trackerDirectionVisuals.Count < i + 1)
{
trackerDirectionVisuals.Add(
new[]
{
GameObject.CreatePrimitive(PrimitiveType.Cube),
GameObject.CreatePrimitive(PrimitiveType.Cube),
GameObject.CreatePrimitive(PrimitiveType.Cube)
});
trackerDirectionVisuals[i][0].GetComponent <MeshRenderer>().material.color = Color.red;
trackerDirectionVisuals[i][1].GetComponent <MeshRenderer>().material.color = Color.green;
trackerDirectionVisuals[i][2].GetComponent <MeshRenderer>().material.color = Color.blue;
foreach (GameObject obj in trackerDirectionVisuals[i])
{
obj.transform.localScale = new Vector3(0.02f, 0.02f, 0.02f);
obj.transform.SetParent(transform);
}
}
VRTrackerType trackerType = profile.trackerDirections.Keys.ToArray()[i];
if (alsoHead == false && trackerType == VRTrackerType.Head)
{
continue;
}
OffsetsToTrackers?offsetType = CalibrationProfile.GetMatchingTrackerOffsetForTracker(trackerType);
if (offsetType == null)
{
continue;
}
Matrix4x4 trackerMatrix;
if (profile.trackerOffsets.ContainsKey((OffsetsToTrackers)offsetType))
{
TrackerOffset trackerOffset = profile.trackerOffsets[(OffsetsToTrackers)offsetType];
TransformValues trackerTransform =
trackers.GetTrackerWithOffset(trackerType, trackerOffset.Position, Quaternion.identity)
?? new TransformValues(Vector3.zero, Quaternion.identity);
trackerMatrix = Matrix4x4.TRS(trackerTransform.position, trackerTransform.rotation, Vector3.one);
}
else
{
TransformValues trackerTransform =
trackers.GetTracker(trackerType)
?? new TransformValues(Vector3.zero, Quaternion.identity);
trackerMatrix = Matrix4x4.TRS(trackerTransform.position, trackerTransform.rotation, Vector3.one);
}
TrackerDirection trackerDirection = profile.trackerDirections[trackerType];
if (trackerDirection.X != Vector3.zero)
{
trackerDirectionVisuals[i][0].transform.localScale = new Vector3(0.02f, 0.02f, 0.02f);
trackerDirectionVisuals[i][0].transform.position = trackerMatrix.GetPosition() + trackerMatrix.MultiplyVector(trackerDirection.X) * 0.1f;
}
else
{
trackerDirectionVisuals[i][0].transform.localScale = Vector3.zero;
}
if (trackerDirection.Y != Vector3.zero)
{
trackerDirectionVisuals[i][1].transform.localScale = new Vector3(0.02f, 0.02f, 0.02f);
trackerDirectionVisuals[i][1].transform.position = trackerMatrix.GetPosition() + trackerMatrix.MultiplyVector(trackerDirection.Y) * 0.1f;
}
else
{
trackerDirectionVisuals[i][1].transform.localScale = Vector3.zero;
}
if (trackerDirection.Z != Vector3.zero)
{
trackerDirectionVisuals[i][2].transform.localScale = new Vector3(0.02f, 0.02f, 0.02f);
trackerDirectionVisuals[i][2].transform.position = trackerMatrix.GetPosition() + trackerMatrix.MultiplyVector(trackerDirection.Z) * 0.1f;
}
else
{
trackerDirectionVisuals[i][2].transform.localScale = Vector3.zero;
}
}
}
private void ProcessData(Data[] arcData)
{
foreach (Data data in arcData)
{
switch (data.dataType)
{
case DataType.OffsetToTracker:
// Add offset on local plane
if (data.onLocalPlane)
{
// if profile doesn't contain a offset yet, just add it normally
if (profile.trackerOffsets.ContainsKey(data.trackerOffset) && profile.trackerOffsets[data.trackerOffset].position != null)
{
TrackerOffset offset = profile.trackerOffsets[data.trackerOffset];
VRTrackerType trackerType = (VRTrackerType)CalibrationProfile.GetMatchingTrackerFromOffset(data.trackerOffset);
// if profile doesn't contain the required axis ignore
if (profile.trackerDirections.ContainsKey(trackerType) && profile.trackerDirections[trackerType].GetAxis(data.localPlane) != null)
{
TransformValues trackerTransform = (TransformValues)trackers.GetTracker(trackerType);
Matrix4x4 trackerMatrix = Matrix4x4.TRS(trackerTransform.position, trackerTransform.rotation, Vector3.one);
Matrix4x4 inverseTrackerMatrix = trackerMatrix.inverse;
Vector3 localAxis = trackerMatrix.MultiplyVector(profile.trackerDirections[trackerType].GetAxis(data.localPlane) ?? Vector3.zero);
Vector3 newPositionDirection = trackerMatrix.MultiplyPoint3x4(arcArray[data.arcPositionIndex].GetOffsetToTracker()) - trackerMatrix.MultiplyPoint3x4((Vector3)offset.position);
Vector3 newPosition = trackerMatrix.MultiplyPoint3x4((Vector3)offset.position) + Vector3.ProjectOnPlane(newPositionDirection, localAxis);
newPosition = inverseTrackerMatrix.MultiplyPoint3x4(newPosition);
// Debug.Log(newPosition.magnitude);
// Debug.DrawLine(trackerMatrix.MultiplyPoint3x4(arcArray[data.arcPositionIndex].GetOffsetToTracker()), trackerMatrix.MultiplyPoint3x4(arcArray[data.arcPositionIndex].GetOffsetToTracker()), Color.blue, 10f);
// Debug.DrawRay(trackerMatrix.MultiplyPoint3x4(arcArray[data.arcPositionIndex].GetOffsetToTracker()), localAxis, Color.red, 10f);
arcArray[data.arcPositionIndex].GetOffsetToTracker();
profile.AddTrackerOffset(data.trackerOffset, newPosition);
continue;
}
Debug.LogError($"Could not apply local offset to {trackerType}, plane axis is not applied");
}
}
profile.AddTrackerOffset(data.trackerOffset, arcArray[data.arcPositionIndex].GetOffsetToTracker());
break;
case DataType.Length:
// TODO: TMP
foreach (int index in data.arcMeasurementIndices)
{
GameObject sphere = GameObject.CreatePrimitive(PrimitiveType.Sphere);
sphere.transform.position = arcArray[index].IntersectionPoint;
sphere.transform.localScale = new Vector3(.05f, .05f, .05f);
}
float value = data.arcMeasurementIndices.Sum(arc => arcArray[arc].GetArcRadius()) / data.arcMeasurementIndices.Length;
profile.AddBodyMeasurement(data.measurement, value);
break;
case DataType.Distance:
Vector3 point1 = Vector3.zero;
Vector3 point2 = Vector3.zero;
// TODO: make this a function
switch (data.point1.pointType)
{
case PointType.ArcPoint:
point1 = arcArray[data.point1.arcIndex].IntersectionPoint;
break;
case PointType.Tracker:
point1 = Vector3.zero; // TODO: calculate local offset to tracker
break;
}
switch (data.point2.pointType)
{
case PointType.ArcPoint:
point2 = arcArray[data.point2.arcIndex].IntersectionPoint;
break;
case PointType.Tracker:
point2 = Vector3.zero; // TODO: calculate local offset to tracker
break;
}
float distance = Vector3.Distance(point1, point2);
profile.AddBodyMeasurement(data.distanceMeasurement, distance);
break;
case DataType.Direction:
Vector3 normal = arcArray[data.arcDirectionIndex].GetArcNormalFromTracker();
VRTrackerType trackerDirectionType = settings[data.arcDirectionIndex].parentTracker;
TransformValues?trackerDirectionTransform = trackers.GetTracker(trackerDirectionType);
if (trackerDirectionTransform == null)
{
Debug.Log("Not all trackers are assigned");
return;
}
Matrix4x4 trackerDirectionMatrix = Matrix4x4.TRS(trackerDirectionTransform.Value.position, trackerDirectionTransform.Value.rotation, Vector3.one);
Vector3 worldNormal = trackerDirectionMatrix.MultiplyVector(normal);
switch (data.directionClosest.type)
{
case DirectionClosestType.SingleDirection:
Vector3 dir = Vector3.zero;
// TODO: make this a function
switch (data.directionClosest.singleDirection.type)
{
case DirectionType.WorldDirection:
dir = data.directionClosest.singleDirection.worldDirection;
dir = dir.normalized;
break;
case DirectionType.TrackerDirection:
TransformValues?trackerFrom = trackers.GetTracker(data.directionClosest.singleDirection.trackerFrom);
TransformValues?trackerTo = trackers.GetTracker(data.directionClosest.singleDirection.trackerTo);
if (trackerFrom == null || trackerTo == null)
{
Debug.LogError("Not all trackers are connected");
return;
}
dir = trackerTo.Value.position - trackerFrom.Value.position;
dir = dir.normalized;
break;
}
Debug.DrawRay(trackers.GetTracker(VRTrackerType.LeftHand).Value.position, worldNormal, Color.magenta, 10f);
Debug.DrawRay(trackers.GetTracker(VRTrackerType.LeftHand).Value.position, dir, Color.cyan, 10f);
if (Vector3.Distance(dir, worldNormal) > Vector3.Distance(dir, worldNormal * -1))
{
normal *= -1f;
}
break;
case DirectionClosestType.Cross:
Vector3 crossDir1 = Vector3.zero;
Vector3 crossDir2 = Vector3.zero;
// TODO: make this a function
switch (data.directionClosest.crossDirection1.type)
{
case DirectionType.WorldDirection:
crossDir1 = data.directionClosest.crossDirection1.worldDirection;
break;
case DirectionType.TrackerDirection:
TransformValues?trackerFrom = trackers.GetTracker(data.directionClosest.crossDirection1.trackerFrom);
TransformValues?trackerTo = trackers.GetTracker(data.directionClosest.crossDirection1.trackerTo);
if (trackerFrom == null || trackerTo == null)
{
Debug.LogError("Not all trackers are connected");
return;
}
crossDir1 = trackerTo.Value.position - trackerFrom.Value.position;
break;
}
switch (data.directionClosest.crossDirection2.type)
{
case DirectionType.WorldDirection:
crossDir2 = data.directionClosest.crossDirection2.worldDirection;
break;
case DirectionType.TrackerDirection:
TransformValues?trackerFrom = trackers.GetTracker(data.directionClosest.crossDirection2.trackerFrom);
TransformValues?trackerTo = trackers.GetTracker(data.directionClosest.crossDirection2.trackerTo);
if (trackerFrom == null || trackerTo == null)
{
Debug.LogError("Not all trackers are connected");
return;
}
crossDir2 = trackerTo.Value.position - trackerFrom.Value.position;
break;
}
Vector3 cross = Vector3.Cross(crossDir1, crossDir2);
if (Vector3.Distance(cross, worldNormal) > Vector3.Distance(cross, worldNormal * -1))
{
normal *= -1f;
}
Debug.DrawRay(trackers.GetTracker(VRTrackerType.LeftHand).Value.position, worldNormal, Color.magenta, 10f);
Debug.DrawRay(trackers.GetTracker(VRTrackerType.LeftHand).Value.position, cross, Color.cyan, 10f);
break;
default:
Debug.LogError("Implement your shit");
break;
}
profile.AddTrackerDirection(trackerDirectionType, data.directionAxis, normal);
break;
}
}
}
