/// <inheritdocs />
public AnchorId AddAlignmentAnchor(string uniqueName, Pose virtualPose, Pose lockedPose)
{
FragmentId fragmentId = CurrentFragmentId;
AnchorId anchorId = ClaimAnchorId();
if (IsGlobal)
{
/// Bake in current snapshot of any application imposed transform (teleport).
virtualPose = manager.PinnedFromFrozen.Multiply(virtualPose);
}
else
{
/// For subtree, applied adjustment transform is LockedFromPinned. Remove existing
/// adjustment here by premultiplying PinnedFromLocked.
virtualPose = PinnedFromLocked.Multiply(virtualPose);
}
#if WLT_EXTRA_LOGGING
string label = "AddAlign1";
Debug.Log($"F{Time.frameCount} {label} {uniqueName} vp={virtualPose.ToString("F3")} lp={lockedPose.ToString("F3")} sp={manager.SpongyFromLocked.Multiply(lockedPose).ToString("F3")}");
#endif // WLT_EXTRA_LOGGING
ReferencePose refPose = new ReferencePose()
{
name = uniqueName,
fragmentId = fragmentId,
anchorId = anchorId,
virtualPose = virtualPose
};
refPose.LockedPose = lockedPose;
referencePoses.Add(refPose);
QueueForSave(refPose);
return(anchorId);
}
/// <inheritdocs />
public AnchorId RestoreAlignmentAnchor(string uniqueName, Pose virtualPose)
{
/// mafinc - this API needs settling.
/// virtualPose unused, it needs to be checked against the virtualPose in the found refPose (if any).
ReferencePose refPose = poseDB.Get(uniqueName);
if (refPose == null)
{
return(AnchorId.Invalid);
}
var index = referencePoses.FindIndex(x => x.name == uniqueName);
if (index >= 0)
{
/// The reference pose already exists. Update it by replacing it
/// with the new refpose using same anchor id.
refPose.anchorId = referencePoses[index].anchorId;
referencePoses[index] = refPose;
}
else
{
referencePoses.Add(refPose);
}
/// If the referencePose has an invalid fragment id, it's only because there isn't a valid
/// fragment right now. Flag the condition and set the proper fragment id when there is
/// a valid one.
if (!refPose.fragmentId.IsKnown())
{
needFragment = true;
}
return(refPose.anchorId);
}
/// <summary>
/// Compute the PinnedFromLocked pose for the given reference pose.
/// </summary>
/// <param name="refPose">The reference pose to evaluate.</param>
/// <returns>The computed PinnedFromLocked pose.</returns>
private Pose ComputePinnedFromLocked(ReferencePose refPose)
{
Pose pinnedFromLocked = Pose.identity;
if (IsGlobal)
{
/// Here we essentially solve for pose Z, where
/// refPose.virtualPose == FrozenFromPinned * Z * refPose.lockedPose.
Pose pinnedFromFrozen = Pose.identity;
if (manager.AdjustmentFrame.parent != null)
{
pinnedFromFrozen = manager.AdjustmentFrame.parent.GetGlobalPose().Inverse();
}
Pose frozenFromObject = refPose.virtualPose;
Pose objectFromLocked = refPose.LockedPose.Inverse();
pinnedFromLocked = pinnedFromFrozen
.Multiply(frozenFromObject)
.Multiply(objectFromLocked);
}
else
{
/// The math is slightly different for an alignment manager being applied to a subgraph of the scene.
/// Here we are essentially solving for pose Z, such that
/// Z * refPose.virtualPose == FrozenFromLocked * refPose.LockedPose.
Pose frozenFromVirtual = manager.FrozenFromLocked
.Multiply(refPose.LockedPose)
.Multiply(refPose.virtualPose.Inverse());
pinnedFromLocked = frozenFromVirtual.Inverse();
}
// mafinc - obviously this could be cached when refPose.LockedPose is set (and changed).
return(pinnedFromLocked);
}
/// <summary>
/// Compute the PinnedFromLocked pose for the given reference pose.
/// </summary>
/// <param name="refPose">The reference pose to evaluate.</param>
/// <returns>The computed PinnedFromLocked pose.</returns>
private Pose ComputePinnedFromLocked(ReferencePose refPose)
{
Pose pinnedFromLocked = Pose.identity;
if (IsGlobal)
{
/// Here we essentially solve for pose Z, where
/// refPose.virtualPose == Z * refPose.lockedPose.
/// More precisely, we solve for PfL in:
/// AppFromHolder * HolderFromObject = AppFromPinned * PinnedFromLocked * LockedFromObject, or
/// AfH * HfO = AfP * PfL * LfO
/// PfA * AfH * HfO * OfL = PfL
/// refPose.virtualPose == PfA * AfH * HfO, and refPose.LockedPose == LockedFromObject, so it reduces to the above simpler line.
Pose pinnedFromObject = refPose.virtualPose;
Pose objectFromLocked = refPose.LockedPose.Inverse();
pinnedFromLocked = pinnedFromObject.Multiply(objectFromLocked);
}
else
{
/// The math is slightly different for an alignment manager being applied to a subgraph of the scene.
/// Here we are essentially solving for pose Z, such that
/// Z * refPose.virtualPose == FrozenFromLocked * refPose.LockedPose.
Pose frozenFromVirtual = manager.FrozenFromLocked
.Multiply(refPose.LockedPose)
.Multiply(refPose.virtualPose.Inverse());
pinnedFromLocked = frozenFromVirtual.Inverse();
}
// mafinc - obviously this could be cached when refPose.LockedPose is set (and changed).
return(pinnedFromLocked);
}
/// <summary>
/// Add to queue for being saved to database next chance.
/// </summary>
/// <param name="refPose"></param>
private void QueueForSave(ReferencePose refPose)
{
int idx = FindReferencePoseById(referencePosesToSave, refPose.anchorId);
if (idx < 0)
{
referencePosesToSave.Add(refPose);
}
}
/// <summary>
/// Add or update a reference pose to the database.
/// </summary>
/// <param name="refPose">The reference pose to add/update to the database.</param>
/// <returns>True on success.</returns>
public bool Set(ReferencePose refPose)
{
Element elem = new Element()
{
virtualPose = refPose.virtualPose, lockedPose = refPose.LockedPose
};
data[refPose.name] = elem;
return(true);
}
/// <summary>
/// Add to queue for being saved to database next chance.
/// </summary>
/// <param name="refPose"></param>
private void QueueForSave(ReferencePose refPose)
{
DebugLogSaveLoad($"QueueForSave {SaveFileName}");
int idx = FindReferencePoseById(referencePosesToSave, refPose.anchorId);
if (idx < 0)
{
referencePosesToSave.Add(refPose);
}
}
/// <summary>
/// If the given name is represented in the database, create a corresponding reference point.
/// </summary>
/// <param name="uniqueName">Unique name for the reference point.</param>
/// <returns>A valid reference point if found, else null.</returns>
public ReferencePose Get(string uniqueName)
{
Element src;
if (!data.TryGetValue(uniqueName, out src))
{
return(null);
}
ReferencePose refPose = new ReferencePose();
refPose.name = uniqueName;
refPose.fragmentId = CurrentFragmentId;
refPose.anchorId = ClaimAnchorId();
refPose.virtualPose = src.virtualPose;
refPose.LockedPose = src.lockedPose;
return(refPose);
}
public override void Update()
{
if (BasePose == null || ReferencePose == null || AddPose == null)
{
return;
}
var addPose = AddPose.Clone();
var refPose = ReferencePose.Clone();
var basePose = BasePose.Clone();
var tempPose = BasePose.Clone();
Bricks.Animation.Runtime.CGfxAnimationRuntime.ConvertRotationToMeshSpace(addPose);
Bricks.Animation.Runtime.CGfxAnimationRuntime.ConvertRotationToMeshSpace(refPose);
Bricks.Animation.Runtime.CGfxAnimationRuntime.MinusPose(tempPose, refPose, addPose);
Bricks.Animation.Runtime.CGfxAnimationRuntime.ConvertRotationToMeshSpace(basePose);
Bricks.Animation.Runtime.CGfxAnimationRuntime.AddPose(OutPose, basePose, tempPose, 1);
Bricks.Animation.Runtime.CGfxAnimationRuntime.ConvertRotationToLocalSpace(OutPose);
}
/// <inheritdocs />
public AnchorId AddAlignmentAnchor(string uniqueName, Pose virtualPose, Pose lockedPose)
{
FragmentId fragmentId = CurrentFragmentId;
AnchorId anchorId = ClaimAnchorId();
ReferencePose refPose = new ReferencePose()
{
name = uniqueName,
fragmentId = fragmentId,
anchorId = anchorId,
virtualPose = virtualPose
};
refPose.LockedPose = lockedPose;
referencePoses.Add(refPose);
QueueForSave(refPose);
return(anchorId);
}
/// <inheritdocs />
public AnchorId RestoreAlignmentAnchor(string uniqueName, Pose virtualPose)
{
/// mafinc - this API needs settling.
/// virtualPose unused, it needs to be checked against the virtualPose in the found refPose (if any).
ReferencePose refPose = poseDB.Get(uniqueName);
if (refPose == null)
{
return(AnchorId.Invalid);
}
referencePoses.Add(refPose);
/// If the referencePose has an invalid fragment id, it's only because there isn't a valid
/// fragment right now. Flag the condition and set the proper fragment id when there is
/// a valid one.
if (!refPose.fragmentId.IsKnown())
{
needFragment = true;
}
return(refPose.anchorId);
}
