//TODO: Expose outputs that visualizers or neural can use //TODO: Can't just say where != null. Need to account for null times public Tuple <TrackedItemBase, Point, Vector> GetPreviousPosition(double time) { // Find the two snapshots that straddle this requested time ItemHistoryEntry low = null; ItemHistoryEntry high = null; foreach (ItemHistoryEntry item in _snapshots.EnumerateReverse()) { if (item.Time.IsNearValue(time)) { if (item.Item == null) { // There was no item at this time return(null); } else { return(Tuple.Create(item.Item, item.Position_Velocity.Item1, item.Position_Velocity.Item2)); } } else if (item.Time < time) { low = item; break; } else { high = item; } } if (_snapshots.HasWrapped && (low == null || high == null)) { _snapshots.ChangeSize(Convert.ToInt32(_snapshots.MaxCount * 1.5)); } // Return the lerp of those two if (low?.Item == null && high?.Item == null) { return(null); } else if (low?.Item != null && high?.Item != null) { double percent = (time - low.Time) / (high.Time - low.Time); if (percent.IsInvalid()) // it should only be invalid if the denominator is zero { percent = .5; } if (low.Item.Token == high.Item.Token) { return(Tuple.Create( low.Item, Math2D.LERP(low.Position_Velocity.Item1, high.Position_Velocity.Item1, percent), Math2D.LERP(low.Position_Velocity.Item2, high.Position_Velocity.Item2, percent))); } else if (percent < .5) { // The item tokens changed between time steps, and this time is closer to the request, so return it return(Tuple.Create(low.Item, low.Position_Velocity.Item1, low.Position_Velocity.Item2)); } else { return(Tuple.Create(high.Item, high.Position_Velocity.Item1, high.Position_Velocity.Item2)); } } else if (low?.Item != null) { return(Tuple.Create(low.Item, low.Position_Velocity.Item1, low.Position_Velocity.Item2)); // inaccurate, but as good as can be found (shouldn't happen, or very rarely) } else { // This happens when the harness is first created and being used. There hasn't been enough history yet. So return null instead of the item's current position //return Tuple.Create(high.Item, high.Position, high.Velocity); return(null); } }