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