thank you for all your replies, i have included snippets of the code below:

boolean Leg::PrepareNewOrigin(int _ox, int _oy, int _oz)

{

return(PrepareNewPosition(_ox,_oy,_oz,dx,dy,dz));

}

boolean Leg::PrepareNewPosition(int _ox, int _oy, int _oz, int _dx, int _dy, int _dz)

{

double wcoxa, deltax, deltay, deltaz, s, r_sq, ws_r, wtibfemur, wfemur_r;

unsigned long current_time=micros();

char buffer[16];

wcoxa=atan2(_dy-_oy,_dx-_ox);

deltax=(_dx-_ox)-cos(wcoxa)*LENGTH_COXA;

deltay=(_dy-_oy)-sin(wcoxa)*LENGTH_COXA;

deltaz=_dz-_oz;

r_sq=deltax*deltax+deltay*deltay;

s=sqrt(r_sq);

r_sq+=deltaz*deltaz;

ws_r=atan(deltaz/s);

wtibfemur=acos((LENGTH_FEMUR*LENGTH_FEMUR+LENGTH_TIBIA*LENGTH_TIBIA-r_sq)/(2*LENGTH_FEMUR*LENGTH_TIBIA));

wfemur_r=acos((LENGTH_FEMUR*LENGTH_FEMUR-LENGTH_TIBIA*LENGTH_TIBIA+r_sq)/(2*LENGTH_FEMUR*sqrt(r_sq)));

if(r_sq<((LENGTH_FEMUR+LENGTH_TIBIA)*(LENGTH_FEMUR+LENGTH_TIBIA)))

{

if(wcoxa > 0)

angleCoxa=(int)(wcoxa*180/M_PI);

else

angleCoxa=(int)(180+wcoxa*180/M_PI);

angleHip=(int)(90+(ws_r+wfemur_r)*180/M_PI);

angleKnee=(int)(wtibfemur*180/M_PI);

IKdx=_dx;

IKdy=_dy;

IKdz=_dz;

IKox=_ox;

IKoy=_oy;

IKoz=_oz;

KinematicOK=true;

}

else KinematicOK=false;

current_time=micros()-current_time;

Serial.print("IKCalc/us");

Serial.println(current_time);

return KinematicOK;

}

boolean Robot::SetBodyPosition(byte _roll, byte _pitch, byte _crawl)

{

int crawl_amount,pitch_amount,roll_amount,i;

double cos_b, cos_c, sin_b, sin_c, r21, r23, r31,x,y,z;

unsigned long current_time;

// _roll, _pitch = 0..0xff; only high nibble used

// so 0,1,2,3,4,5,6 (0-111) = left or forward, 7,8 (112-143) = still; 9,a,b,c,d,e,f (144-255) = right or backward

// crawl = 0..0x07 - 0 = upright; 0x07 = crawl on the floor (MAX_CRAWL); only last 3 bits used

_roll>>=4;

_pitch>>=4;

_crawl&=0x07;

if(_roll != roll || _pitch != pitch || _crawl != crawl)

{

if(_roll < 7)

roll_amount=-((int)(7-_roll))*(MAX_ROLL/7);

else if(_roll >

roll_amount=((int)(_roll-8))*(MAX_ROLL/7);

else roll_amount=0;

if(_pitch < 7)

pitch_amount=-((int)(7-_pitch))*(MAX_PITCH/7);

else if(_pitch >

pitch_amount=((int)(_pitch-8))*(MAX_PITCH/7);

else pitch_amount=0;

current_time=micros();

r21=(double)roll_amount*M_PI/180;

sin_b=sin(r21);

cos_b=cos(r21);

r23=(double)pitch_amount*M_PI/180;

sin_c=sin(r23);

cos_c=cos(r23);

r21=sin_b*sin_c;

r23=-cos_b*sin_c;

r31=-sin_b*cos_c;

current_time=micros()-current_time;

Serial.print("MatrixOne ");

Serial.println(current_time);

crawl_amount=(MAX_CRAWL/7)*(int)_crawl;

current_time=micros();

double x,y,z;

x=cos_b*double(RF_ORIGINX)+sin_b*double(RF_ORIGINZ);

y=r21*double(RF_ORIGINX)+cos_c*double(RF_ORIGINY)+r23*double(RF_ORIGINZ);

z=r31*double(RF_ORIGINX)+sin_c*double(RF_ORIGINY)+cos_b*double(RF_ORIGINZ);

current_time=micros()-current_time;

Serial.print("MatrixRF ");

Serial.println(current_time);

if(!RobotLegs[RIGHT_FRONT].PrepareNewOrigin(

lrint(x),

lrint(y),

lrint(z)-crawl_amount))

return false;

current_time=micros();

// x=cos_b*double(RR_ORIGINX)+sin_b*double(RR_ORIGINZ);

// y=r21*double(RR_ORIGINX)+cos_c*double(RR_ORIGINY)+r23*double(RR_ORIGINZ);

// z=r31*double(RR_ORIGINX)+sin_c*double(RR_ORIGINY)+cos_b*double(RR_ORIGINZ);

x=cos_b*50.0+sin_b*0.0;

y=r21*50.0+cos_c*(-50.0)+r23*0.0;

z=r31*50.0+sin_c*(-50.0)+cos_b*0.0;

current_time=micros()-current_time;

Serial.print("MatrixRR pre");

Serial.println(current_time);

:

:

:

}