private void MainForm_Load(object sender, EventArgs e)
{
// Ok, let's start to build our virtual robot arm !
model = new DenavitHartenbergModel(new Vector3(0, 0, 0));
// Add the first joint
model.Joints.Add(alpha: 0, theta: Math.PI / 4, radius: 35, offset: 0);
// Add the second joint
model.Joints.Add(alpha: 0, theta: -Math.PI / 3, radius: 35, offset: 0);
// Create the top finger
model_tgripper = new DenavitHartenbergModel();
model_tgripper.Joints.Add(alpha: 0, theta: Math.PI / 4, radius: 20, offset: 0);
model_tgripper.Joints.Add(alpha: 0, theta: -Math.PI / 3, radius: 20, offset: 0);
// Create the bottom finger
model_bgripper = new DenavitHartenbergModel();
model_bgripper.Joints.Add(alpha: 0, theta: -Math.PI / 4, radius: 20, offset: 0);
model_bgripper.Joints.Add(alpha: 0, theta: Math.PI / 3, radius: 20, offset: 0);
// Create the model combinator from the parent model
arm = new DenavitHartenbergNode(model);
// Add the top finger
arm.Children.Add(model_tgripper);
// Add the bottom finger
arm.Children.Add(model_bgripper);
// Calculate the whole model (parent model + children models)
arm.Compute();
// Create the model visualizer
viewer = new DenavitHartenbergViewer(pictureBox1.Width, pictureBox1.Height);
// Assign each projection image of the model to a picture box
pictureBox1.Image = viewer.PlaneXY;
pictureBox2.Image = viewer.PlaneXZ;
pictureBox3.Image = viewer.PlaneYZ;
// Start the animation
timer1.Interval = 40;
timer1.Enabled = true;
}
/// <summary>
/// Computes the three images of a list of models
/// </summary>
///
/// <param name="model">List of arguments of models</param>
///
public void ComputeImages(params DenavitHartenbergModel[] model)
{
// Clear the 3 images with the background color
gxy.Clear(BackColor);
gxz.Clear(BackColor);
gyz.Clear(BackColor);
// Draw each model
for (int i = 0; i < model.Length; i++)
{
DenavitHartenbergModel mdl = model[i];
Pen pLinks = new Pen(LinksColor, 1);
// Draw each link of the model
Vector3 previous = mdl.Position;
for (int j = 0; j < mdl.Joints.Count; j++)
{
Vector3 current = mdl.Joints[j].Position;
// XY
gxy.DrawLine(pLinks,
(xy.Width / 2) + previous.X * Scale,
(xy.Height / 2) - previous.Y * Scale,
(xy.Width / 2) + current.X * Scale,
(xy.Height / 2) - current.Y * Scale);
// ZY
gyz.DrawLine(pLinks,
(yz.Width / 2) + previous.Z * Scale,
(yz.Height / 2) - previous.Y * Scale,
(yz.Width / 2) + current.Z * Scale,
(yz.Height / 2) - current.Y * Scale);
// XZ
gxz.DrawLine(pLinks,
(xz.Width / 2) + previous.X * Scale,
(xz.Height / 2) - previous.Z * Scale,
(xz.Width / 2) + current.X * Scale,
(xz.Height / 2) - current.Z * Scale);
previous = current;
}
Brush pJoints = new SolidBrush(JointsColor);
// Draw each joint
for (int j = 0; j < mdl.Joints.Count + 1; j++)
{
Vector3 current;
// Select the color of the joint
if (j == 0)
{
pJoints = new SolidBrush(BaseJointColor);
current = mdl.Position;
}
else
{
current = mdl.Joints[j - 1].Position;
if (j == mdl.Joints.Count)
{
pJoints = new SolidBrush(EndJointColor);
}
else
{
pJoints = new SolidBrush(JointsColor);
}
}
// XY
gxy.FillEllipse(pJoints,
(xy.Width / 2) + current.X * Scale - (JointRadius / 2),
(xy.Height / 2) - current.Y * Scale - (JointRadius / 2),
JointRadius, JointRadius);
// YZ
gyz.FillEllipse(pJoints,
(yz.Width / 2) + current.Z * Scale - (JointRadius / 2),
(yz.Height / 2) - current.Y * Scale - (JointRadius / 2),
JointRadius, JointRadius);
// XZ
gxz.FillEllipse(pJoints,
(xz.Width / 2) + current.X * Scale - (JointRadius / 2),
(xz.Height / 2) - current.Z * Scale - (JointRadius / 2),
JointRadius, JointRadius);
}
}
// Draw the arrows on the windows
DrawArrows(ref gxy, "Y", "X");
DrawArrows(ref gyz, "Y", "Z");
DrawArrows(ref gxz, "Z", "X");
}
