public static void FABRIKTest()
{
TDPoint target = new TDPoint(70f, 70f, 70f);
float r = (float)(Math.PI / 180);
ArmPartQ a = new ArmPartQ(50, 0, 0, 0, 0, 0 * r, 180 * r);
ArmPartQ b = new ArmPartQ(50, 0, 0, 0, 0, 0 * r, 180 * r);
ArmPartQ c = new ArmPartQ(50, 0, 0, 0, 0, 0 * r, 180 * r);
ArmPartQ[] asdf = { a, b, c };
float[] Zs = { 0f, 0f, 0f };
ArmQ arm1 = new ArmQ(Zs, 0 * r, asdf);
arm1.converge(target);
arm1.printAngles();
}
public void solve(int armNum)
{
Quaternion arm;
Quaternion XAxis;
Quaternion YAxis;
Quaternion ZAxis;
if (armNum == 0)
{
arm = new Quaternion(0, 1, 0, 0);
XAxis = new Quaternion(0, 1, 0, 0);
YAxis = new Quaternion(0, 0, 1, 0);
ZAxis = new Quaternion(0, 0, 0, 1);
}
else
{
arm = quatList[armNum].arm.Normalized();
XAxis = XAxes[armNum];
YAxis = YAxes[armNum];
ZAxis = ZAxes[armNum];
}
//Quaternion R;
//Quaternion P;
//Quaternion Y;
//Rotating
for (int b = armNum; b < angleX.Length; b++)
{
float theta, phi, alpha;
//checks if angle is fixed or not. A negative angle means the angle of the joint
//is fixed at that angle (-30 means fixed at 30)
{
if (angleX[b] < 0)
{
theta = -angleX[b];
}
else
{
theta = angleX[b];
}
if (angleY[b] < 0)
{
phi = -angleY[b];
}
else
{
phi = angleY[b];
}
if (angleZ[b] < 0)
{
alpha = -angleZ[b];
}
else
{
alpha = angleZ[b];
}
}
double ro = Math.Sin(rad(theta / 2));
Quaternion R = new Quaternion((float)Math.Cos(rad(theta / 2)), (float)(XAxis.i * ro), (float)(XAxis.j * ro), (float)(XAxis.k * ro));
ro = Math.Sin(rad(phi / 2));
Quaternion Y = new Quaternion((float)Math.Cos(rad(phi / 2)), (float)(YAxis.i * ro), (float)(YAxis.j * ro), (float)(YAxis.k * ro));
ro = Math.Sin(rad(alpha / 2));
Quaternion P = new Quaternion((float)Math.Cos(rad(alpha / 2)), (float)(ZAxis.i * ro), (float)(ZAxis.j * ro), (float)(ZAxis.k * ro));
arm = (R * (Y * (P * arm * !P) * !Y) * !R);
quatList[b] = new ArmPartQ(arm.r, arm.i, arm.j, arm.k, Xmin[b], Xmax[b], Ymin[b], Ymax[b], Zmin[b], Zmax[b]);
XAxes[b] = XAxis;
YAxes[b] = YAxis;
ZAxes[b] = ZAxis;
XAxis = R * (Y * (P * XAxis * !P) * !Y) * !R; //x axis always equals temp so change later maybe
YAxis = R * (Y * (P * YAxis * !P) * !Y) * !R;
ZAxis = R * (Y * (P * ZAxis * !P) * !Y) * !R;
}
//Elongating
for (int i = armNum; i < armLength.Length; i++)
{
quatList[i].arm *= armLength[i];
}
}