unsafe public void AddFrozenAnchor(AnchorId id, Pose frozenPose)
{
int idx = FindFrozenAnchor(id);
if (idx >= 0)
{
var frozenAnchorPose = frozenAnchors[idx];
frozenAnchorPose.anchorPose.pose = frozenPose;
frozenAnchors[idx] = frozenAnchorPose;
}
else
{
idx = ~idx;
var frozenAnchorPose = new AnchorPoseRelevance()
{
anchorPose = new AnchorPose()
{
anchorId = id,
pose = frozenPose
},
relevance = 0.0f
};
frozenAnchors.Insert(idx, frozenAnchorPose);
}
checkError();
}
private int FindFrozenAnchor(AnchorId targetAnchorId)
{
AnchorPoseRelevance target = new AnchorPoseRelevance()
{
anchorPose = new AnchorPose()
{
anchorId = targetAnchorId,
pose = Pose.identity
},
relevance = 0.0f
};
int idx = frozenAnchors.BinarySearch(target, new FrozenAnchorById());
return(idx);
}
unsafe public void AddSpongyAnchors(List <AnchorPose> spongyAnchors)
{
spongyAnchors.Sort((x, y) => { return(x.anchorId.CompareTo(y.anchorId)); });
int spongyIdx = 0;
for (int frozenIdx = 0; frozenIdx < frozenAnchors.Count; ++frozenIdx)
{
if (spongyIdx >= spongyAnchors.Count)
{
break;
}
if (frozenAnchors[frozenIdx].anchorPose.anchorId == spongyAnchors[spongyIdx].anchorId)
{
var frozenAnchor = frozenAnchors[frozenIdx];
frozenAnchor.anchorPose.pose = spongyAnchors[spongyIdx].pose;
frozenAnchor.relevance = 1.0f;
frozenAnchors[frozenIdx] = frozenAnchor;
++spongyIdx;
}
else if (frozenAnchors[frozenIdx].anchorPose.anchorId > spongyAnchors[spongyIdx].anchorId)
{
// Insert.
var frozenAnchor = new AnchorPoseRelevance()
{
anchorPose = spongyAnchors[spongyIdx],
relevance = 1.0f
};
frozenAnchors.Insert(frozenIdx, frozenAnchor);
++spongyIdx;
}
/// else frozen[fIdx] < spongy[sIdx]
/// ++frozenIdx (but not spongyIdx, because we haven't processed this one yet).
}
while (spongyIdx < spongyAnchors.Count)
{
// append
var frozenAnchor = new AnchorPoseRelevance()
{
anchorPose = spongyAnchors[spongyIdx],
relevance = 1.0f
};
frozenAnchors.Add(frozenAnchor);
++spongyIdx;
}
checkError();
}
