public void InterpolatePositionsLeft(RealTimeInterpolator realTimeInterpolator, double normalizedTime)
{
if (InterpolationType == InterpolationType.Configuration)
{
var interpolatedConfiguration = new Configuration
{
A1 = realTimeInterpolator.GetValue(normalizedTime, startConfiguration.A1, endConfiguration.A1),
A2 = realTimeInterpolator.GetValue(normalizedTime, startConfiguration.A2, endConfiguration.A2),
Q2 = realTimeInterpolator.GetValue(normalizedTime, startConfiguration.Q2, endConfiguration.Q2),
A3 = realTimeInterpolator.GetValue(normalizedTime, startConfiguration.A3, endConfiguration.A3),
A4 = realTimeInterpolator.GetValue(normalizedTime, startConfiguration.A4, endConfiguration.A4),
A5 = realTimeInterpolator.GetValue(normalizedTime, startConfiguration.A5, endConfiguration.A5)
};
getInterpolatedPositions(interpolatedConfiguration);
}
else if (InterpolationType == InterpolationType.InternalPositions)
{
joints[0].Frame.P.Value = realTimeInterpolator.GetVectorValue(normalizedTime, startInternalPositions[0],
endInternalPositions[0]);
joints[1].Frame.P.Value = realTimeInterpolator.GetVectorValue(normalizedTime, startInternalPositions[1],
endInternalPositions[1]);
joints[2].Frame.P.Value = realTimeInterpolator.GetVectorValue(normalizedTime, startInternalPositions[2],
endInternalPositions[2]);
joints[3].Frame.P.Value = realTimeInterpolator.GetVectorValue(normalizedTime, startInternalPositions[3],
endInternalPositions[3]);
joints[4].Frame.P.Value = realTimeInterpolator.GetVectorValue(normalizedTime, startInternalPositions[4],
endInternalPositions[4]);
joints[5].Frame.P.Value = realTimeInterpolator.GetVectorValue(normalizedTime, startInternalPositions[5],
endInternalPositions[5]);
}
UpdatePositionsVisual();
}
private void InitializeScene()
{
linearInterpolator = new LinearInterpolator();
sphericalLinearInterpolator = new SphericalLinearInterpolator();
realTimeInterpolator = new RealTimeInterpolator();
const double maxVal = 8;
var arrowX = new ArrowVisual3D();
arrowX.Direction = new Vector3D(1, 0, 0);
arrowX.Point1 = new Point3D(0, 0, 0);
arrowX.Point2 = new Point3D(maxVal, 0, 0);
arrowX.Diameter = 0.1;
arrowX.Fill = System.Windows.Media.Brushes.Black;
HelixViewportLeft.Children.Add(arrowX);
arrowX = new ArrowVisual3D();
arrowX.Direction = new Vector3D(1, 0, 0);
arrowX.Point1 = new Point3D(0, 0, 0);
arrowX.Point2 = new Point3D(maxVal, 0, 0);
arrowX.Diameter = 0.1;
arrowX.Fill = System.Windows.Media.Brushes.Black;
HelixViewportRight.Children.Add(arrowX);
var arrowMX = new ArrowVisual3D();
arrowMX.Direction = new Vector3D(-1, 0, 0);
arrowMX.Point1 = new Point3D(0, 0, 0);
arrowMX.Point2 = new Point3D(-maxVal, 0, 0);
arrowMX.Diameter = 0.1;
arrowMX.Fill = System.Windows.Media.Brushes.Black;
HelixViewportLeft.Children.Add(arrowMX);
arrowMX = new ArrowVisual3D();
arrowMX.Direction = new Vector3D(-1, 0, 0);
arrowMX.Point1 = new Point3D(0, 0, 0);
arrowMX.Point2 = new Point3D(-maxVal, 0, 0);
arrowMX.Diameter = 0.1;
arrowMX.Fill = System.Windows.Media.Brushes.Black;
HelixViewportRight.Children.Add(arrowMX);
var arrowY = new ArrowVisual3D();
arrowY.Direction = new Vector3D(0, 0, 1);
arrowY.Point1 = new Point3D(0, 0, 0);
arrowY.Point2 = new Point3D(0, 0, maxVal);
arrowY.Diameter = 0.1;
arrowY.Fill = System.Windows.Media.Brushes.Black;
HelixViewportLeft.Children.Add(arrowY);
arrowY = new ArrowVisual3D();
arrowY.Direction = new Vector3D(0, 0, 1);
arrowY.Point1 = new Point3D(0, 0, 0);
arrowY.Point2 = new Point3D(0, 0, maxVal);
arrowY.Diameter = 0.1;
arrowY.Fill = System.Windows.Media.Brushes.Black;
HelixViewportRight.Children.Add(arrowY);
var arrowMY = new ArrowVisual3D();
arrowMY.Direction = new Vector3D(0, 0, -1);
arrowMY.Point1 = new Point3D(0, 0, 0);
arrowMY.Point2 = new Point3D(0, 0, -maxVal);
arrowMY.Diameter = 0.1;
arrowMY.Fill = System.Windows.Media.Brushes.Black;
HelixViewportLeft.Children.Add(arrowMY);
arrowMY = new ArrowVisual3D();
arrowMY.Direction = new Vector3D(0, 0, -1);
arrowMY.Point1 = new Point3D(0, 0, 0);
arrowMY.Point2 = new Point3D(0, 0, -maxVal);
arrowMY.Diameter = 0.1;
arrowMY.Fill = System.Windows.Media.Brushes.Black;
HelixViewportRight.Children.Add(arrowMY);
var arrowZ = new ArrowVisual3D();
arrowZ.Direction = new Vector3D(0, 1, 0);
arrowZ.Point1 = new Point3D(0, 0, 0);
arrowZ.Point2 = new Point3D(0, maxVal, 0);
arrowZ.Diameter = 0.1;
arrowZ.Fill = System.Windows.Media.Brushes.Black;
HelixViewportLeft.Children.Add(arrowZ);
arrowZ = new ArrowVisual3D();
arrowZ.Direction = new Vector3D(0, 1, 0);
arrowZ.Point1 = new Point3D(0, 0, 0);
arrowZ.Point2 = new Point3D(0, maxVal, 0);
arrowZ.Diameter = 0.1;
arrowZ.Fill = System.Windows.Media.Brushes.Black;
HelixViewportRight.Children.Add(arrowZ);
var arrowMZ = new ArrowVisual3D();
arrowMZ.Direction = new Vector3D(0, -1, 0);
arrowMZ.Point1 = new Point3D(0, 0, 0);
arrowMZ.Point2 = new Point3D(0, -maxVal, 0);
arrowMZ.Diameter = 0.1;
arrowMZ.Fill = System.Windows.Media.Brushes.Black;
HelixViewportLeft.Children.Add(arrowMZ);
arrowMZ = new ArrowVisual3D();
arrowMZ.Direction = new Vector3D(0, 0, -1);
arrowMZ.Point1 = new Point3D(0, 0, 0);
arrowMZ.Point2 = new Point3D(0, -maxVal, 0);
arrowMZ.Diameter = 0.1;
arrowMZ.Fill = System.Windows.Media.Brushes.Black;
HelixViewportRight.Children.Add(arrowMZ);
var xArrowText = new TextVisual3D();
xArrowText.Text = "X";
xArrowText.Position = new Point3D(maxVal - 0.5, 0, 0.5);
xArrowText.Height = 0.5;
xArrowText.FontWeight = System.Windows.FontWeights.Bold;
HelixViewportLeft.Children.Add(xArrowText);
xArrowText = new TextVisual3D();
xArrowText.Text = "X";
xArrowText.Position = new Point3D(maxVal - 0.5, 0, 0.5);
xArrowText.Height = 0.5;
xArrowText.FontWeight = System.Windows.FontWeights.Bold;
HelixViewportRight.Children.Add(xArrowText);
var yArrowText = new TextVisual3D();
yArrowText.Text = "Z";
yArrowText.Position = new Point3D(0, 0.5, maxVal - 0.5);
yArrowText.Height = 0.5;
yArrowText.FontWeight = System.Windows.FontWeights.Bold;
HelixViewportLeft.Children.Add(yArrowText);
yArrowText = new TextVisual3D();
yArrowText.Text = "Z";
yArrowText.Position = new Point3D(0, 0.5, maxVal - 0.5);
yArrowText.Height = 0.5;
yArrowText.FontWeight = System.Windows.FontWeights.Bold;
HelixViewportRight.Children.Add(yArrowText);
var zArrowText = new TextVisual3D();
zArrowText.Text = "Y";
zArrowText.Position = new Point3D(0.5, maxVal - 0.5, 0);
zArrowText.Height = 0.5;
zArrowText.FontWeight = System.Windows.FontWeights.Bold;
HelixViewportLeft.Children.Add(zArrowText);
zArrowText = new TextVisual3D();
zArrowText.Text = "Y";
zArrowText.Position = new Point3D(0.5, maxVal - 0.5, 0);
zArrowText.Height = 0.5;
zArrowText.FontWeight = System.Windows.FontWeights.Bold;
HelixViewportRight.Children.Add(zArrowText);
var leftText = new TextVisual3D();
leftText.Text = "Internal angles interpolation";
leftText.Position = new Point3D(0, 0, maxVal + 0.5);
leftText.Height = 1;
leftText.FontWeight = System.Windows.FontWeights.Bold;
HelixViewportLeft.Children.Add(leftText);
var rightText = new TextVisual3D();
rightText.Text = "Inverse chain";
rightText.Position = new Point3D(0, 0, maxVal + 0.5);
rightText.Height = 1;
rightText.FontWeight = System.Windows.FontWeights.Bold;
HelixViewportRight.Children.Add(rightText);
var floor = new RectangleVisual3D
{
Fill = new SolidColorBrush(Color.FromArgb(30, 0, 0, 0)),
Normal = new Vector3D(0, 0, 1),
Origin = new Point3D(0, 0, 0)
};
floor.Width = floor.Length = 10;
floor.LengthDirection = new Vector3D(1, 0, 0);
HelixViewportLeft.Children.Add(floor);
var wall = new RectangleVisual3D
{
Fill = new SolidColorBrush(Color.FromArgb(30, 0, 0, 0)),
Normal = new Vector3D(0, 0, 1),
Origin = new Point3D(0, 0, 0)
};
wall.Width = wall.Length = 10;
wall.LengthDirection = new Vector3D(1, 0, 0);
HelixViewportRight.Children.Add(wall);
frameEuler = new CombinedManipulator()
{
CanRotateX = false,
CanRotateY = false,
CanRotateZ = false
};
frameQuaternion = new CombinedManipulator()
{
CanRotateX = false,
CanRotateY = false,
CanRotateZ = false
};
}
