public OrientationInterpolator(IOrientable target, quat start, quat end, EaseTypes easeType,
float duration = 0, bool isRepeatable = true) : base(start, end, duration, easeType, isRepeatable)
{
Debug.Assert(target != null, NullMessage);
this.target = target;
}
/// <summary>
/// Compute yaw and pitch to align virtual line with physical.
/// </summary>
/// <param name="a">First point</param>
/// <param name="b">Second point</param>
/// <returns>Computed rotation weighted by inverse distance between points.</returns>
protected override WeightedRotation ComputeRotation(IOrientable a, IOrientable b)
{
Vector3 lockedAtoB = b.LockedPosition - a.LockedPosition;
lockedAtoB.Normalize();
Vector3 virtualAtoB = b.ModelPosition - a.ModelPosition;
virtualAtoB.Normalize();
Quaternion rotVirtualFromLocked = Quaternion.FromToRotation(virtualAtoB, lockedAtoB);
rotVirtualFromLocked.Normalize();
float weight = (a.ModelPosition - b.ModelPosition).sqrMagnitude;
float minDistSq = 0.0f;
weight = weight > minDistSq ? 1.0f / weight : 1.0f;
return(new WeightedRotation()
{
orientable = null,
rotation = rotVirtualFromLocked,
weight = weight
});
}
/// <inheritdocs />
public void Register(IOrientable orientable)
{
int idx = orientables.FindIndex(o => o == orientable);
if (idx < 0)
{
orientables.Add(orientable);
}
}
/// <summary>
/// Compute 3-DOF rotation to align virtual to physical space.
/// </summary>
/// <param name="a">First point</param>
/// <param name="b">Second point</param>
/// <param name="c">Third point</param>
/// <returns>Alignment rotation weighted by suitability of points.</returns>
private WeightedRotation ComputeRotation(IOrientable a, IOrientable b, IOrientable c)
{
Vector3 lockedA = a.LockedPosition;
Vector3 lockedB = b.LockedPosition;
Vector3 lockedC = c.LockedPosition;
Vector3 lockedBtoA = lockedA - lockedB;
Vector3 lockedBtoC = lockedC - lockedB;
float weight = ComputeWeight(a.ModelPosition, b.ModelPosition, c.ModelPosition);
Quaternion rotVirtualFromLocked = Quaternion.identity;
if (weight > 0)
{
Vector3 virtualBtoA = a.ModelPosition - b.ModelPosition;
Vector3 virtualBtoC = c.ModelPosition - b.ModelPosition;
// First compute a rotation aligning the virtual line A-B to the locked line A-B.
Quaternion rotationFirst = Quaternion.FromToRotation(virtualBtoA, lockedBtoA);
// Now compute a roll about that line which aligns the third virtual point C to locked C.
virtualBtoC = rotationFirst * virtualBtoC;
Vector3 dir = lockedBtoA;
dir.Normalize();
Vector3 up = Vector3.Cross(lockedBtoC, dir);
up.Normalize();
Vector3 right = Vector3.Cross(dir, up);
float sinRads = Vector3.Dot(virtualBtoC, up);
float cosRads = Vector3.Dot(virtualBtoC, right);
float rotRads = Mathf.Atan2(sinRads, cosRads);
Quaternion rotationSecond = Quaternion.AngleAxis(Mathf.Rad2Deg * rotRads, dir);
rotVirtualFromLocked = rotationSecond * rotationFirst;
rotVirtualFromLocked.Normalize();
}
return(new WeightedRotation()
{
orientable = null,
rotation = rotVirtualFromLocked,
weight = weight
});
}
public OrientationInterpolator(IOrientable target, EaseTypes easeType, bool isRepeatable = true) :
this(target, quat.Identity, quat.Identity, easeType, 0, isRepeatable)
{
}
/// <inheritdocs />
public void Unregister(IOrientable orientable)
{
orientables.Remove(orientable);
}
public OrientationSmoother(IOrientable target, quat start, quat end, float duration, EaseTypes easeType) :
base(start, end, duration, easeType)
{
this.target = target;
}
public Motor(IOrientable parent)
{
this.parent = parent;
}
