EXPORT void check_double_cone_point(
INTERFACE *intfc)
{
SURFACE **s;
TRI *tri,*ptris[30],**tris;
int i,j,num_ptris,num_tris;
POINT *p;
HYPER_SURF *hs;
HYPER_SURF_ELEMENT *hse;
bool ptri_in_tri_list;
(void) printf("Start checking double cone point\n");
next_point(intfc,NULL,NULL,NULL);
while (next_point(intfc,&p,&hse,&hs))
{
num_ptris = 0;
num_tris = set_tri_list_around_point(p,Tri_of_hse(hse),
&tris,intfc);
for (s = intfc->surfaces; s && *s; ++s)
{
for (tri = first_tri(*s); !at_end_of_tri_list(tri,*s);
tri = tri->next)
{
if (Point_of_tri(tri)[0] == p ||
Point_of_tri(tri)[1] == p ||
Point_of_tri(tri)[2] == p)
ptris[num_ptris++] = tri;
}
}
for (i = 0; i < num_ptris; ++i)
{
ptri_in_tri_list = NO;
tri = ptris[i];
for (j = 0; j < num_tris; ++j)
{
if (tri == tris[j])
{
ptri_in_tri_list = YES;
break;
}
}
if (!ptri_in_tri_list)
{
(void) printf("double cone point found:\n");
(void) printf("double cone point p(%d) = %f %f %f\n",p,
Coords(p)[0],Coords(p)[1],Coords(p)[2]);
}
}
}
(void) printf("End checking double cone point\n");
} /* end check_double_cone_point */
bool HuboPath::Trajectory::_optimal_interpolation(const Eigen::VectorXd& velocities,
const Eigen::VectorXd& accelerations,
double frequency)
{
IndexArray joint_mapping;
get_active_indices(joint_mapping);
std::list<Eigen::VectorXd> waypoints;
Eigen::VectorXd next_point(joint_mapping.size());
for(size_t i=0; i<elements.size(); ++i)
{
for(size_t j=0; j<joint_mapping.size(); ++j)
{
next_point[j] = elements[i].references[joint_mapping[j]];
}
waypoints.push_back(next_point);
}
double tolerance = params.tolerance;
if(tolerance == 0)
{
tolerance = 0.01;
}
optimal_interpolation::Path optimal_path(waypoints, tolerance);
optimal_interpolation::Trajectory optimal_traj(optimal_path, velocities, accelerations);
if(!optimal_traj.isValid())
{
std::cout << "The trajectory interpolator for HUBO_PATH_OPTIMIZE has returned invalid!"
<< std::endl;
return false;
}
double dt = 1.0/frequency;
size_t traj_count = optimal_traj.getDuration()*frequency;
std::vector<hubo_path_element_t> savedElements = elements;
elements.clear();
elements.resize(traj_count);
for(size_t i=0; i<traj_count; ++i)
{
next_point = optimal_traj.getPosition(i*dt);
elements[i] = savedElements[optimal_traj.getPathSegmentIndex(i*dt)];
for(size_t j=0; j<joint_mapping.size(); ++j)
{
elements[i].references[joint_mapping[j]] = next_point[j];
}
}
params.interp = HUBO_PATH_RAW;
std::cout << "Successfully performed an optimal interpolation" << std::endl;
return true;
}
int execute_move(int *move, int do_flag, int algo)
{
int next[2], i;
//Make sure the move is valid
if (move[0] < 0 || move[0] > 999 || move[1] < 0 || move[1] > 999)
return -1;
if (do_flag)
{
//Make sure stone can be placed
if (abs(board[move[0]][move[1]]) == INF)
return -1;
//Place the stone
if (next_to_set)
board[move[0]][move[1]] = INF;
else
board[move[0]][move[1]] = -INF;
}
else
{
//Make sure stone can be removed
if (abs(board[move[0]][move[1]]) != INF)
return -1;
//Restore its value
board[move[0]][move[1]] = exact_pull(move); //is this necessary?
}
//Update the board
for(i = 0; i < MAX_NUMBER_OF_POINTS; i++)
{
if (!next_point(next, i, 1))
break;
//If we are not next_to_set subtract the pull
update_point(next, next_to_set, move);
}
//Flip the player
next_to_set = next_to_set > 0 ? 0 : 1;
//Store the move, and adjust number of moves remaining
i = MAX_NUMBER_OF_MOVES - NUM_MOVES_REMAINING;
if (do_flag)
{
moves[i * 2] = move[0];
moves[(i * 2) + 1] = move[1];
NUM_MOVES_REMAINING--;
}
else
{
i = i - 1;
moves[i * 2] = -1;
moves[(i * 2) + 1] = -1;
NUM_MOVES_REMAINING++;
}
return our_area();
}
/*ARGSUSED**/
EXPORT void set_propagation_limits(
Front *front,
Front *newfront)
{
HYPER_SURF_ELEMENT *hse;
HYPER_SURF *hs;
POINT *p;
(void) next_point(newfront->interf,NULL,NULL,NULL);
while (next_point(newfront->interf,&p,&hse,&hs))
{
n_pt_propagated(p) = NO;
t_pt_propagated(p) = NO;
}
if (front->interf->dim == 2)
{
NODE **n;
CURVE **c;
int i;
for (n = front->interf->nodes; *n; ++n)
propagation_status(*n) = PROPAGATED_NODE;
for (n = newfront->interf->nodes; *n; ++n)
{
for (i = 0; i < 2; ++i)
Node_vel(*n)[i] = 0.0;
propagation_status(*n) = UNPROPAGATED_NODE;
}
for (c = newfront->interf->curves; c && *c; ++c)
{
if (!is_closed_curve(*c))
(*c)->orientation = 0;
else
(*c)->orientation = (area_of_closed_curve(*c) > 0.0) ?
1 : -1;
}
}
initialize_max_front_speed(front);
} /*end set_propagation_limits*/
point random_sample(point p, bool (*inside)(const point), int niter) {
point cur_point(p.n), next_point(p.n), delta(p.n);
cur_point = p;
real eps = 0.1;
for(int iter = 0; iter < niter; iter++) {
next_point = cur_point;
random_unit_ball(&delta);
delta *= eps;
next_point += delta;
if(inside(next_point)) {
cur_point = next_point;
}
}
return cur_point;
}
void init_board()
{
int pos[2], i = 0;
FILE *file;
file = fopen("input", "r");
assert(file);
//TODO NEED TO FLIP NEXT_TO_SET AS MANY TIMES AS THERE ARE MOVES!
for (i = 0; i < MAX_NUMBER_OF_POINTS; i++)
{
if (!next_point(pos, i, 1))
break;
if(abs(board[pos[0]][pos[1]]) != INF)
board[pos[0]][pos[1]] = exact_pull(pos);
}
fclose(file);
}
bool HuboPath::Trajectory::_spline_interpolation(const Eigen::VectorXd& velocities,
const Eigen::VectorXd& accelerations,
double frequency)
{
IndexArray joint_mapping;
get_active_indices(joint_mapping);
std::vector<Eigen::VectorXd> waypoints;
Eigen::VectorXd next_point(joint_mapping.size());
for(size_t i=0; i<elements.size(); ++i)
{
for(size_t j=0; j<joint_mapping.size(); ++j)
{
next_point[j] = elements[i].references[joint_mapping[j]];
}
waypoints.push_back(next_point);
}
spline_interpolation::Spline cubic_spline(waypoints, velocities, accelerations, frequency);
if(!cubic_spline.valid())
{
std::cout << "ERROR: Could not produce a valid cubic spline interpolation!" << std::endl;
return false;
}
std::vector<Eigen::VectorXd> interpolation = cubic_spline.getTrajectory();
elements.clear();
elements.resize(interpolation.size());
for(size_t i=0; i<interpolation.size(); ++i)
{
Eigen::VectorXd& point = interpolation[i];
for(size_t j=0; j<joint_mapping.size(); ++j)
{
elements[i].references[joint_mapping[j]] = point[j];
}
}
params.interp = HUBO_PATH_RAW;
std::cout << "Successfully performed a spline interpolation" << std::endl;
return true;
}
/* Recursively realizes feasible sequences of moves and calls
* the evaulation function
*/
void alpha_better(int *move, int algo, int max_depth)
{
int best_v = 2 * INF, v = INF, i;
int best_move[2];
int max = next_to_set;
if(max)
best_v = -2 * INF;
for (i = 0; i < MAX_NUMBER_OF_POINTS; i = i + 1)
{
if(!next_point(move, i, algo))
break;
if (do_move(move) == -1)
continue;
if (max)
{
v = value(-1 * INF, INF, 1, 0, algo, max_depth);
if (v > best_v)
{
best_v = v;
best_move[0] = move[0];
best_move[1] = move[1];
}
}
else
{
v = value(-1 * INF, INF, 1, 1, algo, max_depth);
if (v < best_v)
{
best_v = v;
best_move[0] = move[0];
best_move[1] = move[1];
}
}
undo_move(move);
}
move[0] = best_move[0];
move[1] = best_move[1];
}
int value(int alpha, int beta, int depth, int max, int algo, int max_depth)
{
int v = -INF, i, move[2];
if (depth > min(max_depth, NUM_MOVES_REMAINING)){
return eval_fn();
}
for (i = 0; i < MAX_NUMBER_OF_POINTS; i++)
{
if(!next_point(move, i, algo))
break;
if(do_move(move) > 0)
{
if (max)
{
v = max(v, value(alpha, beta, depth + 1, 0, algo, max_depth));
if (v >= beta)
{
undo_move(move);
return v;
}
alpha = max(alpha, v);
}
else
{
v = min(v, value(alpha, beta, depth + 1, 1, algo, max_depth));
if (v <= alpha)
{
undo_move(move);
return v;
}
beta = min(beta, v);
}
undo_move(move);
}
}
return v;
}
LIB_LOCAL void copy_tg_pts_from_intfc(
TRI_GRID *ntg,
P_LINK *hash_table,
int h_size)
{
#if defined(TWOD) || defined(THREED)
TG_PT *fr_pt;
INTERFACE *intfc = ntg->grid_intfc;
POINT *point;
#endif /* defined(TWOD) || defined(THREED) */
reset_hash_table(hash_table,h_size);
#if defined(TWOD)
if (intfc->dim == 2)
{
BOND *b;
CURVE **c;
NODE **n;
fr_pt = ntg->front_points;
for (n = intfc->nodes; n && *n; ++n)
{
point = (*n)->posn;
Coords(fr_pt)[0] = Coords(point)[0];
Coords(fr_pt)[1] = Coords(point)[1];
(void) add_to_hash_table((POINTER)point,(POINTER)fr_pt,
hash_table,h_size);
++fr_pt;
}
for (c = intfc->curves; c && *c; ++c)
{
for (b = (*c)->first; b != (*c)->last; b = b->next)
{
point = b->end;
Coords(fr_pt)[0] = Coords(point)[0];
Coords(fr_pt)[1] = Coords(point)[1];
(void) add_to_hash_table((POINTER)point,(POINTER)fr_pt,
hash_table,h_size);
++fr_pt;
}
}
}
#endif /* defined(TWOD) */
#if defined(THREED)
if (intfc->dim == 3)
{
HYPER_SURF *hs;
HYPER_SURF_ELEMENT *hse;
fr_pt = ntg->front_points;
(void) next_point(intfc,NULL,NULL,NULL);
while (next_point(intfc,&point,&hse,&hs))
{
Coords(fr_pt)[0] = Coords(point)[0];
Coords(fr_pt)[1] = Coords(point)[1];
Coords(fr_pt)[2] = Coords(point)[2];
(void) add_to_hash_table((POINTER)point,(POINTER)fr_pt,
hash_table,h_size);
++fr_pt;
}
}
#endif /* defined(THREED) */
} /*end copy_tg_pts_from_intfc*/
inline void calculate_turns(Piece const& piece1, Piece const& piece2)
{
typedef typename boost::range_value<Rings const>::type ring_type;
typedef typename boost::range_value<Turns const>::type turn_type;
typedef typename boost::range_iterator<ring_type const>::type iterator;
segment_identifier seg_id1 = piece1.first_seg_id;
segment_identifier seg_id2 = piece2.first_seg_id;
if (seg_id1.segment_index < 0 || seg_id2.segment_index < 0)
{
return;
}
ring_type const& ring1 = m_rings[seg_id1.multi_index];
iterator it1_first = boost::begin(ring1) + seg_id1.segment_index;
iterator it1_last = boost::begin(ring1) + piece1.last_segment_index;
ring_type const& ring2 = m_rings[seg_id2.multi_index];
iterator it2_first = boost::begin(ring2) + seg_id2.segment_index;
iterator it2_last = boost::begin(ring2) + piece2.last_segment_index;
turn_type the_model;
the_model.operations[0].piece_index = piece1.index;
the_model.operations[0].seg_id = piece1.first_seg_id;
iterator it1 = it1_first;
for (iterator prev1 = it1++;
it1 != it1_last;
prev1 = it1++, the_model.operations[0].seg_id.segment_index++)
{
the_model.operations[1].piece_index = piece2.index;
the_model.operations[1].seg_id = piece2.first_seg_id;
iterator next1 = next_point(ring1, it1);
iterator it2 = it2_first;
for (iterator prev2 = it2++;
it2 != it2_last;
prev2 = it2++, the_model.operations[1].seg_id.segment_index++)
{
// Revert (this is used more often - should be common function TODO)
the_model.operations[0].other_id = the_model.operations[1].seg_id;
the_model.operations[1].other_id = the_model.operations[0].seg_id;
iterator next2 = next_point(ring2, it2);
// TODO: internally get_turn_info calculates robust points.
// But they are already calculated.
// We should be able to use them.
// this means passing them to this visitor,
// and iterating in sync with them...
typedef detail::overlay::get_turn_info
<
detail::overlay::assign_null_policy
> turn_policy;
turn_policy::apply(*prev1, *it1, *next1,
*prev2, *it2, *next2,
false, false, false, false,
the_model, m_robust_policy,
std::back_inserter(m_turns));
}
}
}
inline void calculate_turns(Piece const& piece1, Piece const& piece2,
Section const& section1, Section const& section2)
{
typedef typename geofeatures_boost::range_value<Rings const>::type ring_type;
typedef typename geofeatures_boost::range_value<Turns const>::type turn_type;
typedef typename geofeatures_boost::range_iterator<ring_type const>::type iterator;
signed_size_type const piece1_first_index = piece1.first_seg_id.segment_index;
signed_size_type const piece2_first_index = piece2.first_seg_id.segment_index;
if (piece1_first_index < 0 || piece2_first_index < 0)
{
return;
}
// Get indices of part of offsetted_rings for this monotonic section:
signed_size_type const sec1_first_index = piece1_first_index + section1.begin_index;
signed_size_type const sec2_first_index = piece2_first_index + section2.begin_index;
// index of last point in section, beyond-end is one further
signed_size_type const sec1_last_index = piece1_first_index + section1.end_index;
signed_size_type const sec2_last_index = piece2_first_index + section2.end_index;
// get geometry and iterators over these sections
ring_type const& ring1 = m_rings[piece1.first_seg_id.multi_index];
iterator it1_first = geofeatures_boost::begin(ring1) + sec1_first_index;
iterator it1_beyond = geofeatures_boost::begin(ring1) + sec1_last_index + 1;
ring_type const& ring2 = m_rings[piece2.first_seg_id.multi_index];
iterator it2_first = geofeatures_boost::begin(ring2) + sec2_first_index;
iterator it2_beyond = geofeatures_boost::begin(ring2) + sec2_last_index + 1;
// Set begin/end of monotonic ranges, in both x/y directions
signed_size_type index1 = sec1_first_index;
move_begin_iterator<0>(it1_first, it1_beyond, index1,
section1.directions[0], section2.bounding_box);
move_end_iterator<0>(it1_first, it1_beyond,
section1.directions[0], section2.bounding_box);
move_begin_iterator<1>(it1_first, it1_beyond, index1,
section1.directions[1], section2.bounding_box);
move_end_iterator<1>(it1_first, it1_beyond,
section1.directions[1], section2.bounding_box);
signed_size_type index2 = sec2_first_index;
move_begin_iterator<0>(it2_first, it2_beyond, index2,
section2.directions[0], section1.bounding_box);
move_end_iterator<0>(it2_first, it2_beyond,
section2.directions[0], section1.bounding_box);
move_begin_iterator<1>(it2_first, it2_beyond, index2,
section2.directions[1], section1.bounding_box);
move_end_iterator<1>(it2_first, it2_beyond,
section2.directions[1], section1.bounding_box);
turn_type the_model;
the_model.operations[0].piece_index = piece1.index;
the_model.operations[0].seg_id = piece1.first_seg_id;
the_model.operations[0].seg_id.segment_index = index1; // override
iterator it1 = it1_first;
for (iterator prev1 = it1++;
it1 != it1_beyond;
prev1 = it1++, the_model.operations[0].seg_id.segment_index++)
{
the_model.operations[1].piece_index = piece2.index;
the_model.operations[1].seg_id = piece2.first_seg_id;
the_model.operations[1].seg_id.segment_index = index2; // override
iterator next1 = next_point(ring1, it1);
iterator it2 = it2_first;
for (iterator prev2 = it2++;
it2 != it2_beyond;
prev2 = it2++, the_model.operations[1].seg_id.segment_index++)
{
iterator next2 = next_point(ring2, it2);
// TODO: internally get_turn_info calculates robust points.
// But they are already calculated.
// We should be able to use them.
// this means passing them to this visitor,
// and iterating in sync with them...
typedef detail::overlay::get_turn_info
<
#if defined(BOOST_GEOMETRY_BUFFER_USE_SIDE_OF_INTERSECTION)
buffer_assign_turn
#else
detail::overlay::assign_null_policy
#endif
> turn_policy;
turn_policy::apply(*prev1, *it1, *next1,
*prev2, *it2, *next2,
false, false, false, false,
the_model, m_robust_policy,
std::back_inserter(m_turns));
}
}
}
/*ARGSUSED*/
LOCAL int advance_front1d(
double dt,
double *dt_frac,
Front *front,
Front **newfront,
POINTER wave)
{
POINT *oldp, *newp;
HYPER_SURF_ELEMENT *oldhse, *newhse;
HYPER_SURF *oldhs, *newhs;
INTERFACE *intfc_old, *intfc_new;
int status;
double V[MAXD];
boolean has_tracked_points;
static const char *fname = "advance_front1d";
debug_print("front","Entered %s(step %d time %g dt %g)\n",fname,
front->step,front->time,dt);
debug_front("old_front","into advance front",front);
*newfront = copy_front(front);
Interface_redistributed(*newfront) = NO;
has_tracked_points = (front->interf->points != NULL) ? YES : NO;
if (pp_max_status(has_tracked_points) == NO)
{
set_size_of_intfc_state(size_of_state(front->interf));
set_copy_intfc_states(YES);
(*newfront)->interf = pp_copy_interface(front->interf);
status = ((*newfront)->interf != NULL) ? GOOD_STEP : ERROR_IN_STEP;
return return_advance_front(front,newfront,status,fname);
}
start_clock("propagate");
/* Initialize Newfront */
start_clock("init_new_front");
set_size_of_intfc_state(size_of_state(front->interf));
set_copy_intfc_states(NO);
set_add_to_correspond_list(YES);
(*newfront)->interf = pp_copy_interface(front->interf);
if ((*newfront)->interf == NULL)
{
(void) printf("ERROR in advance_front1d(), "
"unable to copy interface\n");
return return_advance_front(front,newfront,ERROR_IN_STEP,fname);
}
stop_clock("init_new_front");
/* Propagate the points */
set_propagation_limits(front,*newfront);
set_copy_intfc_states(YES);
intfc_old = front->interf;
intfc_new = (*newfront)->interf;
(void) next_point(intfc_old,NULL,NULL,NULL);
(void) next_point(intfc_new,NULL,NULL,NULL);
while (next_point(intfc_old,&oldp,&oldhse,&oldhs) &&
next_point(intfc_new,&newp,&newhse,&newhs))
{
point_propagate(front,wave,oldp,newp,oldhse,oldhs,dt,V);
}
copy_hypersurface_flags(intfc_new);
debug_front("pt_front","after point propagate",*newfront);
switch (redistribute(*newfront,YES,NO))
{
case GOOD_REDISTRIBUTION:
status = GOOD_STEP;
break;
case MODIFY_TIME_STEP_REDISTRIBUTE:
(void) printf("WARNING in advance_front1d(), redistribution "
"of front failed, reducing time step\n");
debug_front("ERROR_front","after error",*newfront);
*dt_frac = max(Min_time_step_modification_factor(front),*dt_frac);
status = MODIFY_TIME_STEP;
break;
case UNABLE_TO_UNTANGLE:
case BAD_REDISTRIBUTION:
default:
(void) printf("WARNING in advance_front1d(), "
"redistribution of front failed\n");
debug_front("ERROR_front","after error",*newfront);
*dt_frac = Min_time_step_modification_factor(front);
status = ERROR_IN_STEP;
break;
}
debug_front("redist_front","after redistribute",*newfront);
if (status != GOOD_STEP)
return return_advance_front(front,newfront,status,fname);
if (!scatter_front(*newfront))
{
(void) printf("WARNING in advance_front1d(), "
"scatter_front() failed for "
"normally propagated front\n");
return return_advance_front(front,newfront,ERROR_IN_STEP,fname);
}
(*newfront)->step = front->step + 1;
(*newfront)->time = front->time + dt;
interpolate_intfc_states(intfc_new) = YES;
set_size_of_intfc_state(size_of_state(intfc_new));
if (intfc_new->modified)
(void) make_point_comp_lists(intfc_new);
stop_clock("propagate");
debug_front("new_front","from advance front",*newfront);
return return_advance_front(front,newfront,status,fname);
} /*end advance_front1d*/
EXPORT void set_topological_grid(
INTERFACE *intfc,
RECT_GRID *input_grid)
{
enum { DEFAULT_GMAX = 20 };
HYPER_SURF *hs;
HYPER_SURF_ELEMENT *hse;
POINT *p;
RECT_GRID *top_grid = &topological_grid(intfc);
double *L, *U;
int dim = intfc->dim;
int i;
static int dgmax[3] = {DEFAULT_GMAX, DEFAULT_GMAX, DEFAULT_GMAX};
if (DEBUG)
(void) printf("\n\nEntered set_topological_grid()\n");
intfc->table->new_grid = YES;
if (input_grid != NULL)
{
copy_rect_grid(top_grid,input_grid);
intfc->table->fixed_grid = YES;
if (DEBUG)
(void) printf("Left set_topological_grid()\n\n");
return;
}
else
intfc->table->fixed_grid = NO;
/* Find Rectangle Containing INTERFACE: */
L = top_grid->L;
U = top_grid->U;
for (i = 0; i < dim; ++i)
{
L[i] = HUGE_VAL;
U[i] = -HUGE_VAL;
}
(void) next_point(intfc,NULL,NULL,NULL);
while (next_point(intfc,&p,&hse,&hs))
{
for (i = 0; i < dim; ++i)
{
L[i] = min(L[i],Coords(p)[i]);
U[i] = max(U[i],Coords(p)[i]);
}
}
if (DEBUG)
{
(void) printf("Rectsolid: ");
print_general_vector("L = ",L,dim,"");
print_general_vector("U = ",L,dim,"\n");
}
set_rect_grid(L,U,L,U,NOBUF,NOBUF,dgmax,dim,remap_info(),top_grid);
if (DEBUG)
(void) printf("Left set_topological_grid()\n\n");
return;
} /*end set_toplogical_grid*/
LOCAL void change_states_param(
Front *front,
Wave *wave,
int comp,
Gas_param *new_param)
{
HYPER_SURF *hs;
HYPER_SURF_ELEMENT *hse;
POINT *pt;
SURFACE **s;
Locstate stl, str, ref_st;
int i, d, gridmax, vgmax[MAXD], tmp, ic[MAXD];
FD_DATA *fd_data;
INTERFACE *intfc = front->interf;
int dim = intfc->dim;
printf("#change dirichlet boundary condition params.\n");
for(s=intfc->surfaces; s && *s; s++)
{
hs = Hyper_surf(*s);
if(wave_type(hs) != DIRICHLET_BOUNDARY)
continue;
if(boundary_state_data(hs) == NULL)
continue;
fd_data = (FD_DATA *)boundary_state_data(hs);
ref_st = fd_data->state;
if(gas_params_for_comp(comp, intfc) != Params(ref_st))
continue;
printf("change param for FD_DATA.\n");
verbose_print_state("bf ref", ref_st);
change_param(ref_st, comp, new_param);
verbose_print_state("af ref", ref_st);
}
printf("#change intfc params.\n");
next_point(intfc, NULL,NULL,NULL);
while (next_point(intfc,&pt,&hse,&hs))
{
slsr(pt,hse,hs,&stl,&str);
change_param(str,positive_component(hs),new_param);
change_param(stl,negative_component(hs),new_param);
if(the_point(pt))
{
printf("%d %d\n", negative_component(hs), positive_component(hs));
verbose_print_state("stl", stl);
verbose_print_state("str", str);
}
}
//check_print_intfc("After change intfc params", "ch_param", 'g',
// intfc, 1, -1, NO);
printf("#change interior params.\n");
gridmax = 1;
for (d = 0; d < dim; d++)
{
vgmax[d] = wave->rect_grid->gmax[d] + wave->rect_grid->lbuf[d]
+ wave->rect_grid->ubuf[d];
gridmax *= vgmax[d];
}
for (i = 0; i < gridmax; i++)
{
tmp = i;
for (d = 0; d < dim; d++)
{
ic[d] = tmp % vgmax[d] - wave->rect_grid->lbuf[d];
tmp /= vgmax[d];
}
change_param(Rect_state(ic,wave), Rect_comp(ic,wave), new_param);
}
}
LOCAL bool i_consistent_interface3d(
INTERFACE *intfc)
{
HYPER_SURF_ELEMENT *hse;
HYPER_SURF *hs;
CURVE **c;
NODE **n;
POINT *p;
SURFACE **ss, *s;
TRI *tri;
bool status = YES;
const char *warn = "WARNING in i_consistent_interface(), ";
/* Check Nodes */
for (n = intfc->nodes; n && *n; ++n)
{
if ((*n)->interface != intfc)
{
(void) printf("%s n = %llu n->interface (%llu) != intfc (%llu)\n",
warn,node_number(*n),
interface_number((*n)->interface),
interface_number(intfc));
status = NO;
}
for (c = (*n)->in_curves; c && *c; ++c)
{
if ((*c)->end != *n)
{
(void) printf("%s inconsistent node (%llu) "
"curve (%llu) pair, "
"curve->end != n\n",
warn,node_number(*n),curve_number(*c));
status = NO;
}
}
for (c = (*n)->out_curves; c && *c; ++c)
{
if ((*c)->start != *n)
{
(void) printf("%s inconsistent node (%llu) "
"curve (%llu) pair, "
"curve->start != n\n",
warn,node_number(*n),curve_number(*c));
status = NO;
}
}
}
/* Check Curves */
for (c = intfc->curves; c && *c; c++)
{
if (!check_curve3d(*c,intfc))
{
(void) printf("%s inconsistency in curve (%llu) found\n",
warn,curve_number(*c));
status = NO;
}
}
for (ss = intfc->surfaces; ss && *ss; ++ss)
{
if (!check_consistency_of_tris_on_surface(*ss))
{
(void) printf("%s inconsistency in surface (%llu) found\n",
warn,surface_number(*ss));
status = NO;
}
}
(void) next_point(intfc,NULL,NULL,NULL);
while (next_point(intfc,&p,&hse,&hs))
{
BOND *b = NULL, *bb;
BOND_TRI **bts;
CURVE **c;
TRI **tris;
int i, ntris;
int v, pside, nside;
tri = Tri_of_hse(hse);
s = Surface_of_hs(hs);
if ((v = Vertex_of_point(tri,p)) == ERROR)
{
(void) printf("%s point not on tri, s = %llu\n",
warn,surface_number(s));
(void) printf("p(%llu) - (%g, %g, %g), ",
point_number(p),
Coords(p)[0],Coords(p)[1],Coords(p)[2]);
print_tri(tri,hs->interface);
status = NO;
}
if (!Boundary_point(p))
{
ntris = set_tri_list_around_point(p,tri,&tris,intfc);
if ((tri != tris[0]) ||
(tri != Prev_tri_at_vertex(tris[ntris-1],p)))
{
bool consistent_tri_list = NO;
if (allow_null_sides)
{
if ((Next_tri_at_vertex(tris[0],p) == NULL) &&
(Prev_tri_at_vertex(tris[ntris-1],p) == NULL))
consistent_tri_list = YES;
}
if (!consistent_tri_list)
{
(void) printf("\n%s Corrupt tri list s (%llu) "
"p(%llu) - (%g, %g, %g)\n",
warn,surface_number(s),
point_number(p),
Coords(p)[0],Coords(p)[1],Coords(p)[2]);
print_tri(tri,hs->interface);
(void) printf("%d tris about point\n",ntris);
for (i = 0; i < ntris; ++i)
{
(void) printf("tris[%d] - ",i);
print_tri(tris[i],hs->interface);
}
(void) printf("End printout of "
"Corrupt tri list s (%llu) "
"p(%llu) - (%g, %g, %g)\n\n",
surface_number(s),point_number(p),
Coords(p)[0],Coords(p)[1],Coords(p)[2]);
status = NO;
}
}
continue;
}
nside = v;
pside = Prev_m3(v);
if (is_side_bdry(tri,nside))
b = Bond_on_side(tri,nside);
else if (is_side_bdry(tri,pside))
b = Bond_on_side(tri,pside);
else //#bjet2
{
ntris = set_tri_list_around_point(p,tri,&tris,intfc);
v = Vertex_of_point(tris[0],p);
nside = v;
pside = Prev_m3(v);
if (is_side_bdry(tris[0],nside))
b = Bond_on_side(tris[0],nside);
else if (is_side_bdry(tris[0],pside))
b = Bond_on_side(tris[0],pside);
else
{
int i;
(void) printf("%s Boundary_point has no adjacent "
"tri with a bond\n",warn);
(void) printf("p(%llu) - (%g, %g, %g), ",
point_number(p),
Coords(p)[0],Coords(p)[1],Coords(p)[2]);
print_tri(tri,hs->interface);
for (i = 0; i < ntris; ++i)
{
(void) printf("tris[%d] - ",i);
print_tri(tris[i],hs->interface);
}
status = NO;
}
tri = tris[0];
}
for (bts = Btris(b); bts && *bts; ++bts)
if ((*bts)->tri == tri)
break;
if ((bts == NULL) || (*bts == NULL))
{
(void) printf("%s bond tri for tri not found\n",warn);
(void) printf("p(%llu) - (%g, %g, %g), ",point_number(p),
Coords(p)[0],Coords(p)[1],Coords(p)[2]);
print_tri(tri,hs->interface);
print_bond(b);
status = NO;
}
else
{
if ((*bts)->bond != b)
{
(void) printf("%s (*bts)->bond != b\n",warn);
(void) printf("p(%llu) - (%g, %g, %g), ",point_number(p),
Coords(p)[0],Coords(p)[1],Coords(p)[2]);
print_tri(tri,hs->interface);
print_bond(b);
status = NO;
}
if ((*bts)->surface != s)
{
(void) printf("%s inconsistent surfaces at bond tri\n",
warn);
(void) printf("p(%llu) - (%g, %g, %g), ",point_number(p),
Coords(p)[0],Coords(p)[1],Coords(p)[2]);
print_tri(tri,hs->interface);
print_bond(b);
status = NO;
}
if (orientation_of_bond_at_tri(b,tri) != (*bts)->orient)
{
(void) printf("%s inconsistent orientation at bond tri\n",
warn);
(void) printf("p(%llu) - (%g, %g, %g), ",point_number(p),
Coords(p)[0],Coords(p)[1],Coords(p)[2]);
print_tri(tri,hs->interface);
print_bond(b);
status = NO;
}
switch ((*bts)->orient)
{
case POSITIVE_ORIENTATION:
for (c = s->pos_curves; c && *c; c++)
if ((*c) == (*bts)->curve)
break;
break;
case NEGATIVE_ORIENTATION:
for (c = s->neg_curves; c && *c; c++)
if ((*c) == (*bts)->curve)
break;
break;
case ORIENTATION_NOT_SET:
c = NULL;
(void) printf("%s undetermined orientation at "
"bond on tri\n",warn);
(void) printf("p(%llu) - (%g, %g, %g), ",point_number(p),
Coords(p)[0],Coords(p)[1],Coords(p)[2]);
print_tri(tri,hs->interface);
print_bond(b);
status = NO;
break;
}
if ((c == NULL) || (*c == NULL))
{
(void) printf("%s curve with bond on tri not found\n",warn);
(void) printf("p(%llu) - (%g, %g, %g), ",point_number(p),
Coords(p)[0],Coords(p)[1],Coords(p)[2]);
print_tri(tri,hs->interface);
print_bond(b);
status = NO;
}
else
{
for (bb = (*c)->first; bb != NULL; bb = bb->next)
if (bb == b)
break;
if (bb == NULL)
{
(void) printf("%s bond not on curve\n",warn);
(void) printf("p(%llu) - (%g, %g, %g), ",point_number(p),
Coords(p)[0],Coords(p)[1],Coords(p)[2]);
print_tri(tri,hs->interface);
print_bond(b);
print_curve(*c);
status = NO;
}
}
}
}
if (status == NO)
{
(void) printf("WARNING in i_consistent_interface(), "
"Inconsistent interface found\n");
print_interface(intfc);
}
allow_null_sides = NO;
return status;
} /*end i_consistent_interface*/
/*-
* draw_swirl
*
* Draw one iteration of swirling
*
* - win is the window to draw in
*/
void
draw_swirl(ModeInfo * mi)
{
swirlstruct *sp;
if (swirls == NULL)
return;
sp = &(swirls[MI_SCREEN(mi)]);
if (sp->knots == NULL)
return;
MI_IS_DRAWN(mi) = True;
/* are we going? */
if (sp->started) {
/* in the middle of drawing? */
if (sp->drawing) {
if(sp->cycle_p) {
rotate_colors(MI_DISPLAY(mi), sp->cmap, sp->colors, sp->ncolors, sp->direction);
if (!(LRAND() % 1000))
sp->direction = -sp->direction;
}
/* draw a batch of points */
sp->batch_todo = BATCH_DRAW;
while ((sp->batch_todo > 0) && sp->drawing) {
/* draw a point */
draw_point(mi, sp);
/* move to the next point */
next_point(sp);
/* done a point */
sp->batch_todo--;
}
} else {
if(sp->cycle_p) {
rotate_colors(MI_DISPLAY(mi), sp->cmap, sp->colors, sp->ncolors, sp->direction);
if (!(LRAND() % 1000))
sp->direction = -sp->direction;
}
/* time for a higher resolution? */
if (sp->resolution > sp->max_resolution) {
/* move to higher resolution */
sp->resolution--;
/* calculate the pixel step for this resulution */
sp->r = (1 << (sp->resolution - 1));
/* start drawing again */
sp->drawing = True;
/* start in the middle of the screen */
sp->x = (sp->width - sp->r) / 2;
sp->y = (sp->height - sp->r) / 2;
/* initialise spiral drawing parameters */
sp->dir = DRAW_RIGHT;
sp->dir_todo = 1;
sp->dir_done = 0;
} else {
/* all done, decide when to restart */
if (sp->start_again == -1) {
/* start the counter */
sp->start_again = RESTART;
} else if (sp->start_again == 0) {
/* reset the counter */
sp->start_again = -1;
/* start again */
init_swirl(mi);
} else
/* decrement the counter */
sp->start_again--;
}
}
}
}
EXPORT INTERFACE *remap_interface(
INTERFACE *intfc,
void (*remap)(POINT*,BOND*,CURVE*,POINT*,
BOND*,CURVE*,boolean,RECT_GRID*,POINTER),
void (*remap_rect_grid)(INTERFACE*,INTERFACE*,
void (*)(POINT*,BOND*,CURVE*,
POINT*,BOND*,CURVE*,boolean,
RECT_GRID*,POINTER),
POINTER),
POINTER params)
{
HYPER_SURF *hs, *nhs;
HYPER_SURF_ELEMENT *hse, *nhse;
POINT *p, *np;
INTERFACE *new_intfc;
RECT_GRID *rgr;
boolean first = YES;
double eps;
if (DEBUG)
(void) printf("Entering remap_interface(%llu)\n",
(long long unsigned int)interface_number(intfc));
if (remap == NULL || remap_rect_grid == NULL)
return NULL;
if (exists_interface(intfc) != YES)
return NULL;
if ((new_intfc = copy_interface(intfc)) == NULL)
return NULL;
rgr = &topological_grid(new_intfc);
/* TODO: check for validity at hyper surface boundaries */
(void) next_point(intfc,NULL,NULL,NULL);
(void) next_point(new_intfc,NULL,NULL,NULL);
while (next_point(intfc,&p,&hse,&hs) &&
next_point(new_intfc,&np,&nhse,&nhs))
{
(*remap)(p,Bond_of_hse(hse),Curve_of_hs(hs),np,
Bond_of_hse(nhse),Curve_of_hs(nhs),first,rgr,params);
first = NO;
}
/* Remap the Underlying Grid: */
(*remap_rect_grid)(intfc,new_intfc,remap,params);
/* Reset boundary flags */
eps = grid_tolerance(rgr);
if (new_intfc->dim == 1)
{
POINT **pt;
for (pt = new_intfc->points; pt && *pt; pt++)
{
if (boundary_side(Coords(*pt),rgr,eps) == NOT_A_BDRY)
set_not_bdry(*pt);
else
set_is_bdry(*pt);
}
}
if (new_intfc->dim == 2)
{
CURVE **c;
NODE **n;
for (n = new_intfc->nodes; n && *n; n++)
{
if (boundary_side(Coords((*n)->posn),rgr,eps) == NOT_A_BDRY)
set_not_bdry(*n);
else
set_is_bdry(*n);
}
for (c = new_intfc->curves; c && *c; c++)
{
BDRY_SIDE side_start, side_end, side;
BOND *b;
NODE *ns, *ne;
ns = (*c)->start;
ne = (*c)->end;
side_start = boundary_side(Coords(ns->posn),rgr,eps);
side_end = boundary_side(Coords(ne->posn),rgr,eps);
if ((side_start != side_end) || (side_start == NOT_A_BDRY))
{
set_not_bdry(*c);
continue;
}
side = side_start;
for (b = (*c)->first; b != (*c)->last; b = b->next)
{
if (boundary_side(Coords(b->end),rgr,eps) != side)
{
side = NOT_A_BDRY;
break;
}
}
if (side == NOT_A_BDRY)
set_not_bdry(*c);
else
set_is_bdry(*c);
}
}
if (new_intfc->dim == 3)
{
CURVE **c;
NODE **n;
SURFACE **s;
for (n = new_intfc->nodes; n && *n; n++)
{
if (boundary_side(Coords((*n)->posn),rgr,eps) == NOT_A_BDRY)
set_not_bdry(*n);
else
set_is_bdry(*n);
}
for (c = new_intfc->curves; c && *c; c++)
{
BDRY_SIDE side_start, side_end, side;
BOND *b;
NODE *ns, *ne;
ns = (*c)->start;
ne = (*c)->end;
side_start = boundary_side(Coords(ns->posn),rgr,eps);
side_end = boundary_side(Coords(ne->posn),rgr,eps);
if ((side_start != side_end) || (side_start == NOT_A_BDRY))
{
set_not_bdry(*c);
continue;
}
side = side_start;
for (b = (*c)->first; b != (*c)->last; b = b->next)
{
if (boundary_side(Coords(b->end),rgr,eps) != side)
{
side = NOT_A_BDRY;
break;
}
}
if (side == NOT_A_BDRY)
set_not_bdry(*c);
else
set_is_bdry(*c);
}
for (s = new_intfc->surfaces; s && *s; s++)
{
TRI *t;
BDRY_SIDE side0, side1, side2, side;
const double *p0, *p1, *p2;
t = first_tri(*s);
p0 = Coords(Point_of_tri(t)[0]);
p1 = Coords(Point_of_tri(t)[1]);
p2 = Coords(Point_of_tri(t)[2]);
side0 = boundary_side(p0,rgr,eps);
side1 = boundary_side(p1,rgr,eps);
if ((side0 != side1) || (side0 == NOT_A_BDRY))
{
set_not_bdry(*s);
continue;
}
side = side0;
side2 = boundary_side(p2,rgr,eps);
if (side2 != side0)
{
set_not_bdry(*s);
continue;
}
for (t = t->next; !at_end_of_tri_list(t,*s); t = t->next)
{
p0 = Coords(Point_of_tri(t)[0]);
p1 = Coords(Point_of_tri(t)[1]);
p2 = Coords(Point_of_tri(t)[2]);
if ((boundary_side(p0,rgr,eps) != side) ||
(boundary_side(p1,rgr,eps) != side) ||
(boundary_side(p2,rgr,eps) != side))
{
side = NOT_A_BDRY;
break;
}
}
if (side == NOT_A_BDRY)
set_not_bdry(*s);
else
set_is_bdry(*s);
}
}
if (DEBUG) (void) printf("Leaving remap_interface()\n\n");
return new_intfc;
} /*end remap_interface*/
bool TargetedMovementGenerator<T>::Update(T &owner, const uint32 & time_diff)
{
if (!i_target.isValid() || !i_target->IsInWorld())
return false;
if (!&owner || !owner.isAlive())
return true;
if (owner.hasUnitState(UNIT_STAT_ROOT | UNIT_STAT_STUNNED | UNIT_STAT_FLEEING | UNIT_STAT_DISTRACTED))
return true;
// prevent movement while casting spells with cast time or channel time
if (owner.hasUnitState(UNIT_STAT_CASTING))
{
if (!owner.IsStopped())
owner.StopMoving();
return true;
}
// prevent crash after creature killed pet
if (!owner.hasUnitState(UNIT_STAT_FOLLOW) && owner.getVictim() != i_target.getTarget())
return true;
if (i_path && (i_path->getPathType() & PATHFIND_NOPATH))
return true;
Traveller<T> traveller(owner);
if (!i_destinationHolder.HasDestination())
_setTargetLocation(owner);
if (i_destinationHolder.UpdateTraveller(traveller, time_diff, i_recalculateTravel || owner.IsStopped()))
{
// put targeted movement generators on a higher priority
if (owner.GetObjectSize())
i_destinationHolder.ResetUpdate(100);
float dist = owner.GetCombatReach() + sWorld.getRate(RATE_TARGET_POS_RECALCULATION_RANGE);
float x,y,z;
i_target->GetPosition(x, y, z);
PathNode next_point(x, y, z);
bool targetMoved = false, needNewDest = false;
if (i_path)
{
PathNode end_point = i_path->getEndPosition();
next_point = i_path->getNextPosition();
needNewDest = i_destinationHolder.HasArrived() && !inRange(next_point, i_path->getActualEndPosition(), dist, dist);
// GetClosePoint() will always return a point on the ground, so we need to
// handle the difference in elevation when the creature is flying
if (owner.GetTypeId() == TYPEID_UNIT && owner.ToCreature()->canFly())
targetMoved = i_target->GetDistanceSqr(end_point.x, end_point.y, end_point.z) >= dist * dist;
else
targetMoved = i_target->GetDistance2d(end_point.x, end_point.y) >= dist;
}
// target moved
if (!i_path || targetMoved || needNewDest || i_recalculateTravel || owner.IsStopped())
{
// (re)calculate path
_setTargetLocation(owner);
next_point = i_path->getNextPosition();
// Set new Angle For Map::
owner.SetOrientation(owner.GetAngle(next_point.x, next_point.y));
}
// Update the Angle of the target only for Map::, no need to send packet for player
else if (!i_angle && !owner.HasInArc(0.01f, next_point.x, next_point.y))
owner.SetOrientation(owner.GetAngle(next_point.x, next_point.y));
if ((owner.IsStopped() && !i_destinationHolder.HasArrived()) || i_recalculateTravel)
{
i_recalculateTravel = false;
//Angle update will take place into owner.StopMoving()
owner.SetOrientation(owner.GetAngle(next_point.x, next_point.y));
owner.StopMoving();
if (owner.IsWithinMeleeRange(i_target.getTarget()) && !owner.hasUnitState(UNIT_STAT_FOLLOW))
owner.Attack(i_target.getTarget(), true);
}
}
// Implemented for PetAI to handle resetting flags when pet owner reached
if (i_destinationHolder.HasArrived())
MovementInform(owner);
return true;
}
bool
SSLPathPlanner::doPlanForRobot (const int& id/*, bool& is_stop*/)
{
// is_send_plan = true;
tmp_robot_id_queried = id;
State* tmp_start = manifold->allocState ();
tmp_start->as<RealVectorStateManifold::StateType> ()->values[0] = team_state_[id].pose.x;
tmp_start->as<RealVectorStateManifold::StateType> ()->values[1] = team_state_[id].pose.y;
PathGeometric direct_path (planner_setup->getSpaceInformation ());
direct_path.states.push_back (manifold->allocState ());
manifold->copyState (direct_path.states[0], tmp_start);
State* tmp_goal = manifold->allocState ();
tmp_goal->as<RealVectorStateManifold::StateType> ()->values[0] = pose_control.pose[id].pose.x;
tmp_goal->as<RealVectorStateManifold::StateType> ()->values[1] = pose_control.pose[id].pose.y;
// std::cout<<pose_control.pose[id].pose.x<<"\t"<<pose_control.pose[id].pose.y<<std::endl;
direct_path.states.push_back (manifold->allocState ());
manifold->copyState (direct_path.states[1], tmp_goal);
Point_2 p_start (team_state_[id].pose.x, team_state_[id].pose.y);
Point_2 p_goal (pose_control.pose[id].pose.x, pose_control.pose[id].pose.y);
//too close to the target point, no need for planning, just take it as a next_target_pose
if (sqrt (getSquaredDistance (p_start, p_goal)) < VERY_CRITICAL_DIST)
{
next_target_poses[id].x = pose_control.pose[id].pose.x;
next_target_poses[id].y = pose_control.pose[id].pose.y;
next_target_poses[id].theta = pose_control.pose[id].pose.theta;
return true;
}
// manifold->printState (direct_path.states[0], std::cout);
// manifold->printState (direct_path.states[1], std::cout);
//direct path is available, no need for planning
// if (direct_path.check ())
if (!doesIntersectObstacles (id, p_start, p_goal))
{
// std::cout << "direct path is AVAILABLE for robot " << id << std::endl;
if (solution_data_for_robots[id].size () > direct_path.states.size ())
{
for (uint32_t i = direct_path.states.size (); i < solution_data_for_robots[id].size (); i++)
manifold->freeState (solution_data_for_robots[id][i]);
solution_data_for_robots[id].resize (direct_path.states.size ());
}
else if (solution_data_for_robots[id].size () < direct_path.states.size ())
{
for (uint32_t i = solution_data_for_robots[id].size (); i < direct_path.states.size (); i++)
solution_data_for_robots[id].push_back (manifold->allocState ());
}
for (uint32_t i = 0; i < direct_path.states.size (); i++)
manifold->copyState (solution_data_for_robots[id][i], direct_path.states[i]);
manifold->freeState (tmp_start);
manifold->freeState (tmp_goal);
next_target_poses[id].x = pose_control.pose[id].pose.x;
next_target_poses[id].y = pose_control.pose[id].pose.y;
next_target_poses[id].theta = pose_control.pose[id].pose.theta;
// std::cout<<next_target_poses[id].x<<"\t"<<next_target_poses[id].y<<std::endl;
return true;
}
manifold->freeState (tmp_start);
manifold->freeState (tmp_goal);
// std::cout << "direct path is NOT AVAILABLE for robot " << id << ", doing planning" << std::endl;
//direct path is not available, DO PLAN if the earlier plan is invalidated!
//earlier plan is still valid
if (checkPlanForRobot (id))
return true;
else if (is_plan_done[id])
return false;
//earlier plan is not valid anymore, replan
planner_setup->clear ();
planner_setup->clearStartStates ();
ScopedState<RealVectorStateManifold> start_state (manifold);
ScopedState<RealVectorStateManifold> goal_state (manifold);
start_state->values[0] = team_state_[id].pose.x;
start_state->values[1] = team_state_[id].pose.y;
goal_state->values[0] = pose_control.pose[id].pose.x;
goal_state->values[1] = pose_control.pose[id].pose.y;
planner_setup->setStartAndGoalStates (start_state, goal_state);
planner_setup->getProblemDefinition ()->fixInvalidInputStates (ssl::config::ROBOT_BOUNDING_RADIUS / 2.0,
ssl::config::ROBOT_BOUNDING_RADIUS / 2.0, 100);
bool solved = planner_setup->solve (0.100);//100msec
if (solved)
{
planner_setup->simplifySolution ();
PathGeometric* path;
path = &planner_setup->getSolutionPath ();
// PathSimplifier p (planner_setup->getSpaceInformation ());
// p.reduceVertices (*path, 1000);
if (solution_data_for_robots[id].size () < path->states.size ())
{
for (unsigned int i = solution_data_for_robots[id].size (); i < path->states.size (); i++)
solution_data_for_robots[id].push_back (manifold->allocState ());
}
else if (solution_data_for_robots[id].size () > path->states.size ())
{
for (unsigned int i = path->states.size (); i < solution_data_for_robots[id].size (); i++)
manifold->freeState (solution_data_for_robots[id][i]);
//drop last elements that are already being freed
solution_data_for_robots[id].resize (path->states.size ());
}
for (unsigned int i = 0; i < path->states.size (); i++)
manifold->copyState (solution_data_for_robots[id][i], path->states[i]);
//leader-follower approach based segment enlargement
//pick next location
Point_2 curr_point (path->states[0]->as<RealVectorStateManifold::StateType> ()->values[0], path->states[0]->as<
RealVectorStateManifold::StateType> ()->values[1]);
for (uint32_t i = 1; i < path->states.size (); i++)
{
Point_2 next_point (path->states[i]->as<RealVectorStateManifold::StateType> ()->values[0], path->states[i]->as<
RealVectorStateManifold::StateType> ()->values[1]);
if (!doesIntersectObstacles (id, curr_point, next_point))
{
next_target_poses[id].x = path->states[i]->as<RealVectorStateManifold::StateType> ()->values[0];
next_target_poses[id].y = path->states[i]->as<RealVectorStateManifold::StateType> ()->values[1];
next_target_poses[id].theta = pose_control.pose[id].pose.theta;
}
else
break;
}
// next_target_poses[id].x = path->states[1]->as<RealVectorStateManifold::StateType> ()->values[0];
// next_target_poses[id].y = path->states[1]->as<RealVectorStateManifold::StateType> ()->values[1];
// planner_data_for_robots[id].clear ();
// planner_data_for_robots[id] = planner_setup->getPlannerData ();
}
return solved;
}
bool TargetedMovementGeneratorMedium<T,D>::Update(T &owner, const uint32 & time_diff)
{
if (!i_target.isValid() || !i_target->IsInWorld())
return false;
if (!owner.isAlive())
return true;
if (owner.hasUnitState(UNIT_STAT_NOT_MOVE))
{
D::_clearUnitStateMove(owner);
return true;
}
// prevent movement while casting spells with cast time or channel time
if (owner.IsNonMeleeSpellCasted(false, false, true))
{
if (!owner.IsStopped())
owner.StopMoving();
return true;
}
// prevent crash after creature killed pet
if (static_cast<D*>(this)->_lostTarget(owner))
{
D::_clearUnitStateMove(owner);
return true;
}
if (i_path && (i_path->getPathType() & PATHFIND_NOPATH))
return true;
Traveller<T> traveller(owner);
if (!i_destinationHolder.HasDestination())
_setTargetLocation(owner);
if (owner.IsStopped() && !i_destinationHolder.HasArrived())
{
D::_addUnitStateMove(owner);
if (owner.GetTypeId() == TYPEID_UNIT && ((Creature*)&owner)->CanFly())
((Creature&)owner).AddSplineFlag(SPLINEFLAG_FLYING);
i_destinationHolder.StartTravel(traveller);
return true;
}
if (i_destinationHolder.UpdateTraveller(traveller, time_diff, i_recalculateTravel || owner.IsStopped()))
{
if (!IsActive(owner)) // force stop processing (movement can move out active zone with cleanup movegens list)
return true; // not expire now, but already lost
// put targeted movement generators on a higher priority
if (owner.GetObjectBoundingRadius())
i_destinationHolder.ResetUpdate(100);
//More distance let have better performance, less distance let have more sensitive reaction at target move.
float dist = i_target->GetObjectBoundingRadius() + owner.GetObjectBoundingRadius() + sWorld.getConfig(CONFIG_FLOAT_RATE_TARGET_POS_RECALCULATION_RANGE);
float x,y,z;
i_target->GetPosition(x, y, z);
PathNode next_point(x, y, z);
bool targetMoved = false, needNewDest = false;
if (i_path)
{
PathNode end_point = i_path->getEndPosition();
next_point = i_path->getNextPosition();
needNewDest = i_destinationHolder.HasArrived() && !inRange(next_point, i_path->getActualEndPosition(), dist, dist);
// GetClosePoint() will always return a point on the ground, so we need to
// handle the difference in elevation when the creature is flying
if (owner.GetTypeId() == TYPEID_UNIT && ((Creature*)&owner)->CanFly())
targetMoved = i_target->GetDistanceSqr(end_point.x, end_point.y, end_point.z) >= dist*dist;
else
targetMoved = i_target->GetDistance2d(end_point.x, end_point.y) >= dist;
}
if (!i_path || targetMoved || needNewDest || i_recalculateTravel || owner.IsStopped())
{
// (re)calculate path
_setTargetLocation(owner);
next_point = i_path->getNextPosition();
// Set new Angle For Map::
owner.SetOrientation(owner.GetAngle(next_point.x, next_point.y));
}
// Update the Angle of the target only for Map::, no need to send packet for player
else if (!i_angle && !owner.HasInArc(0.01f, next_point.x, next_point.y))
owner.SetOrientation(owner.GetAngle(next_point.x, next_point.y));
if ((owner.IsStopped() && !i_destinationHolder.HasArrived()) || i_recalculateTravel)
{
i_recalculateTravel = false;
//Angle update will take place into owner.StopMoving()
owner.SetOrientation(owner.GetAngle(next_point.x, next_point.y));
owner.StopMoving();
static_cast<D*>(this)->_reachTarget(owner);
}
}
return true;
}
bool TargetedMovementGenerator<T>::Update(T &owner, const uint32 & time_diff)
{
if (!i_target.isValid() || !i_target->IsInWorld())
return false;
if (!&owner || !owner.isAlive())
return true;
if (owner.hasUnitState(UNIT_STAT_ROOT | UNIT_STAT_STUNNED | UNIT_STAT_FLEEING | UNIT_STAT_DISTRACTED))
return true;
// prevent movement while casting spells with cast time or channel time
if (owner.IsNonMeleeSpellCasted(false, false, true) || owner.hasUnitState(UNIT_STAT_CASTING))
{
if (!owner.IsStopped())
owner.StopMoving();
return true;
}
if (!owner.HasAuraType(SPELL_AURA_MOD_INVISIBILITY) && !owner.canSeeOrDetect(i_target.getTarget(), true))
{
owner.AttackStop();
if (owner.GetOwner())
owner.GetMotionMaster()->MoveFollow(owner.GetOwner(), PET_FOLLOW_DIST, owner.GetFollowAngle());
else
owner.StopMoving();
return true;
}
// prevent crash after creature killed pet
if (!owner.hasUnitState(UNIT_STAT_FOLLOW) && owner.getVictim() != i_target.getTarget())
return true;
if (m_usePathfinding)
{
if (i_path && (i_path->getPathType() & PATHFIND_NOPATH))
return true;
Traveller<T> traveller(owner);
if (!i_destinationHolder.HasDestination())
_setTargetLocation(owner);
if (i_destinationHolder.UpdateTraveller(traveller, time_diff, i_recalculateTravel || owner.IsStopped()))
{
// put targeted movement generators on a higher priority
if (owner.GetCombatReach())
i_destinationHolder.ResetUpdate(100);
float x,y,z;
i_target->GetPosition(x, y, z);
PathNode next_point(x, y, z);
bool targetMoved = false, needNewDest = false;
bool forceRecalc = i_recalculateTravel || owner.IsStopped();
if (i_path && !forceRecalc)
{
PathNode end_point = i_path->getEndPosition();
next_point = i_path->getNextPosition();
//More distance let have better performance, less distance let have more sensitive reaction at target move.
float dist = owner.GetCombatReach() + sWorld.getRate(RATE_TARGET_POS_RECALCULATION_RANGE);
needNewDest = i_destinationHolder.HasArrived() && (!i_path->inRange(next_point, i_path->getActualEndPosition(), dist, dist) || !owner.IsWithinLOSInMap(i_target.getTarget()));
if(!needNewDest)
{
// GetClosePoint() will always return a point on the ground, so we need to
// handle the difference in elevation when the creature is flying
if (owner.GetTypeId() == TYPEID_UNIT && ((Creature*)&owner)->canFly())
targetMoved = i_target->GetDistanceSqr(end_point.x, end_point.y, end_point.z) > dist*dist;
else
targetMoved = i_target->GetDistance2d(end_point.x, end_point.y) > dist;
}
}
// target moved
if (!i_path || targetMoved || needNewDest || forceRecalc)
{
// (re)calculate path
_setTargetLocation(owner);
next_point = i_path->getNextPosition();
// Set new Angle For Map::
owner.SetOrientation(owner.GetAngle(next_point.x, next_point.y));
}
// Update the Angle of the target only for Map::, no need to send packet for player
else if (!i_angle && !owner.HasInArc(0.01f, next_point.x, next_point.y))
owner.SetOrientation(owner.GetAngle(next_point.x, next_point.y));
if ((owner.IsStopped() && !i_destinationHolder.HasArrived()) || i_recalculateTravel)
{
i_recalculateTravel = false;
//Angle update will take place into owner.StopMoving()
owner.SetInFront(i_target.getTarget());
owner.StopMoving();
if(owner.IsWithinMeleeRange(i_target.getTarget()) && !owner.hasUnitState(UNIT_STAT_FOLLOW))
owner.Attack(i_target.getTarget(), true);
}
}
}
else
{
Traveller<T> traveller(owner);
if (!i_destinationHolder.HasDestination())
{
_setTargetLocation(owner);
}
else if (owner.IsStopped() && !i_destinationHolder.HasArrived())
{
owner.addUnitState(UNIT_STAT_CHASE);
i_destinationHolder.StartTravel(traveller);
return true;
}
if (i_destinationHolder.UpdateTraveller(traveller, time_diff))
{
// target moved
if (i_targetX != i_target->GetPositionX() || i_targetY != i_target->GetPositionY()
|| i_targetZ != i_target->GetPositionZ())
{
if(_setTargetLocation(owner) || !owner.hasUnitState(UNIT_STAT_FOLLOW))
owner.SetInFront(i_target.getTarget());
i_target->GetPosition(i_targetX, i_targetY, i_targetZ);
}
if ((owner.IsStopped() && !i_destinationHolder.HasArrived()) || i_recalculateTravel)
{
i_recalculateTravel = false;
//Angle update will take place into owner.StopMoving()
owner.SetInFront(i_target.getTarget());
owner.StopMoving();
if(owner.IsWithinMeleeRange(i_target.getTarget()) && !owner.hasUnitState(UNIT_STAT_FOLLOW))
owner.Attack(i_target.getTarget(),true);
}
}
}
// Implemented for PetAI to handle resetting flags when pet owner reached
if (i_destinationHolder.HasArrived())
MovementInform(owner);
return true;
}
LOCAL void f_second_order_intfc_propagate3d(
Front *fr,
POINTER wave,
INTERFACE *old_intfc,
INTERFACE *new_intfc,
double dt)
{
INTERFACE *tmp_intfc;
HYPER_SURF *oldhs, *tmphs, *newhs;
HYPER_SURF_ELEMENT *oldhse, *tmphse, *newhse;
POINT *oldp, *tmpp, *newp;
int i;
double V[MAXD];
printf("Entering f_second_order_intfc_propagate3d()\n");
set_copy_intfc_states(NO);
tmp_intfc = pp_copy_interface(fr->interf);
(void) next_point(old_intfc,NULL,NULL,NULL);
(void) next_point(tmp_intfc,NULL,NULL,NULL);
while (next_point(old_intfc,&oldp,&oldhse,&oldhs) &&
next_point(tmp_intfc,&tmpp,&tmphse,&tmphs))
{
point_propagate(fr,wave,oldp,tmpp,oldhse,oldhs,dt,V);
}
(void) next_point(tmp_intfc,NULL,NULL,NULL);
(void) next_point(new_intfc,NULL,NULL,NULL);
while (next_point(tmp_intfc,&tmpp,&tmphse,&tmphs) &&
next_point(new_intfc,&newp,&newhse,&newhs))
{
point_propagate(fr,wave,tmpp,newp,tmphse,tmphs,dt,V);
}
(void) next_point(old_intfc,NULL,NULL,NULL);
(void) next_point(tmp_intfc,NULL,NULL,NULL);
(void) next_point(new_intfc,NULL,NULL,NULL);
while (next_point(old_intfc,&oldp,&oldhse,&oldhs) &&
next_point(tmp_intfc,&tmpp,&tmphse,&tmphs) &&
next_point(new_intfc,&newp,&newhse,&newhs))
{
for (i = 0; i < 3; ++i)
Coords(newp)[i] = Coords(oldp)[i] + 0.5*(oldp->vel[i] +
tmpp->vel[i])*dt;
}
delete_interface(tmp_intfc);
printf("Leaving f_second_order_intfc_propagate3d()\n");
} /* end f_second_order_intfc_propagate3d */
