/// <summary>
/// Get the Euler angles of a link
/// </summary>
/// <param name="linkindex"></param>
/// <returns>Returns Euler angles as: Pitch, Yaw, Roll</returns>
public EulerParams linkAngles(int linkindex)
{
int i, j, rowsc = 0, colsc = 0;
double[] angles = new double[3];
double[][] R = new double[4][];
for (i = 0; i < 4; i++)
{
R[i] = new double[4];
for (j = 0; j < 4; j++)
{
R[i][j] = links[linkindex].T[i][j];
}
}
for (i = linkindex - 1; i > 0; i--)
{
R = ArmLink.mulMatrices(links[i].T, R, 4, 4, 4, ref rowsc, ref colsc);
}
EulerParams p = new EulerParams();
p.Yaw = Math.Atan2(-R[2][0], Math.Sqrt(Math.Pow(R[0][0], 2) + Math.Pow(R[1][0], 2))); // yaw (about y)
p.Pitch = Math.Atan2(R[1][0] / Math.Cos(angles[1]), R[0][0] / Math.Cos(angles[1])); // pitch (about x)
p.Roll = Math.Atan2(R[2][1] / Math.Cos(angles[1]), R[2][2] / Math.Cos(angles[1])); // roll (about z)
return(p);
}
public void drawArmcyl(int xdim, int ydim, Graphics gr)
{
int rowsc = 0, colsc = 0;
double H = 0;
double R = 1;
// drawing the effector now
Effector effector = new Effector(3, 1.5, 2, 10, -3, 0);
effector.opening = effectorwidth;
effector.T = ArmLink.mulMatrices(links[5].T, links[6].T, 4, 4, 4, ref rowsc, ref colsc);
effector.T = ArmLink.mulMatrices(links[4].T, effector.T, 4, 4, 4, ref rowsc, ref colsc);
effector.T = ArmLink.mulMatrices(links[3].T, effector.T, 4, 4, 4, ref rowsc, ref colsc);
effector.T = ArmLink.mulMatrices(links[2].T, effector.T, 4, 4, 4, ref rowsc, ref colsc);
effector.T = ArmLink.mulMatrices(links[1].T, effector.T, 4, 4, 4, ref rowsc, ref colsc);
effector.drawEffector(gr, xdim, ydim);
// link #1 cylinder
Cylinder cylinder1 = new Cylinder(4.5, 4.5, 10, -3, 0, Color.Blue, Cylinder.ALONGY);
cylinder1.T = links[0].T;
cylinder1.drawCylinder(gr, xdim, ydim);
// link #4 cylinder
Cylinder cylinder4 = new Cylinder(R, 4.5, 10, -3, 0, Color.Peru, Cylinder.ALONGX);
cylinder4.T = ArmLink.mulMatrices(links[3].T, links[4].T, 4, 4, 4, ref rowsc, ref colsc);
cylinder4.T = ArmLink.mulMatrices(links[2].T, cylinder4.T, 4, 4, 4, ref rowsc, ref colsc);
cylinder4.T = ArmLink.mulMatrices(links[1].T, cylinder4.T, 4, 4, 4, ref rowsc, ref colsc);
cylinder4.drawCylinder(gr, xdim, ydim);
// link #3 cylinder
Cylinder cylinder3 = new Cylinder(R, 16.5, 10, -3, 0, Color.Green, Cylinder.ALONGX);
cylinder3.T = ArmLink.mulMatrices(links[2].T, links[3].T, 4, 4, 4, ref rowsc, ref colsc);
cylinder3.T = ArmLink.mulMatrices(links[1].T, cylinder3.T, 4, 4, 4, ref rowsc, ref colsc);
cylinder3.drawCylinder(gr, xdim, ydim);
// link#2 cylinder
Cylinder cylinder2 = new Cylinder(R, 13.5, 10, -3, 0, Color.Red, Cylinder.ALONGX);
cylinder2.T = ArmLink.mulMatrices(links[1].T, links[2].T, 4, 4, 4, ref rowsc, ref colsc);
cylinder2.drawCylinder(gr, xdim, ydim);
}
public double[][] linkPosition(int linkindex)
{
int i, rows = 0, cols = 0;
double[][] zeropoint;
zeropoint = new double[4][];
for (i = 0; i < 4; i++)
{
zeropoint[i] = new double[1];
}
zeropoint[0][0] = 0;
zeropoint[1][0] = 0;
zeropoint[2][0] = 0;
zeropoint[3][0] = 1;
zeropoint = ArmLink.mulMatrices(links[linkindex].T, zeropoint, 4, 4, 1, ref rows, ref cols);
for (i = linkindex - 1; i > 0; i--)
{
zeropoint = ArmLink.mulMatrices(links[i].T, zeropoint, 4, 4, 1, ref rows, ref cols);
}
return(zeropoint);
}
public void drawEffector(Graphics gr, int panelxdim, int panelydim)
{
double x, y, z;
System.Drawing.Pen pen1 = new Pen(Color.Aqua);
System.Drawing.Pen pen2 = new Pen(Color.Brown);
double[][] point1 = new double[4][];
double[][] point2 = new double[4][];
double[][] point3 = new double[4][];
double[][] point4 = new double[4][];
int i;
for (i = 0; i < 4; i++)
{
point1[i] = new double[1];
point2[i] = new double[1];
point3[i] = new double[1];
point4[i] = new double[1];
}
for (y = -height / 2; y <= height / 2; y += 0.05)
{
point1[0][0] = 0;
point1[1][0] = y;
point1[2][0] = opening * width / 200;
point1[3][0] = 1;
point2[0][0] = fingerlen;
point2[1][0] = y;
point2[2][0] = opening * width / 200;
point2[3][0] = 1;
double x1, y1, z1;
double x2, y2, z2;
point3[0][0] = 0;
point3[1][0] = y;
point3[2][0] = -opening * width / 200;
point3[3][0] = 1;
point4[0][0] = fingerlen;
point4[1][0] = y;
point4[2][0] = -opening * width / 200;
point4[3][0] = 1;
double x3 = 0, y3, z3;
double x4, y4, z4;
// transforming points using transformation matrix T
int rows = 0, cols = 0;
point1 = ArmLink.mulMatrices(T, point1, 4, 4, 1, ref rows, ref cols);
point2 = ArmLink.mulMatrices(T, point2, 4, 4, 1, ref rows, ref cols);
point3 = ArmLink.mulMatrices(T, point3, 4, 4, 1, ref rows, ref cols);
point4 = ArmLink.mulMatrices(T, point4, 4, 4, 1, ref rows, ref cols);
x1 = point1[0][0];
y1 = point1[1][0];
z1 = point1[2][0];
x2 = point2[0][0];
y2 = point2[1][0];
z2 = point2[2][0];
x3 = point3[0][0];
y3 = point3[1][0];
z3 = point3[2][0];
x4 = point4[0][0];
y4 = point4[1][0];
z4 = point4[2][0];
// converting coordinates to 2D
double Sx1 = ((x1 - eyeofsX) * eyedistance / (z1 + eyedistance));
double Sy1 = ((y1 - eyeofsY) * eyedistance / (z1 + eyedistance));
double Sx2 = ((x2 - eyeofsX) * eyedistance / (z2 + eyedistance));
double Sy2 = ((y2 - eyeofsY) * eyedistance / (z2 + eyedistance));
double Sx3 = ((x3 - eyeofsX) * eyedistance / (z3 + eyedistance));
double Sy3 = ((y3 - eyeofsY) * eyedistance / (z3 + eyedistance));
double Sx4 = ((x4 - eyeofsX) * eyedistance / (z4 + eyedistance));
double Sy4 = ((y4 - eyeofsY) * eyedistance / (z4 + eyedistance));
double pixpercmX = panelxdim / 35;
double pixpercmY = panelydim / 35;
// drawing lines
gr.DrawLine(pen2, (float)(Sx1 * pixpercmX), (float)(panelydim - Sy1 * pixpercmY), (float)(Sx2 * pixpercmX), (float)(panelydim - Sy2 * pixpercmY));
gr.DrawLine(pen2, (float)(Sx3 * pixpercmX), (float)(panelydim - Sy3 * pixpercmY), (float)(Sx4 * pixpercmX), (float)(panelydim - Sy4 * pixpercmY));
}
for (y = -height / 2; y <= height / 2; y += 0.05)
{
point1[0][0] = 0;
point1[1][0] = y;
point1[2][0] = -width / 2;
point1[3][0] = 1;
point2[0][0] = 0;
point2[1][0] = y;
point2[2][0] = width / 2;
point2[3][0] = 1;
double x1, y1, z1;
double x2, y2, z2;
// transforming points using transformation matrix T
int rows = 0, cols = 0;
point1 = ArmLink.mulMatrices(T, point1, 4, 4, 1, ref rows, ref cols);
point2 = ArmLink.mulMatrices(T, point2, 4, 4, 1, ref rows, ref cols);
x1 = point1[0][0];
y1 = point1[1][0];
z1 = point1[2][0];
x2 = point2[0][0];
y2 = point2[1][0];
z2 = point2[2][0];
// converting coordinates to 2D
double Sx1 = ((x1 - eyeofsX) * eyedistance / (z1 + eyedistance));
double Sy1 = ((y1 - eyeofsY) * eyedistance / (z1 + eyedistance));
double Sx2 = ((x2 - eyeofsX) * eyedistance / (z2 + eyedistance));
double Sy2 = ((y2 - eyeofsY) * eyedistance / (z2 + eyedistance));
double pixpercmX = panelxdim / 35;
double pixpercmY = panelydim / 35;
// drawing lines
gr.DrawLine(pen1, (float)(Sx1 * pixpercmX), (float)(panelydim - Sy1 * pixpercmY), (float)(Sx2 * pixpercmX), (float)(panelydim - Sy2 * pixpercmY));
}
pen1.Dispose();
pen2.Dispose();
}
public void drawCylinder(Graphics gr, int panelxdim, int panelydim)
{
int i;
double[][] basecenter = new double[4][];
for (i = 0; i < 4; i++)
{
basecenter[i] = new double[1];
}
double[][] peekcenter = new double[4][];
for (i = 0; i < 4; i++)
{
peekcenter[i] = new double[1];
}
double[][] baseperim = new double[4][];
for (i = 0; i < 4; i++)
{
baseperim[i] = new double[1];
}
double[][] peekperim = new double[4][];
for (i = 0; i < 4; i++)
{
peekperim[i] = new double[1];
}
// constructing base center point
basecenter[0][0] = 0;
basecenter[1][0] = 0;
basecenter[2][0] = 0;
basecenter[3][0] = 1;
switch (direction)
{
case ALONGX:
// constructing peek center point
peekcenter[0][0] = H;
peekcenter[1][0] = 0;
peekcenter[2][0] = 0;
peekcenter[3][0] = 1;
break;
case ALONGY:
// constructing peek center point
peekcenter[0][0] = 0;
peekcenter[1][0] = H;
peekcenter[2][0] = 0;
peekcenter[3][0] = 1;
break;
case ALONGZ:
// constructing peek center point
peekcenter[0][0] = 0;
peekcenter[1][0] = H;
peekcenter[2][0] = 0;
peekcenter[3][0] = 1;
break;
}
for (i = 0; i < 360; i++)
{
switch (direction)
{
case ALONGX:
// base circumference point
baseperim[0][0] = 0;
baseperim[1][0] = R * Math.Cos(Math.PI * i / 180);
baseperim[2][0] = R * Math.Sin(Math.PI * i / 180);
baseperim[3][0] = 1;
// peek base circumference point
peekperim[0][0] = H;
peekperim[1][0] = R * Math.Cos(Math.PI * i / 180);
peekperim[2][0] = R * Math.Sin(Math.PI * i / 180);
peekperim[3][0] = 1;
break;
case ALONGY:
// base circumference point
baseperim[0][0] = R * Math.Cos(Math.PI * i / 180);
baseperim[1][0] = 0;
baseperim[2][0] = R * Math.Sin(Math.PI * i / 180);
baseperim[3][0] = 1;
// peek base circumference point
peekperim[0][0] = R * Math.Cos(Math.PI * i / 180);
peekperim[1][0] = H;
peekperim[2][0] = R * Math.Sin(Math.PI * i / 180);
peekperim[3][0] = 1;
break;
case ALONGZ:
// base circumference point
baseperim[0][0] = R * Math.Cos(Math.PI * i / 180);
baseperim[1][0] = R * Math.Sin(Math.PI * i / 180);
baseperim[2][0] = 0;
baseperim[3][0] = 1;
// peek base circumference point
peekperim[0][0] = R * Math.Cos(Math.PI * i / 180);
peekperim[1][0] = R * Math.Sin(Math.PI * i / 180);
peekperim[2][0] = H;
peekperim[3][0] = 1;
break;
}
// transforming coordinates with translation matrix
int rows = 0, cols = 0;
baseperim = ArmLink.mulMatrices(T, baseperim, 4, 4, 1, ref rows, ref cols);
peekperim = ArmLink.mulMatrices(T, peekperim, 4, 4, 1, ref rows, ref cols);
// Computing screen coordinates
double Xs1, Ys1, Xs2, Ys2, x1, y1, z1, x2, y2, z2;
x1 = baseperim[0][0];
y1 = baseperim[1][0];
z1 = baseperim[2][0];
x2 = peekperim[0][0];
y2 = peekperim[1][0];
z2 = peekperim[2][0];
Ys1 = ((y1 - eyeofsY) * eyedistance / (z1 + eyedistance));
Xs1 = ((x1 - eyeofsX) * eyedistance / (z1 + eyedistance));
Ys2 = (y2 - eyeofsY) * eyedistance / (z2 + eyedistance);
Xs2 = (x2 - eyeofsX) * eyedistance / (z2 + eyedistance);
double pixpercmX = panelxdim / 35;
double pixpercmY = panelydim / 35;
System.Drawing.Pen pen = new Pen(color);
// drawing circles
gr.DrawEllipse(pen, (float)(Xs1 * pixpercmX), (float)(panelydim - Ys1 * pixpercmY), 1, 1);
gr.DrawEllipse(pen, (float)(Xs2 * pixpercmX), (float)(panelydim - Ys2 * pixpercmY), 1, 1);
if (i % 15 == 0)
{
gr.DrawLine(pen, (float)(Xs1 * pixpercmX), (float)(panelydim - Ys1 * pixpercmY), (float)(Xs2 * pixpercmX), (float)(panelydim - Ys2 * pixpercmY));
}
}
}
