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); }