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