/// <summary> /// Create an Anchors GameObjects /// </summary> /// <param name="newAnchor"></param> /// <param name="parentAnchor"></param> /// <returns></returns> private SpatialAnchorController CreateAnchorGameObject(SpatialAnchor newAnchor, SpatialAnchor parentAnchor = null) { GameObject anchorObject = Instantiate(SpatialAnchorPrefab, SpatialAnchorsContainer.transform); SpatialAnchorController controller = anchorObject.GetComponent <SpatialAnchorController>(); mission.AddAnchor(newAnchor); controller.InitAsync(newAnchor); SpatialAnchorToControllerMap[newAnchor] = controller; if (parentAnchor == null) { parentAnchor = mission.SpatialAnchors.FirstOrDefault(x => x.Id == newAnchor.ParentAnchorId); } if (parentAnchor != null) { controller.SetPose(SpatialAnchorToControllerMap[parentAnchor]); } return(controller); }
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()); } }
public void SetPose(SpatialAnchorController parentController) { this.transform.localPosition = parentController.transform.localPosition + Anchor.RelativePoseToParent.ToPositionVector(); this.transform.localRotation = parentController.transform.localRotation * Anchor.RelativePoseToParent.ToRotationQuaternion(); }
/// <summary> /// Sets the current anchor, such that all future commands will be sent realtive to it. /// </summary> /// <param name="anchor"></param> public void SetCurrentAnchor(SpatialAnchorController anchor) { currentAnchorController = anchor; }