public bool GetData(ulong time_, ref TransformStorage data_out, out string error_str)
{
TransformStorage temp1 = null, temp2 = null;
int num_nodes = FindClosest(ref temp1, ref temp2, time_, out error_str);
switch (num_nodes)
{
case 0:
return(false);
case 1:
data_out = temp1;
break;
case 2:
if (temp1.FrameId == temp2.FrameId)
{
Interpolate(temp1, temp2, time_, ref data_out);
}
else
{
data_out = temp1;
}
break;
default:
throw new Exception("Function getData in TimeCache.cs failed: num_nodes has to be <=2.");
}
return(true);
}
public TimeAndFrameId GetLatestTimeAndParent()
{
lock (storage)
{
if (storage.Count == 0)
{
return(new TimeAndFrameId(0, 0));
}
TransformStorage ts = storage.Last().Value;
return(new TimeAndFrameId(ts.Stamp, ts.FrameId));
}
}
public uint GetParent(ulong time, out string errorMessage)
{
TransformStorage temp1 = null, temp2 = null;
int num_nodes = FindClosest(ref temp1, ref temp2, time, out errorMessage);
if (num_nodes == 0)
{
return(0);
}
return(temp1.FrameId);
}
public uint getParent(ulong time, out string error_str)
{
TransformStorage temp1 = null, temp2 = null;
int num_nodes = findClosest(ref temp1, ref temp2, time, out error_str);
if (num_nodes == 0)
{
return(0);
}
return(temp1.frame_id);
}
public bool InsertData(TransformStorage newData)
{
lock (storage)
{
if (storage.Count > 0 && storage.First().Key > newData.Stamp + maxStorageTime)
{
if (!SimTime.Instance.IsTimeSimulated)
{
return(false);
}
storage.Clear();
}
storage[newData.Stamp] = newData;
PruneList();
}
return(true);
}
private void Interpolate(TransformStorage one, TransformStorage two, ulong time, ref TransformStorage output)
{
if (one.Stamp == two.Stamp)
{
output = two;
return;
}
if (output == null)
{
output = new TransformStorage();
}
double ratio = (time - one.Stamp) / (two.Stamp - one.Stamp);
output.Translation = Vector3.Lerp(one.Translation, two.Translation, ratio);
output.Rotation = Slerp(one.Rotation, two.Rotation, ratio);
output.Stamp = one.Stamp;
output.FrameId = one.FrameId;
output.ChildFrameId = one.ChildFrameId;
}
private void interpolate(TransformStorage one, TransformStorage two, ulong time, ref TransformStorage output)
{
if (one.stamp == two.stamp)
{
output = two;
return;
}
if (output == null)
{
output = new TransformStorage();
}
double ratio = (time - one.stamp) / (two.stamp - one.stamp);
output.translation.setInterpolate3(one.translation, two.translation, ratio);
output.rotation = slerp(one.rotation, two.rotation, ratio);
output.stamp = one.stamp;
output.frame_id = one.frame_id;
output.child_frame_id = one.child_frame_id;
}
private int FindClosest(ref TransformStorage one, ref TransformStorage two, ulong targetTime, out string errorMessage)
{
errorMessage = null;
lock (storage)
{
if (storage.Count == 0)
{
errorMessage = CreateEmptyException();
return(0);
}
if (targetTime == 0)
{
one = storage.Last().Value;
return(1);
}
if (storage.Count == 1)
{
TransformStorage ts = storage.First().Value;
if (ts.Stamp == targetTime)
{
one = ts;
return(1);
}
errorMessage = CreateExtrapolationException1(targetTime, ts.Stamp);
return(0);
}
ulong latestTime = storage.Last().Key;
ulong earliestTime = storage.First().Key;
if (targetTime == latestTime)
{
one = storage.Last().Value;
return(1);
}
if (targetTime == earliestTime)
{
one = storage.First().Value;
return(1);
}
if (targetTime > latestTime)
{
errorMessage = CreateExtrapolationException2(targetTime, latestTime);
return(0);
}
if (targetTime < earliestTime)
{
errorMessage = CreateExtrapolationException3(targetTime, earliestTime);
return(0);
}
ulong i = 0;
ulong j = storage.Last(kvp =>
{
// look for the first keyvaluepair in the sorted list with a key greater than our target.
// if it is the last keyvaluepair's key, aka, the highest stamp
if (kvp.Key <= targetTime)
{
i = kvp.Key;
return(false);
}
return(true);
}).Key;
one = storage[i];
two = storage[j];
}
return(2);
}
public bool GetData(TimeData time_, ref TransformStorage data_out, out string error_str)
{
return(GetData(ToLong(time_), ref data_out, out error_str));
}
