public void Update() { if (waypoint == null) { return; } waypoint.Pose.SetFromTransform(this.transform); //Check if current anchor is still within the heuristics if (Math.Sqrt(waypoint.Pose.values.Take(3).Select(x => x * x).Sum()) > maxDinstanceFromAnchorCenter) { //we have to change the anchor. //Look for nearby anchors (SpatialAnchorController closest, bool isWithinMaxDistance) = missionController.FindAnchorNearMe(this.transform, maxDinstanceFromAnchorCenter); SpatialAnchorController futureParentController = null; if (closest == null || !isWithinMaxDistance) { //if none are found -> create new one RobotUtilities.Pose pose = new RobotUtilities.Pose(); Vector3 displacement = (this.transform.position - closest.transform.position).normalized * maxDinstanceFromAnchorCenter * 1.5f; displacement.y = 0; Vector3 relativePosToParent = closest.transform.InverseTransformPoint(closest.transform.position + displacement); futureParentController = missionController.CreateAnchor((RobotUtilities.Pose)relativePosToParent.ToIPose <RobotUtilities.Pose>(), closest.GetAnchor()); } else { futureParentController = closest; } futureParentController.SetAnchorAsParent(this.gameObject, true); this.waypoint.Pose.SetFromTransform(transform); this.waypoint.SetPose(waypoint.Pose, futureParentController.GetAnchor()); } }