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 }; }
/// <summary> /// Create the East and Radius label. /// This will display the text for "East" and the size /// of the cylinder radius. /// </summary> /// <param name="numBins">Number of bins in the ensemble.</param> /// <returns>Model of the label.</returns> private Model3D EastArrow(int numBins) { Model3DGroup group = new Model3DGroup(); // Get the length of the cylinder, if it is 0, then display something. double arrowLength = CylinderRadius * SCALE_ARROW; if (arrowLength <= 0) { arrowLength = SCALE_ARROW; } #region Arrow //Create the shape of the object //This will be an arrow //Length will be the scale value //The position will be above the plot double xAxisLoc = 0; double yAxisLoc = 0; double zAxisLoc = 0; if (YAxis) { yAxisLoc = -numBins * TRANSLATION_SCALE; // Go to the bottom of the column } else { xAxisLoc = numBins * TRANSLATION_SCALE; } var mb = new MeshBuilder(false, false); mb.AddArrow(new Point3D(xAxisLoc, yAxisLoc, zAxisLoc), new Point3D(xAxisLoc, yAxisLoc, arrowLength), LABEL_ARROW_HEAD_SIZE, 3, 100); Geometry3D geometry = mb.ToMesh(); //Set the color Material material = MaterialHelper.CreateMaterial(Colors.Lime); material.Freeze(); ////Rotate the object //var rotation = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(xAxisLoc, yAxisLoc, zAxisLoc), 90)); //rotation.Freeze(); //var tg = new Transform3DGroup(); //tg.Children.Add(rotation); var model = new GeometryModel3D(geometry, material) {}; model.BackMaterial = material; #endregion #region Text double xAxisLocLabel = 0; double yAxisLocLabel = 0; double zAxisLocLabel = 0; if (YAxis) { xAxisLocLabel = -LABEL_ARROW_HEAD_SIZE; // Make the text in the middle of the East arrow yAxisLocLabel = (-numBins * TRANSLATION_SCALE) - (ARROW_HEAD_SIZE * 2); // Go to the bottom of the column zAxisLocLabel = (arrowLength / 2); // Make the text just in front of the East arrow } else { xAxisLocLabel = (numBins * TRANSLATION_SCALE) + (ARROW_HEAD_SIZE * 2); // Go to the bottom of the column yAxisLocLabel = -(ARROW_HEAD_SIZE * 2); // Make the text just in front of the East arrow zAxisLocLabel = (arrowLength / 2); // Make the text in the middle of the East arrow } TextVisual3D txt = new TextVisual3D(); txt.Position = new Point3D(xAxisLocLabel, yAxisLocLabel, zAxisLocLabel); txt.Height = 0.5; txt.Text = string.Format("East {0}", (arrowLength / SCALE_ARROW).ToString("0.0")); txt.TextDirection = new Vector3D(0, 0, 1); // Set text to run in line with X axis txt.UpDirection = new Vector3D(0, 1, 0); // Set text to Point Up on Y axis txt.Foreground = new SolidColorBrush(Colors.Black); txt.Background = new SolidColorBrush(Colors.WhiteSmoke); txt.Padding = new Thickness(2); #endregion group.Children.Add(model); group.Children.Add(txt.Content); return group; }
/// <summary> /// Create the text for the north arrow. /// /// It is suggested to use an image instead /// of text for performance purposes. I will /// look into this later. /// </summary> /// <returns>3D Text.</returns> private Model3D NorthArrow(int numBins) { Model3DGroup group = new Model3DGroup(); // Get the length of the cylinder, if it is 0, then display something. double arrowLength = CylinderRadius * SCALE_ARROW; if (arrowLength <= 0) { arrowLength = SCALE_ARROW; } #region Arrow //Create the shape of the object //This will be an arrow //Length will be the scale value //The position will be above the plot double xAxisLoc = 0; double yAxisLoc = 0; double zAxisLoc = 0; if (YAxis) { yAxisLoc = -numBins * TRANSLATION_SCALE; } else { xAxisLoc = numBins * TRANSLATION_SCALE; } var mb = new MeshBuilder(false, false); if (YAxis) { mb.AddArrow(new Point3D(xAxisLoc, yAxisLoc, zAxisLoc), new Point3D(arrowLength, yAxisLoc, zAxisLoc), LABEL_ARROW_HEAD_SIZE, 3, 100); } else { mb.AddArrow(new Point3D(xAxisLoc, yAxisLoc, zAxisLoc), new Point3D(xAxisLoc, arrowLength, zAxisLoc), LABEL_ARROW_HEAD_SIZE, 3, 100); } Geometry3D geometry = mb.ToMesh(); //Set the color Material material = MaterialHelper.CreateMaterial(SELECTED_BIN_COLOR); material.Freeze(); var model = new GeometryModel3D(geometry, material); model.BackMaterial = material; #endregion #region Text double xAxisLocLabel = 0; double yAxisLocLabel = 0; double zAxisLocLabel = LABEL_ARROW_HEAD_SIZE; // Make the text just in front of the North arrow if (YAxis) { xAxisLocLabel = arrowLength / 2; // Make the text in the middle of the North arrow yAxisLocLabel = -numBins * TRANSLATION_SCALE; // Go to the bottom of the column } else { xAxisLocLabel = numBins * TRANSLATION_SCALE; yAxisLocLabel = arrowLength / 2; } TextVisual3D txt = new TextVisual3D(); txt.Position = new Point3D(xAxisLocLabel, yAxisLocLabel, zAxisLocLabel); txt.Height = 0.5; txt.Text = string.Format("North {0}", (arrowLength / SCALE_ARROW).ToString("0.0")); // Need to get the arrowLenght back to m/s txt.TextDirection = new Vector3D(1, 0, 0); // Set text to run in line with X axis txt.UpDirection = new Vector3D(0, 1, 0); // Set text to Point Up on Y axis txt.Foreground = new SolidColorBrush(Colors.Black); txt.Background = new SolidColorBrush(Colors.WhiteSmoke); txt.Padding = new Thickness(2); #endregion group.Children.Add(model); group.Children.Add(txt.Content); return group; }
/// <summary> /// 3D scene initialization /// </summary> private void InitializeScene() { 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; HelixViewport.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; HelixViewport.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; HelixViewport.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; HelixViewport.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; HelixViewport.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; HelixViewport.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; HelixViewport.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; HelixViewport.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; HelixViewport.Children.Add(zArrowText); gravityPlane = new RectangleVisual3D(); gravityPlane.Width = 10; gravityPlane.Length = 10; var brush = new SolidColorBrush(Colors.Green); brush.Opacity = 0.5; gravityPlane.Fill = brush; HelixViewport.Children.Add(gravityPlane); gravityArrow = new ArrowVisual3D(); gravityArrow.Direction = new Vector3D(0, 0, -1); gravityArrow.Point1 = new Point3D(0, 0, 0); gravityArrow.Point2 = new Point3D(0, 0, -3); gravityArrow.Diameter = 0.2; gravityArrow.Fill = Brushes.Green; HelixViewport.Children.Add(gravityArrow); diagonalArrow = new ArrowVisual3D(); diagonalArrow.Direction = new Vector3D(1, 1, 1); diagonalArrow.Point1 = new Point3D(0, 0, 0); diagonalArrow.Point2 = new Point3D(cubeSize, cubeSize, cubeSize); diagonalArrow.Diameter = 0.1; diagonalArrow.Fill = Brushes.Blue; HelixViewport.Children.Add(diagonalArrow); SetupCube(); }