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; } } }