int main()
{
double sidec = hypotenuse( 1E-10, 2E-200);
printf("The hypotenuse of the right triangle is %.10lG\n", sidec);
return 0;
}
int main(int argc, char* argv[])
{
double a, b;
scanf("%lf %lf", &a, &b);
printf("%f\n", hypotenuse(a, b));
return 0;
}
int main(){
double leg1, leg2;
printf("Input the length of a leg: ");
if (!scanf("%lf", &leg1)) die("Input failure.\n");
printf("Input the length of a leg: ");
if (!scanf("%lf", &leg2)) die("Input failure.\n");
printf("The hypotenuse of %f and %f is %f.\n", leg1, leg2, hypotenuse(leg1, leg2));
return 0;
}
int main() {
double s1, s2, s3;
printf("Enter the length of side 1: ");
scanf("%lf", &s1);
printf("Enter the length of side 2: ");
scanf("%lf", &s2);
s3 = hypotenuse(s1, s2);
printf("Length of hypotenuse: %.1f\n", s3);
return 0;
}
void mouse_cb(const geometry_msgs::PoseStamped& msg)
{
if(path_write_step>-1)
{
gap_x = msg.pose.position.x - leader_path.poses[path_write_step].pose.position.x;
gap_y = msg.pose.position.y - leader_path.poses[path_write_step].pose.position.y;
}
if(hypotenuse(gap_x,gap_y) > MIN_SPACING || path_write_step==-1)
{
leader_path.poses.push_back(msg); path_write_step++;
ROS_INFO("Recieved goal: %f,%f", msg.pose.position.x,msg.pose.position.y);
}
}
int main()
{
int i;
double side1, side2;
for(i = 1; i <= 3; i++) {
printf("Give me sides (s1 s2): ");
scanf("%lf%lf", &side1, &side2);
printf("Side1\tSide2\tHypotenuse\n");
printf("%.1f\t%.1f\t%.1f\n", side1, side2, hypotenuse(side1, side2));
}
return 0;
} /* E0F main */
void test_hypotenuse(void){
checkit_double(hypotenuse(3,4),5);
checkit_double(hypotenuse(3.45,1.2),3.652738699);
}
void scan_cb(const sensor_msgs::LaserScan::ConstPtr& scan)
{
//extract 'chains' of scan points
count = chain_started = 0; memset(chain, 0, sizeof chain);
for(i=0; i<scan->ranges.size(); i++)
{
if(within_p(scan->ranges[i+1],scan->ranges[i]) && !chain_started && scan->ranges[i]!=0)
{
point_counter = chain_started = 1;
chain[count][0] = i;
}
else if(chain_started && !within_p(scan->ranges[i+1],scan->ranges[i]))
{
chain_started = 0;
if(point_counter >= 15)
{
chain[count][1] = i;
count++;
}
}
else if(chain_started && within_p(scan->ranges[i+1],scan->ranges[i])) point_counter++;
}
//find chain most likely to match target chain from previous scan, get x/y co-ordinates
closest=second_closest = count; close_chains_count = 0;
if(!pub_bool.data && initial_datum_set) smallest = 1.0; else smallest = 2.0;
second_smallest = 4.0;
for(i=0; i<count; i++)
{
left_ang = (chain[i][1] - 0.5*scan->ranges.size()) * scan->angle_increment;
right_ang = (chain[i][0] - 0.5*scan->ranges.size()) * scan->angle_increment;
lx = scan->ranges[chain[i][1]]*cos(left_ang); rx = scan->ranges[chain[i][0]]*cos(right_ang);
ly = scan->ranges[chain[i][1]]*sin(left_ang); ry = scan->ranges[chain[i][0]]*sin(right_ang);
temp_x = (lx+rx)/2; temp_y = (ly+ry)/2;
diffx = target_real.transform.translation.x-temp_x; diffy = target_real.transform.translation.y-temp_y;
dist_to_datum = hypotenuse(diffx,diffy);
if(dist_to_datum<4.0)
{
close_chains[close_chains_count][0] = temp_x; close_chains[close_chains_count][1] = temp_y;
close_chains[close_chains_count][2] = atan2(rx-lx,ly-ry); fix_ang(close_chains[close_chains_count][2]);
close_chain_points[close_chains_count][0] = chain[i][0]; close_chain_points[close_chains_count][1] = chain[i][1];
if(dist_to_datum<smallest)
{
smallest = dist_to_datum; closest = close_chains_count;
}
else if(dist_to_datum<second_smallest)
{
second_smallest = dist_to_datum; second_closest = close_chains_count;
}
close_chains_count++;
}
}
if(!initial_datum_set)
{
last_datum_l = close_chain_points[closest][1]; last_datum_r = close_chain_points[closest][0];
initial_datum_set = 1;
}
//check if leader has suddenly vanished due to obstruction or spiderman
if(closest>close_chains_count && !pub_bool.data)
{
if(second_closest<=close_chains_count)
{
closest_obs = second_closest;
target_chain_obs = 1;
}
else leader_vanished = 1;
}
else leader_vanished = 0;
//if previously obstructed, track obstructing chain
if(pub_bool.data && closest<=close_chains_count)
{
smallest = 1.0;
for(i=0; i<close_chains_count; i++)
{
diffx = close_chains[i][0]-obs_x; diffy = close_chains[i][1]-obs_y;
dist_to_datum = hypotenuse(diffx,diffy);
if(dist_to_datum<smallest)
{
smallest = dist_to_datum; closest_obs = i;
}
}
if(closest==closest_obs) target_chain_obs = 1;
else target_chain_obs = 0;
}
//check for partial obstruction
ol_l=ol_r = 0; last_datum_ln = (double)(last_datum_l-last_datum_r);
if(!target_chain_obs && closest<close_chains_count-1)
{
for(i=close_chain_points[closest+1][0]; i<last_datum_l; i++)
{
if(scan->ranges[i]>0.5 && target_real.transform.translation.x-scan->ranges[i] > 0.3) ol_l++;
}
}
if(!target_chain_obs && closest>0 && close_chains_count>closest)
{
for(i=close_chain_points[closest-1][1]; i>last_datum_r; i--)
{
if(scan->ranges[i]>0.5 && target_real.transform.translation.x-scan->ranges[i] > 0.3) ol_r++;
}
}
target_chain_part_obs = 1;
if(ol_l/last_datum_ln > 0.05) closest_obs = closest+1;
else if(ol_r/last_datum_ln > 0.05) closest_obs = closest-1;
else target_chain_part_obs = 0;
//publish tf if unobstructed
if(!target_chain_obs && !target_chain_part_obs && !leader_vanished)
{
last_datum_l = close_chain_points[closest][1]; last_datum_r = close_chain_points[closest][0];
target_real.header.stamp = ros::Time::now();
target_real.transform.translation.x = close_chains[closest][0];
target_real.transform.translation.y = close_chains[closest][1];
target_real.transform.rotation = tf::createQuaternionMsgFromYaw(close_chains[closest][2]);
static tf::TransformBroadcaster broadcaster;
broadcaster.sendTransform(target_real);
pub_bool.data = 0;
//gap point placement
//need to find points which have potential to be collision
//left_bound_x = ; left_bound_y = ;
//right_bound_x = ; right_bound_y = ;
//if //points are close, then it's a door. need to handle differently
//-------------------------------------------------------------------
scanout.header = leaderless.header = target_real.header;
scanout.time_increment = leaderless.time_increment = scan->time_increment;
scanout.angle_min = leaderless.angle_min = scan->angle_min;
scanout.angle_max = leaderless.angle_max = scan->angle_max;
scanout.angle_increment = leaderless.angle_increment = scan->angle_increment;
scanout.time_increment = leaderless.time_increment = scan->time_increment;
scanout.scan_time = leaderless.scan_time = scan->scan_time;
scanout.range_min = leaderless.range_min = scan->range_min;
scanout.range_max = leaderless.range_max = scan->range_max;
for(i = 0; i < scan->ranges.size(); i++)
{
scanout.ranges[i] = 0;
leaderless.ranges[i] = scan->ranges[i];
}
for(i=last_datum_r; i < last_datum_l; i++)
{
scanout.ranges[i] = scan->ranges[i];
leaderless.ranges[i] = 0;
}
scan_pub.publish(scanout);
// leaderless.header.stamp = ros::Time::now(); leaderless.header.frame_id = "laser";
scan_pub2.publish(leaderless);
//----------------------------------------------------------------
}
else
{
if(!leader_vanished)
{
obs_x = close_chains[closest_obs][0]; obs_y = close_chains[closest_obs][1];
}
pub_bool.data = 1;
}
pub.publish(pub_bool);
}
