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