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