public AnimationPose GetPosition(double time)
{
_sync.EnterReadLock();
try
{
//find the speedControl pose
PoseSpeedControl currentSpeedControl = null;
var index = 0;
var startTime = 0d;
if (time < 0)
{
currentSpeedControl = _speedControls.FirstOrDefault();
}
else if (time > _totalTime)
{
currentSpeedControl = _speedControls.LastOrDefault();
if (currentSpeedControl != null)
{
startTime = _totalTime - currentSpeedControl.TotalTime;
index = _speedControls.Count - 1;
}
}
else
{
foreach (var speedControl in _speedControls)
{
if (time >= startTime && time <= startTime + speedControl.TotalTime)
{
currentSpeedControl = speedControl;
break;
}
startTime += speedControl.TotalTime;
index++;
}
}
if (currentSpeedControl == null)
{
return(null);
}
//get positions
var timeSlice = time - startTime;
var pose = CreatePose(timeSlice, currentSpeedControl);
var indexCompleted = GetCompleted(timeSlice, currentSpeedControl.TotalTime);
var completed = GetCompleted(time, _totalTime);
//return
return(new AnimationPose()
{
Index = index, Pose = pose, Completed = completed, IndexCompleted = indexCompleted, IsCompleted = completed == 1
});
}
finally
{
_sync.ExitReadLock();
}
}
MotionPose CreatePose(double time, PoseSpeedControl poseSpeedControl)
{
var result = new MotionPose();
for (int i = 0; i < poseSpeedControl.Count; i++)
{
var speedControl = poseSpeedControl[i];
if (speedControl != null)
{
result[i] = speedControl.GetPosition(time);
}
}
return(result);
}
PoseSpeedControl CreateSpeedControl(MotionPose start, MotionPose end)
{
var result = new PoseSpeedControl();
for (int i = 0; i < start.Count; i++)
{
var startPosition = start[i];
var endPosition = end[i];
if (startPosition != null && endPosition != null)
{
result[i] = new SpeedControl(startPosition.Value, endPosition.Value, ConvertSpeed(end.Acceleration, DefaultAcceleration), ConvertSpeed(end.TopSpeed, DefaultTopSpeed));
}
}
result.TotalTime = result.Servos.Max(i => i != null ? i.TotalTime : 0d);
return(result);
}
