public Vector3 GetOffsetToTracker() { if (trackers == null) { return(Vector3.zero); } // Transform trackerTransform = new GameObject("trackerPosition").transform; var offsetsToTracker = new List <Vector3>(); foreach (ArcPoint value in arcPoints) { if (value.parent == null) { continue; } TransformValues parentValues = (TransformValues)value.parent; Matrix4x4 trackerMatrix = Matrix4x4.TRS(parentValues.position, parentValues.rotation, Vector3.one); trackerMatrix = trackerMatrix.inverse; //trackerTransform.position = parentValues.position; //trackerTransform.rotation = parentValues.rotation; Vector3 pointOffset = trackerMatrix.MultiplyPoint3x4(intersectionPoint); // trackerTransform.InverseTransformPoint(intersectionPoint); offsetsToTracker.Add(pointOffset); } // MonoBehaviour.Destroy(trackerTransform.gameObject); // TODO: remove all transform stuff return(AverageVector(offsetsToTracker.ToArray())); }
public Vector3 GetArcNormalFromTracker() { if (trackers == null) { return(Vector3.zero); } var normalsFromTracker = new List <Vector3>(); foreach (ArcPoint value in arcPoints) { if (value.parent == null) { continue; } TransformValues parentValues = (TransformValues)value.parent; Matrix4x4 trackerMatrix = Matrix4x4.TRS(parentValues.position, parentValues.rotation, Vector3.one); trackerMatrix = trackerMatrix.inverse; Vector3 trackerNormal = trackerMatrix.MultiplyVector(normal); normalsFromTracker.Add(trackerNormal); } return(AverageVector(normalsFromTracker.ToArray())); }
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; } } }