//Figure out with our changed axes
public void findPoints()
{
float z = 0;
points[0] = new TDPoint(0, 0, 0);
for (int i = 0; i < Zcur.Length; i++)
{
z += Zcur[i];
points[i + 1] = points[i] + new TDPoint((float)(parts[i].length * Math.Cos(baser) * Math.Cos(z)),
(float)(parts[i].length * Math.Sin(z)),
(float)(parts[i].length * Math.Cos(z) * Math.Sin(baser)));
}
Console.WriteLine();
}
//check later
public double angleBetween(TDPoint other)
{
float crossed = (this % other).y;
if (crossed == 0)
{
if (this.x / other.x < 0)
{
return(Math.PI);
}
return(0);
}
return((crossed / Math.Abs(crossed)) * (Math.Acos((this * other) / (this.Length * other.Length))));
}
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 double angleRelativeTo(TDPoint other)
{
float crossed = (this % other).z;
if (crossed == 0)
{
return(0);
}
else
{
return((crossed / Math.Abs(crossed)) * Math.Acos((this * other) / (this.Length * other.Length)));
}
//else if (crossed > 0) {
// return Math.Acos((this * other) / (this.Length * other.Length));
//}
//else {
// return -Math.Acos((this * other) / (this.Length * other.Length));//(2 * Math.PI) - (Math.Acos((this * other) / (this.Length * other.Length)));
//}
}
public static void ArmFTest()
{
TDPoint target = new TDPoint(80f, 80f, 0f);
float r = (float)(Math.PI / 180);
ArmPartF a = new ArmPartF(50, 0, 0, 0, 0, 0 * r, 180 * r);
ArmPartF b = new ArmPartF(50, 0, 0, 0, 0, 0 * r, 180 * r);
ArmPartF c = new ArmPartF(50, 0, 0, 0, 0, 0 * r, 180 * r);
//ArmPartF d = new ArmPartF(8f, 0, 0, 0, 0, 0 * r, 180 * r);
ArmPartF[] asdf = { a, b, c };
float[] Zs = { 0f, 0f, 0f };
ArmF arm1 = new ArmF(Zs, 0 * r, asdf);
arm1.converge(target);
arm1.printAngles();
}
public bool isEqual(TDPoint other)
{
return(this.x == other.x && this.y == other.y && this.z == other.z);
}
public void converge(TDPoint target)
{
findPoints();
TDPoint tShadow = new TDPoint(target.x, 0, target.z);
TDPoint aShadow = new TDPoint(points[points.Length - 1].x, 0, points[points.Length - 1].z);
//decide sign of baseRotation
double baseRotation = aShadow.angleBetween(tShadow);
Console.WriteLine("Base Rotation: " + baseRotation);
double t = Math.Atan(target.y / (Math.Sqrt(target.x * target.x + target.z * target.z)));
TDPoint tar = new TDPoint((float)(target.Length * Math.Cos(t)), target.y, 0);
float zTot = 0;
points[0] = new TDPoint(0, 0, 0);
for (int i = 1; i < points.Length; i++)
{
zTot += Zcur[i - 1];
points[i] = new TDPoint((float)(parts[i - 1].length * Math.Cos(zTot)),
(float)(parts[i - 1].length * Math.Sin(zTot)),
0) + points[i - 1];
}
cost = calcCost(target);
int count = 1;
while (cost > 0.0001)
{
////////////////////////////////////////////Moving Joints////////////////////////////////////////////
for (int i = points.Length - 2; i >= 0; i--)
{
if (i == points.Length - 2)
{
points[i + 1] = tar;
}
points[i] = points[i + 1] + ((points[i] - points[i + 1]).Normalized() * parts[i].length);
}
points[0].set(0, 0, 0);
for (int i = 1; i < points.Length; i++)
{
points[i] = points[i - 1] + ((points[i] - points[i - 1]).Normalized() * parts[i - 1].length);
}
/////////////////////////////////////////Correcting Angles////////////////////////////////////////////
for (int i = points.Length - 2; i > 0; i--)
{
double ang = (points[i] | points[i - 1]).angleRelativeTo(points[i + 1] | points[i]);
if (ang < parts[i].Zmin || ang > parts[i].Zmax)
{
if (ang < parts[i].Zmin)
{
Zcur[i] = parts[i].Zmin;
}
else
{
Zcur[i] = parts[i].Zmax;
}
}
else
{
Zcur[i] = (float)ang;
}
}
double angle = (new TDPoint(1, 0, 0)).angleRelativeTo(points[1]);
if (angle < parts[0].Zmin)
{
Zcur[0] = parts[0].Zmin;
}
else if (angle > parts[0].Zmax)
{
Zcur[0] = parts[0].Zmax;
}
else
{
Zcur[0] = (float)angle;
}
//////////////////////////////////////Recalculating Points/////////////////////////////////////////
//{
float curZ = 0;
points[0].set(0, 0, 0);
for (int i = 1; i < this.points.Length; i++)
{
curZ = (float)((curZ + this.Zcur[i - 1]));// % (2 * Math.PI));
points[i] = points[i - 1] + (new TDPoint((float)(parts[i - 1].length * Math.Cos(curZ)), (float)(parts[i - 1].length * Math.Sin(curZ)), 0));
}
//}
cost = calcCost(tar);
count++;
}
Console.WriteLine(baseRotation);
printPoints();
Console.WriteLine(count);
Console.WriteLine();
}
public double calcCost(TDPoint target)
{
return(Math.Sqrt(Math.Pow(points[points.Length - 1].x - target.x, 2) + Math.Pow(points[points.Length - 1].y - target.y, 2) + Math.Pow(points[points.Length - 1].z - target.z, 2)));
}
public static void alignmentTest()
{
TDPoint t = new TDPoint(10, 10, 0);
}
