Library to calculate angles from positions and vice versa (also used for shooting angles)
Dependents: includeair includeair calcul_Ueff2 Mesure_energie
Diff: angleandposition.cpp
- Revision:
- 3:067c75c62882
- Parent:
- 2:b99fef8df97e
- Child:
- 4:9492c1c361b9
diff -r b99fef8df97e -r 067c75c62882 angleandposition.cpp --- a/angleandposition.cpp Mon Oct 19 15:05:21 2015 +0000 +++ b/angleandposition.cpp Tue Oct 27 16:13:26 2015 +0000 @@ -22,10 +22,10 @@ double d2_square = pow(bar1,2) - pow( ( pow(virt_bar_right,2) + pow(bar1,2) - pow(bar2,2) ) / (-2*virt_bar_right) ,2); double d2 = sqrt(d2_square); - + double phi; if (virt_bar_right <= virt_bar_ref) { - phi = pi-asin(d2/bar1); + phi = pi-asin(d2/bar1); } else { phi = asin(d2/bar1); } @@ -36,20 +36,63 @@ float angleandposition::positiontoangle2(float x_position, float y_position)//left arm { double virt_bar_left = sqrt(pow((x_position + 0.5*base_spacing) ,2) + pow(y_position,2) ); - + double d2_square = pow(bar1,2) - pow( ( pow(virt_bar_left,2) + pow(bar1,2) - pow(bar2,2) ) / (-2*virt_bar_left), 2); - + double d2 = sqrt(d2_square); - + double phi; - + if (virt_bar_left <= virt_bar_ref) { - phi = pi-asin(d2/bar1); + phi = pi-asin(d2/bar1); } else { - phi = asin(d2/bar1); + phi = asin(d2/bar1); } double theta_l = pi - phi - acos((x_position + 0.5*base_spacing)/virt_bar_left); return theta_l; +} + +float angleandposition::angletoposition(float theta_r,float theta_l) +{ + + double virt_bar_x = base_spacing + cos(theta_l)*bar1 + cos(theta_r)*bar1; + double virt_bar_y = sin(theta_l)*bar1 - sin(theta_r)*bar1; + double virt_bar = sqrt(pow(virt_bar_x,2) + pow(virt_bar_y,2)); + + + double phi = acos(0.5*virt_bar/bar2); + double phi_diff = atan(abs(virt_bar_y)/virt_bar_x); + + double theta_l2; + if (sin(theta_l) >= sin(theta_r)) { + theta_l2 = phi - phi_diff; + } else { + theta_l2 = phi + phi_diff; + } + + double x = -cos(theta_l)*bar1 - 0.5*base_spacing + cos(theta_l2)*bar2; + + return x; +} + +float angleandposition::angletoposition_y (float theta_r, float theta_l) +{ + double virt_bar_x = base_spacing + cos(theta_l)*bar1 + cos(theta_r)*bar1; + double virt_bar_y = sin(theta_l)*bar1 - sin(theta_r)*bar1; + double virt_bar = sqrt(pow(virt_bar_x,2) + pow(virt_bar_y,2)); + + double phi = acos(0.5*virt_bar/bar2); + double phi_diff = atan(abs(virt_bar_y)/virt_bar_x); + + double theta_l2; + if (sin(theta_l) >= sin(theta_r)) { + theta_l2 = phi - phi_diff; + } else { + theta_l2 = phi + phi_diff; + } + double y = sin(theta_l)*bar1 + sin(theta_l2)*bar2; + + return y; } \ No newline at end of file