void waypoint_init(frame_wait_confirm *frame_wait_confirm) { int i; int waypoint_num_this_frame ; int waypoint_total_num; uint8 *waypoint; // extract way point number of this frame waypoint_num_this_frame= frame_wait_confirm->data[CTRL_FRAME_MASK_WP_NUM+2]; waypoint_total_num = frame_wait_confirm->data[CTRL_FRAME_MASK_WP_NUM+1] << 8 | frame_wait_confirm->data[CTRL_FRAME_MASK_WP_NUM]; // if this is the first frame of way point packet, free the way point list in the memory if(frame_wait_confirm->frame_id == 1){ waypoint_list_clear(); waypoint_info.total_num = 0; waypoint_info.received_num = 0; // extract total number of way point waypoint_info.total_num = waypoint_total_num; } if(waypoint_total_num != waypoint_info.total_num){ /* the total way point number field in the frames of whole packet is different, * which means something is wrong */ print_err("way point number err\n"); return; } for (i = 0; i < waypoint_num_this_frame; i++) { waypoint = frame_wait_confirm->data + 4 + i* WAYPOINT_INFO_LEN; waypoint_list_add(waypoint); } if(frame_wait_confirm->frame_id == frame_wait_confirm->frame_num ){ if(waypoint_info.received_num != waypoint_info.total_num){ /* the received way point is not same as total number * which means something is wrong */ print_err("way point missing\n"); return; } } waypoint_list_current = waypoint_list_head; }
//-------------------------------------------------------------------------------------------- bool FindPath( waypoint_list_t& wplst, Object * pchr, float dst_x, float dst_y, bool * used_astar_ptr ) { // FindPath /// @author ZF /// @details Ported the A* path finding algorithm by birdsey and heavily modified it /// This function adds enough waypoints to get from one point to another int src_ix, src_iy; int dst_ix, dst_iy; line_of_sight_info_t los_info; bool straight_line; bool returncode = false; if ( NULL != used_astar_ptr ) { *used_astar_ptr = false; } //Our current position src_ix = ( int )pchr->getPosX() / GRID_ISIZE; src_iy = ( int )pchr->getPosY() / GRID_ISIZE; //Destination position dst_ix = dst_x / GRID_ISIZE; dst_iy = dst_y / GRID_ISIZE; //Always clear any old waypoints waypoint_list_clear( wplst ); //Don't do need to do anything if there is no need to move if ( src_ix == dst_ix && src_iy == dst_iy ) return false; returncode = false; //setup line of sight data for source los_info.stopped_by = pchr->stoppedby; los_info.x0 = pchr->getPosX(); los_info.y0 = pchr->getPosY(); los_info.z0 = 0; //setup line of sight to target los_info.x1 = dst_x; los_info.y1 = dst_y; los_info.z1 = 0; // test for the simple case... a straight line straight_line = !line_of_sight_blocked( &los_info ); if ( !straight_line ) { #ifdef DEBUG_ASTAR printf( "Finding a path from %d,%d to %d,%d: \n", src_ix, src_iy, dst_ix, dst_iy ); #endif //Try to find a path with the AStar algorithm if ( AStar_find_path( _currentModule->getMeshPointer(), pchr->stoppedby, src_ix, src_iy, dst_ix, dst_iy ) ) { returncode = AStar_get_path( dst_x, dst_y, wplst); } if ( NULL != used_astar_ptr ) { *used_astar_ptr = true; } } //failed to find a path if ( !returncode ) { // just use a straight line path waypoint_list_push( wplst, dst_x, dst_y ); } return returncode; }