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