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