algortihm maintai local position and distance

السلام عليكم

انا بصدد برمجة خوارزمية بحيث

 

input = C1[3],C2[3],C3[3],C4[3],C5[3],C6[3],C7[3],Target[3]
output =C'1[3],C'2[3],C'3[3],C'4[3],C'5[3],C'6[3],C'7[3

 

هدف الخوارزمية هو جعل   C7 في مكان target  بحيث يجب ان نحافظ على قيمة C1 نفسها و نحافظ على المسافة بين 

C1-C2C2-C3C3-C4C4-C5C5-C6C6-C7

و نحافظ ايضا على

 

موقع C2 بالنسبة لِC1                    

موقع C3 بالنسبة لِC2

موقع C4 بالنسبة لِC3

موقع C5 بالنسبة لِC4

موقع C6 بالنسبة لِC5

موقع C7 بالنسبة لِC6

 

the imput of my algortihme is
C1,C2,C3,C4,C5,C6,C7, Target
where the aim is :
-C7 will be in target Position
-C1 d'ont change it's position
- keep the distance and the local position between
C7-C6,C6-C5,C3-C2,C2-C1
C7-C6= mean the posiiton of the C7 relative to C6 (we must keep it and distance too)
SO the algorithm is like that

 عملت الخوارزمية  لكن عندي مشكلة في النتيجة  تحقق كل الشروط الا الشرط الاخير 

هاهي خوارزميتي ارجو مساعدتي فيها

Real  C1_Orig[3];
 Ogre::Real  C7_Temp[3];
  Tolerance=1;
  while (Tolerance > 1.0E-5) {
      for (i0 = 0; i0 < 3; i0++) {
        C7_Temp[i0] = C7[i0];
      }
for (i0 = 0; i0 < 3; i0++) {
    C1_Orig[i0] = C1[i0];
  }
      k++;
     
      for (i0 = 0; i0 < 3; i0++) {
        C7[i0] = Target[i0];
      }
 
         maintain_relative_position(C7, pos66,dv0);
      for (i0 = 0; i0 < 3; i0++) {
        C6[i0] = dv0[i0];
      }
 
 
       maintain_relative_position(C6, pos55,dv0);
      for (i0 = 0; i0 < 3; i0++) {
        C5[i0] = dv0[i0];
      }
 
       maintain_relative_position(C5, pos44,dv0);
     
      for (i0 = 0; i0 < 3; i0++) {
        C4[i0] = dv0[i0];
      }
 
       maintain_relative_position(C4, pos33,dv0);
      for (i0 = 0; i0 < 3; i0++) {
        C3[i0] = dv0[i0];
      }
 
 
      maintain_relative_position(C3, pos22,dv0);
   
      for (i0 = 0; i0 < 3; i0++) {
        C2[i0] = dv0[i0];
      }
 
 
 maintain_relative_position(C2, pos11,dv0);
 
      for (i0 = 0; i0 < 3; i0++) {
        C1[i0] = dv0[i0];
      }
 
         
      Tolerance = b_VectorMagnitude(C1, C1_Orig);
 
 
      if (Tolerance > 0.0) {
        /*  cout<<"stage2222";
    cout<<"\n";*/
        for (i0 = 0; i0 < 3; i0++) {
          C1[i0] = C1_Orig[i0];
        }
 
         
         maintain_relative_position(C1, pos2,dv0);
        for (i0 = 0; i0 < 3; i0++) {
          C2[i0] = dv0[i0];
        }
     
         maintain_relative_position(C2, pos3,dv0);
        for (i0 = 0; i0 < 3; i0++) {
          C3[i0] = dv0[i0];
        }
     
         maintain_relative_position(C3, pos4,dv0);
        for (i0 = 0; i0 < 3; i0++) {
          C4[i0] = dv0[i0];
        }
     
        maintain_relative_position(C4, pos5,dv0);
        for (i0 = 0; i0 < 3; i0++) {
          C5[i0] = dv0[i0];
        }
 
        maintain_relative_position(C5, pos6,dv0);
        for (i0 = 0; i0 < 3; i0++) {
          C6[i0] = dv0[i0];
        }
 
        maintain_relative_position(C6, pos7,dv0);
        for (i0 = 0; i0 < 3; i0++) {
          C7[i0] = dv0[i0];
        }
 
        Tolerance = b_VectorMagnitude(C7, Target);
        if (abs(b_VectorMagnitude(C7_Temp, C7)) < 1.0E-5) {
          Tolerance =  5.0E-6;
        }
      }
    }
  }

void maintain_relative_position(Real base_point[3], Real relative_position[3], Real new_point[3]) {    /* new_point is an output parameter whose value on return is the point that is relative_position away from base_point. */    int i;    for (i = 0; i < 3; i++) {        new_point = base_point + relative_position;    }}

"b_VectorMagnitude(C7, Target)" is to calculate the distance between Target and C7