public Pose3D GetAnimatedPose(double time)
{
//var start = Keyframes.Last(kv => kv.Key <= time);
//var end = Keyframes.First(kv => kv.Key >= time);
var _finder = new Pose3D()
{
Time = time
};
var start = Keyframes.WeakPredecessor(_finder);
var end = Keyframes.WeakSuccessor(_finder);
if (start.CompareTo(end) == 0)
{
return(start);
}
return(Pose3D.Interpolate(start, end, time));
}
public void SetKeyframe(double time)
{
var _find = new Pose3D()
{
Time = time
};
if (this.Keyframes.Contains(_find))
{
this.Keyframes.Remove(_find);
}
this.Keyframes.Add(new Pose3D()
{
Orientation = this.Orientation,
Position = this.Position,
Time = time
});
}
public static Pose3D Interpolate(Pose3D start, Pose3D end, double time)
{
var percentage = (time - start.Time) / (end.Time - start.Time);
return(new Pose3D()
{
Position = new Point3D()
{
X = start.Position.X + percentage * (end.Position.X - start.Position.X),
Y = start.Position.Y + percentage * (end.Position.Y - start.Position.Y),
Z = start.Position.Z + percentage * (end.Position.Z - start.Position.Z)
},
Orientation = new AxisAngle3D()
{
X = start.Orientation.X + percentage * (end.Orientation.X - start.Orientation.X),
Y = start.Orientation.Y + percentage * (end.Orientation.Y - start.Orientation.Y),
Z = start.Orientation.Z + percentage * (end.Orientation.Z - start.Orientation.Z),
W = start.Orientation.W + percentage * (end.Orientation.W - start.Orientation.W)
},
Time = time
});
}