public void Interpolate(RealTimeInterpolator realTimeInterpolator, double normalizedTime)
{
Configuration interpolatedConfiguration = new Configuration();
interpolatedConfiguration.A1 = realTimeInterpolator.GetValue(normalizedTime, startConfiguration.A1, endConfiguration.A1);
interpolatedConfiguration.A2 = realTimeInterpolator.GetValue(normalizedTime, startConfiguration.A2, endConfiguration.A2);
interpolatedConfiguration.Q2 = realTimeInterpolator.GetValue(normalizedTime, startConfiguration.Q2, endConfiguration.Q2);
interpolatedConfiguration.A3 = realTimeInterpolator.GetValue(normalizedTime, startConfiguration.A3, endConfiguration.A3);
interpolatedConfiguration.A4 = realTimeInterpolator.GetValue(normalizedTime, startConfiguration.A4, endConfiguration.A4);
interpolatedConfiguration.A5 = realTimeInterpolator.GetValue(normalizedTime, startConfiguration.A5, endConfiguration.A5);
GetInterpolatedPositions(interpolatedConfiguration);
UpdatePositions();
}
private void SetupConfiguration(bool forInitialFrame)
{
var eulerTransformGroup = new Transform3DGroup();
Vector3D dir, pos;
dir = GetZ(eulerTransformGroup);
pos = GetPosition(eulerTransformGroup);
RotateInDirection(dir, pos, a1, ref eulerTransformGroup);
dir = GetZ(eulerTransformGroup);
TranslateInDirection(dir, components[1].Length, ref eulerTransformGroup);
dir = GetY(eulerTransformGroup);
pos = GetPosition(eulerTransformGroup);
RotateInDirection(dir, pos, a2, ref eulerTransformGroup);
dir = GetX(eulerTransformGroup);
TranslateInDirection(dir, q2, ref eulerTransformGroup);
dir = GetY(eulerTransformGroup);
pos = GetPosition(eulerTransformGroup);
RotateInDirection(dir, pos, a3, ref eulerTransformGroup);
dir = GetZ(eulerTransformGroup);
TranslateInDirection(dir, -components[3].Length, ref eulerTransformGroup);
dir = GetZ(eulerTransformGroup);
pos = GetPosition(eulerTransformGroup);
RotateInDirection(dir, pos, a4, ref eulerTransformGroup);
dir = GetX(eulerTransformGroup);
TranslateInDirection(dir, components[4].Length, ref eulerTransformGroup);
dir = GetX(eulerTransformGroup);
pos = GetPosition(eulerTransformGroup);
RotateInDirection(dir, pos, a5, ref eulerTransformGroup);
if (forInitialFrame)
{
startConfiguration = new Configuration() {A1 = a1, A2 = a2, Q2 = q2, A3 = a3, A4 = a4, A5 = a5};
startInternalPositions = new Vector3D[6]
{
joints[0].Frame.P.Value, joints[1].Frame.P.Value, joints[2].Frame.P.Value, joints[3].Frame.P.Value,
joints[4].Frame.P.Value, joints[5].Frame.P.Value
};
startFrame.Transform = eulerTransformGroup;
}
else
{
endConfiguration = new Configuration() {A1 = a1, A2 = a2, Q2 = q2, A3 = a3, A4 = a4, A5 = a5};
endInternalPositions = new Vector3D[6]
{
joints[0].Frame.P.Value, joints[1].Frame.P.Value, joints[2].Frame.P.Value, joints[3].Frame.P.Value,
joints[4].Frame.P.Value, joints[5].Frame.P.Value
};
endFrame.Transform = eulerTransformGroup;
}
}
private void GetInterpolatedPositions(Configuration interpolatedConfiguration)
{
var eulerTransformGroup = new Transform3DGroup();
Position p0, p1, p2, p3, p4, p5;
p0 = new Position(0,0,0);
p1 = new Position(p0);
Vector3D dir, pos;
dir = GetZ(eulerTransformGroup);
pos = GetPosition(eulerTransformGroup);
RotateInDirection(dir, pos, interpolatedConfiguration.A1, ref eulerTransformGroup);
dir = GetZ(eulerTransformGroup);
TranslateInDirection(dir, components[1].Length, ref eulerTransformGroup);
p2 = new Position(eulerTransformGroup.Value.OffsetX, eulerTransformGroup.Value.OffsetY, eulerTransformGroup.Value.OffsetZ);
dir = GetY(eulerTransformGroup);
pos = GetPosition(eulerTransformGroup);
RotateInDirection(dir, pos, interpolatedConfiguration.A2, ref eulerTransformGroup);
dir = GetX(eulerTransformGroup);
TranslateInDirection(dir, interpolatedConfiguration.Q2, ref eulerTransformGroup);
p3 = new Position(eulerTransformGroup.Value.OffsetX, eulerTransformGroup.Value.OffsetY, eulerTransformGroup.Value.OffsetZ);
dir = GetY(eulerTransformGroup);
pos = GetPosition(eulerTransformGroup);
RotateInDirection(dir, pos, interpolatedConfiguration.A3, ref eulerTransformGroup);
dir = GetZ(eulerTransformGroup);
TranslateInDirection(dir, -components[3].Length, ref eulerTransformGroup);
p4 = new Position(eulerTransformGroup.Value.OffsetX, eulerTransformGroup.Value.OffsetY, eulerTransformGroup.Value.OffsetZ);
dir = GetZ(eulerTransformGroup);
pos = GetPosition(eulerTransformGroup);
RotateInDirection(dir, pos, interpolatedConfiguration.A4, ref eulerTransformGroup);
dir = GetX(eulerTransformGroup);
TranslateInDirection(dir, components[4].Length, ref eulerTransformGroup);
p5 = new Position(eulerTransformGroup.Value.OffsetX, eulerTransformGroup.Value.OffsetY, eulerTransformGroup.Value.OffsetZ);
dir = GetX(eulerTransformGroup);
pos = GetPosition(eulerTransformGroup);
RotateInDirection(dir, pos, interpolatedConfiguration.A5, ref eulerTransformGroup);
joints[0].Frame.P = p0;
joints[1].Frame.P = p1;
joints[2].Frame.P = p2;
joints[3].Frame.P = p3;
joints[4].Frame.P = p4;
joints[5].Frame.P = p5;
currentFrame.Transform = eulerTransformGroup;
}