#include "ikLib.h"

/*
length units are mm
angle units are degrees

len   = dist3d(dist p1x, dist p1y, dist p1z, dist p2x, dist p2y, dist p2z);
angle = elbow(dist d, dist l1, dist l2);
angle = dir(dist x, dist y);
 
*/

dist dist3d(dist p1x, dist p1y, dist p1z, dist p2x, dist p2y, dist p2z){
  return sqrt(sqr(p1x-p2x) + sqr(p1y-p2y) + sqr(p1z-p2z));
}

angle elbow(dist C, dist A, dist B){ // returns angle c
  return  fromRads(
            acos(   
               ((double)sqr(A)+(double)sqr(B)-(double)sqr(C)) / ((double)2*(double)A*(double)B) 
            ) 
           );
}

angle dir(dist x, dist y){
  return fromRads(atan2(y, x));
}