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();
}
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 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, 1, 0);
arrowY.Point1 = new Point3D(0, 0, 0);
arrowY.Point2 = new Point3D(0, maxVal, 0);
arrowY.Diameter = 0.1;
arrowY.Fill = System.Windows.Media.Brushes.Black;
HelixViewportLeft.Children.Add(arrowY);
arrowY = new ArrowVisual3D();
arrowY.Direction = new Vector3D(0, 1, 0);
arrowY.Point1 = new Point3D(0, 0, 0);
arrowY.Point2 = new Point3D(0, maxVal, 0);
arrowY.Diameter = 0.1;
arrowY.Fill = System.Windows.Media.Brushes.Black;
HelixViewportRight.Children.Add(arrowY);
var arrowMY = new ArrowVisual3D();
arrowMY.Direction = new Vector3D(0, -1, 0);
arrowMY.Point1 = new Point3D(0, 0, 0);
arrowMY.Point2 = new Point3D(0, -maxVal, 0);
arrowMY.Diameter = 0.1;
arrowMY.Fill = System.Windows.Media.Brushes.Black;
HelixViewportLeft.Children.Add(arrowMY);
arrowMY = new ArrowVisual3D();
arrowMY.Direction = new Vector3D(0, -1, 0);
arrowMY.Point1 = new Point3D(0, 0, 0);
arrowMY.Point2 = new Point3D(0, -maxVal, 0);
arrowMY.Diameter = 0.1;
arrowMY.Fill = System.Windows.Media.Brushes.Black;
HelixViewportRight.Children.Add(arrowMY);
var arrowZ = new ArrowVisual3D();
arrowZ.Direction = new Vector3D(0, 0, 1);
arrowZ.Point1 = new Point3D(0, 0, 0);
arrowZ.Point2 = new Point3D(0, 0, maxVal);
arrowZ.Diameter = 0.1;
arrowZ.Fill = System.Windows.Media.Brushes.Black;
HelixViewportLeft.Children.Add(arrowZ);
arrowZ = new ArrowVisual3D();
arrowZ.Direction = new Vector3D(0, 0, 1);
arrowZ.Point1 = new Point3D(0, 0, 0);
arrowZ.Point2 = new Point3D(0, 0, maxVal);
arrowZ.Diameter = 0.1;
arrowZ.Fill = System.Windows.Media.Brushes.Black;
HelixViewportRight.Children.Add(arrowZ);
var arrowMZ = new ArrowVisual3D();
arrowMZ.Direction = new Vector3D(0, 0, -1);
arrowMZ.Point1 = new Point3D(0, 0, 0);
arrowMZ.Point2 = new Point3D(0, 0, -maxVal);
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, 0, -maxVal);
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 = "Y";
yArrowText.Position = new Point3D(0, maxVal - 0.5, 0.5);
yArrowText.Height = 0.5;
yArrowText.FontWeight = System.Windows.FontWeights.Bold;
HelixViewportLeft.Children.Add(yArrowText);
yArrowText = new TextVisual3D();
yArrowText.Text = "Y";
yArrowText.Position = new Point3D(0, maxVal - 0.5, 0.5);
yArrowText.Height = 0.5;
yArrowText.FontWeight = System.Windows.FontWeights.Bold;
HelixViewportRight.Children.Add(yArrowText);
var zArrowText = new TextVisual3D();
zArrowText.Text = "Z";
zArrowText.Position = new Point3D(0.5, 0, maxVal - 0.5);
zArrowText.Height = 0.5;
zArrowText.FontWeight = System.Windows.FontWeights.Bold;
HelixViewportLeft.Children.Add(zArrowText);
zArrowText = new TextVisual3D();
zArrowText.Text = "Z";
zArrowText.Position = new Point3D(0.5, 0, maxVal - 0.5);
zArrowText.Height = 0.5;
zArrowText.FontWeight = System.Windows.FontWeights.Bold;
HelixViewportRight.Children.Add(zArrowText);
var leftText = new TextVisual3D();
leftText.Text = "Internal Coordinates 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 = "Effector Position Interpolation";
rightText.Position = new Point3D(0, 0, maxVal + 0.5);
rightText.Height = 1;
rightText.FontWeight = System.Windows.FontWeights.Bold;
HelixViewportRight.Children.Add(rightText);
SetupStartConfiguration();
SetupEndConfiguration();
frameEuler = new CombinedManipulator()
{
CanRotateX = false,
CanRotateY = false,
CanRotateZ = false
};
robotForInternalCoordinates.SetMidFrameModel(frameEuler);
frameQuaternion = new CombinedManipulator()
{
CanRotateX = false,
CanRotateY = false,
CanRotateZ = false
};
}
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, 1, 0);
arrowY.Point1 = new Point3D(0, 0, 0);
arrowY.Point2 = new Point3D(0, maxVal, 0);
arrowY.Diameter = 0.1;
arrowY.Fill = System.Windows.Media.Brushes.Black;
HelixViewportLeft.Children.Add(arrowY);
arrowY = new ArrowVisual3D();
arrowY.Direction = new Vector3D(0, 1, 0);
arrowY.Point1 = new Point3D(0, 0, 0);
arrowY.Point2 = new Point3D(0, maxVal, 0);
arrowY.Diameter = 0.1;
arrowY.Fill = System.Windows.Media.Brushes.Black;
HelixViewportRight.Children.Add(arrowY);
var arrowMY = new ArrowVisual3D();
arrowMY.Direction = new Vector3D(0, -1, 0);
arrowMY.Point1 = new Point3D(0, 0, 0);
arrowMY.Point2 = new Point3D(0, -maxVal, 0);
arrowMY.Diameter = 0.1;
arrowMY.Fill = System.Windows.Media.Brushes.Black;
HelixViewportLeft.Children.Add(arrowMY);
arrowMY = new ArrowVisual3D();
arrowMY.Direction = new Vector3D(0, -1, 0);
arrowMY.Point1 = new Point3D(0, 0, 0);
arrowMY.Point2 = new Point3D(0, -maxVal, 0);
arrowMY.Diameter = 0.1;
arrowMY.Fill = System.Windows.Media.Brushes.Black;
HelixViewportRight.Children.Add(arrowMY);
var arrowZ = new ArrowVisual3D();
arrowZ.Direction = new Vector3D(0, 0, 1);
arrowZ.Point1 = new Point3D(0, 0, 0);
arrowZ.Point2 = new Point3D(0, 0, maxVal);
arrowZ.Diameter = 0.1;
arrowZ.Fill = System.Windows.Media.Brushes.Black;
HelixViewportLeft.Children.Add(arrowZ);
arrowZ = new ArrowVisual3D();
arrowZ.Direction = new Vector3D(0, 0, 1);
arrowZ.Point1 = new Point3D(0, 0, 0);
arrowZ.Point2 = new Point3D(0, 0, maxVal);
arrowZ.Diameter = 0.1;
arrowZ.Fill = System.Windows.Media.Brushes.Black;
HelixViewportRight.Children.Add(arrowZ);
var arrowMZ = new ArrowVisual3D();
arrowMZ.Direction = new Vector3D(0, 0, -1);
arrowMZ.Point1 = new Point3D(0, 0, 0);
arrowMZ.Point2 = new Point3D(0, 0, -maxVal);
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, 0, -maxVal);
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 = "Y";
yArrowText.Position = new Point3D(0, maxVal - 0.5, 0.5);
yArrowText.Height = 0.5;
yArrowText.FontWeight = System.Windows.FontWeights.Bold;
HelixViewportLeft.Children.Add(yArrowText);
yArrowText = new TextVisual3D();
yArrowText.Text = "Y";
yArrowText.Position = new Point3D(0, maxVal - 0.5, 0.5);
yArrowText.Height = 0.5;
yArrowText.FontWeight = System.Windows.FontWeights.Bold;
HelixViewportRight.Children.Add(yArrowText);
var zArrowText = new TextVisual3D();
zArrowText.Text = "Z";
zArrowText.Position = new Point3D(0.5, 0, maxVal - 0.5);
zArrowText.Height = 0.5;
zArrowText.FontWeight = System.Windows.FontWeights.Bold;
HelixViewportLeft.Children.Add(zArrowText);
zArrowText = new TextVisual3D();
zArrowText.Text = "Z";
zArrowText.Position = new Point3D(0.5, 0, maxVal - 0.5);
zArrowText.Height = 0.5;
zArrowText.FontWeight = System.Windows.FontWeights.Bold;
HelixViewportRight.Children.Add(zArrowText);
var leftText = new TextVisual3D();
leftText.Text = "Internal Coordinates 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 = "Effector Position Interpolation";
rightText.Position = new Point3D(0, 0, maxVal + 0.5);
rightText.Height = 1;
rightText.FontWeight = System.Windows.FontWeights.Bold;
HelixViewportRight.Children.Add(rightText);
SetupStartConfiguration();
SetupEndConfiguration();
frameEuler = new CombinedManipulator()
{
CanRotateX = false,
CanRotateY = false,
CanRotateZ = false
};
robotForInternalCoordinates.SetMidFrameModel(frameEuler);
frameQuaternion = new CombinedManipulator()
{
CanRotateX = false,
CanRotateY = false,
CanRotateZ = false
};
}
