   ClosedForm/AnalyticalSolution Inverse Kinematics
  So my two-linked arm is rotating really weird and I was wondering if someone here can spot the mistake. This is the only place where I manipulate ArticulatedArm. So I'm pretty sure my matrices are correct. Am I applying the transformations to the limbs correctly?/////////////// InitializingArticulatedArm.push_back( Custom::Point3D(0, 0, 0, 1) );//ShoulderArticulatedArm.push_back( Custom::Point3D(20, 10, 0, 1) );//ElbowArticulatedArm.push_back( Custom::Point3D(30, 20, 0, 1) );//Handvoid UpdateArticulatedArm(){/// Local Variables ///////////////////////////////////////////////////////////float l1,l2;// BONE LENGTH FOR BONE 1 AND 2float ex,ey;// ADJUSTED TARGET POSITIONfloat sin2,cos2;// SINE AND COSINE OF ANGLE 2float angle1,angle2;// ANGLE 1 AND 2 IN RADIANSfloat tan1;// TAN OF ANGLE 1//////////////////////////////////////////////////////////////////////////// Effector positionex = ...ey = ...   Custom::Vector3D ShoulderToElbow( ArticulatedArm[1] - ArticulatedArm[0] );   l1 = ShoulderToElbow.Magnitude();   Custom::Vector3D ElbowToHand( ArticulatedArm[2] - ArticulatedArm[1] );   l2 = ElbowToHand.Magnitude();// CALCULATE THE COSINE OF ANGLE 2//cos2 = ((ex * ex) + (ey * ey) – (l1 * l1) - (l2 * l2)) / (2 * l1 * l2);cos2 = pow(ex,2) + pow(ey,2);cos2 += pow(l1,2)*-1;cos2 += pow(l2,2) *-1;cos2 /= 2 * l1 * l2;// IF IT IS NOT IN THIS RANGE, IT IS UNREACHABLEif (cos2 >= -1.0 && cos2 <= 1.0){   angle2 = (float)acos(cos2);// GET THE ANGLE WITH  ARCCOSINE   AngleForElbowToHand = Custom::RadiansToDegrees(angle2);   sin2 = (float)sin(angle2);// CALC THE SINE OF ANGLE 2   // COMPUTE ANGLE 1   // CALCULATE THE TAN OF ANGLE 1   tan1 = (-(l2 * sin2 * ex) + ((l1 + (l2 * cos2)) * ey)) /   ((l2 * sin2 * ey) + ((l1 + (l2 * cos2)) * ex));   // GET THE ACTUAL ANGLE   angle1 = atan(tan1);   AngleForShoulderToElbow = Custom::RadiansToDegrees(angle1); ///////////// TRANSLATE THE FOREARM TO ORIGIN, ROTATE, TRANSLATE BACK   Custom::Matrix4x4 TranslateToOrigin;   TranslateToOrigin.SetTranslation( -ArticulatedArm[1].x, -ArticulatedArm[1].y, -ArticulatedArm[1].z );   TranslateToOrigin.Apply_Transformation_To_VectorOrPoint( ArticulatedArm[2] );   Custom::Matrix4x4 RotateAboutOrigin;   RotateAboutOrigin.SetRotation( AngleForElbowToHand, Custom::Vector3D(0,0,1,0) );   RotateAboutOrigin.Apply_Transformation_To_VectorOrPoint( ArticulatedArm[2] );   Custom::Matrix4x4 TranslateBack1;   TranslateBack1.SetTranslation( ArticulatedArm[1].x, ArticulatedArm[1].y, ArticulatedArm[1].z );   TranslateBack1.Apply_Transformation_To_VectorOrPoint( ArticulatedArm[2] ); ///////////// ROTATE UPPERARM   Custom::Matrix4x4 RotateAboutOrigin2;   RotateAboutOrigin2.SetRotation( AngleForShoulderToElbow, Custom::Vector3D(0,0,1,0) );   RotateAboutOrigin2.Apply_Transformation_To_VectorOrPoint( ArticulatedArm[1] );}}[Edited by - 16bit_port on April 27, 2009 8:06:56 AM]
  Read my response here: http://www.gamedev.net/community/forums/viewreply.asp?ID=3429118;  That's exactly what I'm doing...   