public Joint(double theta, double d, double a, double alpha, int pos)
{
this.theta = theta;
this.d = d;
this.a = a;
this.alpha = alpha;
this.pos = pos;
this.correct = (d > 0) && (a > 0);
this.trans = new Transmatrix(this);
}
public Transmatrix calcWholeMatrix()
{
Transmatrix newMatrix = new Transmatrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
foreach (Joint joint in Joints)
{
newMatrix = Transmatrix.Multiply(newMatrix, joint.TransMatrix());
}
return(newMatrix);
}
public static Transmatrix Multiply(Transmatrix One, Transmatrix Two)
{
double[] a = One.A();
double[] b = One.B();
double[] c = One.C();
double[] d = One.D();
double[] w = Two.A();
double[] x = Two.B();
double[] y = Two.C();
double[] z = Two.D();
//First Column
double[] r = new double[4];
for (int i = 0; i < 4; i++)
{
r[i] = a[i] * w[0] + b[i] * w[1] + c[i] * w[2] + d[i] * w[3];
}
//Second Cloumn
double[] s = new double[4];
for (int i = 0; i < 4; i++)
{
s[i] = a[i] * x[0] + b[i] * x[1] + c[i] * x[2] + d[i] * x[3];
}
//Third Column
double[] t = new double[4];
for (int i = 0; i < 4; i++)
{
t[i] = a[i] * y[0] + b[i] * y[1] + c[i] * y[2] + d[i] * y[3];
}
//Fourth Column
double[] u = new double[4];
for (int i = 0; i < 4; i++)
{
u[i] = a[i] * z[0] + b[i] * z[1] + c[i] * z[2] + d[i] * z[3];
}
return(new Transmatrix(r[0], r[1], r[2], r[3], s[0], s[1], s[2], s[3], t[0], t[1], t[2], t[3], u[0], u[1], u[2], u[3]));
}
protected void AddJoint(Joint newJoint)
{
Joints.Add(newJoint);
wholeMatrix = calcWholeMatrix();
}
public Manipulator()
{
Joints = new List <Joint>();
wholeMatrix = calcWholeMatrix();
}
