void GridMap::set_cell_item(int p_x, int p_y, int p_z, int p_item, int p_rot) {
ERR_FAIL_INDEX(ABS(p_x), 1 << 20);
ERR_FAIL_INDEX(ABS(p_y), 1 << 20);
ERR_FAIL_INDEX(ABS(p_z), 1 << 20);
IndexKey key;
key.x = p_x;
key.y = p_y;
key.z = p_z;
OctantKey ok;
ok.x = p_x / octant_size;
ok.y = p_y / octant_size;
ok.z = p_z / octant_size;
ok.area = _find_area(key);
if (cell_map.has(key)) {
int prev_item = cell_map[key].item;
OctantKey octantkey = ok;
ERR_FAIL_COND(!octant_map.has(octantkey));
Octant &g = *octant_map[octantkey];
ERR_FAIL_COND(!g.items.has(prev_item));
ERR_FAIL_COND(!g.items[prev_item].cells.has(key));
g.items[prev_item].cells.erase(key);
if (g.items[prev_item].cells.size() == 0) {
VS::get_singleton()->free(g.items[prev_item].multimesh_instance);
g.items.erase(prev_item);
}
if (g.items.empty()) {
PhysicsServer::get_singleton()->free(g.static_body);
if (g.collision_debug.is_valid()) {
PhysicsServer::get_singleton()->free(g.collision_debug);
PhysicsServer::get_singleton()->free(g.collision_debug_instance);
}
memdelete(&g);
octant_map.erase(octantkey);
} else {
g.dirty = true;
}
cell_map.erase(key);
_queue_dirty_map();
}
if (p_item < 0)
return;
OctantKey octantkey = ok;
//add later
if (!octant_map.has(octantkey)) {
Octant *g = memnew(Octant);
g->dirty = true;
g->static_body = PhysicsServer::get_singleton()->body_create(PhysicsServer::BODY_MODE_STATIC);
PhysicsServer::get_singleton()->body_attach_object_instance_ID(g->static_body, get_instance_ID());
if (is_inside_world())
PhysicsServer::get_singleton()->body_set_space(g->static_body, get_world()->get_space());
SceneTree *st = SceneTree::get_singleton();
if (st && st->is_debugging_collisions_hint()) {
g->collision_debug = VisualServer::get_singleton()->mesh_create();
g->collision_debug_instance = VisualServer::get_singleton()->instance_create();
VisualServer::get_singleton()->instance_set_base(g->collision_debug_instance, g->collision_debug);
if (is_inside_world()) {
VisualServer::get_singleton()->instance_set_scenario(g->collision_debug_instance, get_world()->get_scenario());
VisualServer::get_singleton()->instance_set_transform(g->collision_debug_instance, get_global_transform());
}
}
octant_map[octantkey] = g;
}
Octant &g = *octant_map[octantkey];
if (!g.items.has(p_item)) {
Octant::ItemInstances ii;
if (theme.is_valid() && theme->has_item(p_item)) {
ii.mesh = theme->get_item_mesh(p_item);
ii.shape = theme->get_item_shape(p_item);
ii.navmesh = theme->get_item_navmesh(p_item);
}
ii.multimesh = Ref<MultiMesh>(memnew(MultiMesh));
ii.multimesh->set_color_format(MultiMesh::COLOR_NONE);
ii.multimesh->set_transform_format(MultiMesh::TRANSFORM_3D);
ii.multimesh->set_mesh(ii.mesh);
ii.multimesh_instance = VS::get_singleton()->instance_create();
VS::get_singleton()->instance_set_base(ii.multimesh_instance, ii.multimesh->get_rid());
g.items[p_item] = ii;
}
Octant::ItemInstances &ii = g.items[p_item];
ii.cells.insert(key);
g.dirty = true;
_queue_dirty_map();
cell_map[key] = Cell();
Cell &c = cell_map[key];
c.item = p_item;
c.rot = p_rot;
}
float get_cost(struct argu_table *table)
{
int i;
int mark[2];
float cost = 0.0;
float tmp_x;
float tmp_y;
struct node_info nInfo;
struct node_result nRes;
int x1 = table->x1;
int x2 = table->x2;
int y1 = table->y1;
int y2 = table->y2;
printf("test %d\n", test);
for(i = 0; i < NODE_MAX; i++){
nRes.node_x[i] = 0.0;
nRes.node_y[i] = 0.0;
}
nRes.node_count = 0;
if(_find_area(table, mark) == -1)
return -1;
nInfo.x1 = table->x1;
nInfo.y1 = table->y1;
nInfo.m = _slope(table);
printf("Slope : %f\n", nInfo.m);
// Point 1 area == Point 2 area
if(mark[0] == mark[1]){
switch(mark[0]){
case RED:
cost = __distance(x1, y1, x2, y2) * R_cost;
break;
case YELLOW: /* case: Y->Y or Y->R->Y */
// check with Red-Y-LB(y = 10), find X
nInfo.bound = R_y_L;
tmp_x = _chk_node(table, &nInfo, "R-Y-LB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = R_y_L; //node(x, 10)
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_x, "R-Y-LB");
}
// check with Red-Y-UB(y = 40), find X
nInfo.bound = R_y_H;
tmp_x = _chk_node(table, &nInfo, "R-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = R_y_H; //node(x, 40)
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_x, "R-Y-UB");
}
// check with Red-X-LB(x = 10), find y
nInfo.bound = R_x_L;
tmp_y = _chk_node(table, &nInfo, "R-X-LB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_L;
nRes.node_y[nRes.node_count] = tmp_y; //node(10, y)
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_y, "R-X-LB");
}
// check with Red-X-UB(x = 30), find y
nInfo.bound = R_x_H;
tmp_y = _chk_node(table, &nInfo, "R-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_H;
nRes.node_y[nRes.node_count] = tmp_y;
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_y, "R-X-UB");
}
printf("Total %d nodes\n", nRes.node_count);
// means Yellow->Yellow, no other area crossed
if(nRes.node_count == 0){
cost = __distance(x1, y1, x2, y2);
// means Yellow->Red->Yellow
}else if(nRes.node_count == 2){
//add the cost of red area
cost += _get_dist_in_area(&nRes, 1, 2);
}else{
printf("Wrong !!! In bost of Yellow case, only 0/2 node is possiable !! ");
printf("But there are %d nodes\n", nRes.node_count);
return -1;
}
break;
case GREEN: /* CASE: G->G or G->Y->G or G->Y->R->G */
// check with Yellow-Y-UB(y = 60), find x
nInfo.bound = Y_y_H;
tmp_x = _chk_node(table, &nInfo, "Y-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = Y_y_H; //node(x, 60)
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_x, "Y-Y-UB");
}
//check with Yellow-X-UB(x = 50), find y
nInfo.bound = Y_x_H;
tmp_y = _chk_node(table, &nInfo, "Y-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = Y_x_H;
nRes.node_y[nRes.node_count] = tmp_y;
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_y, "Y-X-UB");
}
// check with Red-Y-UB(y = 40), find x
nInfo.bound = R_y_H;
tmp_x = _chk_node(table, &nInfo, "R-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = R_y_H; //node(x, 40)
nRes.node_count++;
/*
* means G->Y->R->G case,
* the Red-Y-UB must link with Yellow-Y-UB
* in node_result array,
* array 0 stored node with Yellow-Y-UB,
* array 2 stored node with Red-Y-UB.
*/
cost += _get_dist_in_area(&nRes, 1, 3);
}
nInfo.bound = R_x_H;
tmp_y = _chk_node(table, &nInfo, "R-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_H;
nRes.node_y[nRes.node_count] = tmp_y;
nRes.node_count++;
/*
* means G->Y->R->G case,
* the Red-X-UB must link with Yellow-X-UB
* in node_result array,
* array 1 stored node with Yellow-X-UB,
* array 3 stored node with Red-X-UB.
*/
cost += _get_dist_in_area(&nRes, 2, 4);
}
printf("Total %d nodes\n", nRes.node_count);
// means Green->Green, no other area crossed
if(nRes.node_count == 0){
cost = __distance(x1, y1, x2, y2);
// means Green->Yellow->Green
}else if(nRes.node_count == 2){
cost += _get_dist_in_area(&nRes, 1, 2);
// means Green->Yellow->Red->Green
}else if(nRes.node_count == 4){
// the Red area cost
cost += _get_dist_in_area(&nRes, 3, 4);
}else{
printf("Wrong !!! In both of Green case, ");
printf(" only 0/2/4 node is possible ! ");
printf("But there are %d nodes\n", nRes.node_count);
return -1;
}
break;
default:
printf("Impossible region!\n");
exit(0);
}
/* Point1 = R, Point2 = Y or Point1 = Y, Point2 = R */
}else if(mark[0] != GREEN && mark[1] != GREEN &&
mark[0] != mark[1]){
/* Only one case, R<->Y */
// check with Red-Y-LB(y = 10), find X
nInfo.bound = R_y_L;
tmp_x = _chk_node(table, &nInfo, "R-Y-LB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_x[nRes.node_count] = R_y_L; //node(x, 10)
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_x, "R-Y-LB");
}
// check with Red-Y-UB(y = 40), find X
nInfo.bound = R_y_H;
tmp_x = _chk_node(table, &nInfo, "R-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = R_y_H; //node(x, 40)
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_x, "R-Y-UB");
}
// check with Red-X-LB(x = 10), find y
nInfo.bound = R_x_L;
tmp_y = _chk_node(table, &nInfo, "R-X-LB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_L;
nRes.node_y[nRes.node_count] = tmp_y; //node(10, y)
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_y, "R-X-LB");
}
// check with Red-X-UB(x = 30), find y
nInfo.bound = R_x_H;
tmp_y = _chk_node(table, &nInfo, "R-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_H;
nRes.node_y[nRes.node_count] = tmp_y; //node(30, y)
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_y, "R-X-UB");
}
printf("Total %d nodes\n", nRes.node_count);
// In this case, must have one corss node!
if(nRes.node_count == 1){
if(mark[0] == RED){
cost += __distance(nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1], x1, y1);
}else if(mark[1] == RED){
cost += __distance(nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1], x2, y2);
}else{
printf("Error! in R<->Y case ");
printf("one of mark must be RED!\n");
return -1;
}
}else{
printf("Error! in R<->Y case, must have 1 cross node!\n");
return -1;
}
/* Point1 = R, Point2 = G */
}else if(mark[0] == RED && mark[1] == GREEN){
// only R->Y->G case
// First, check Red boundary
// check with Red-Y-LB(y = 10), find X
nInfo.bound = R_y_L;
tmp_x = _chk_node(table, &nInfo, "R-Y-LB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_x[nRes.node_count] = R_y_L; //node(x, 10)
nRes.node_count++;
cost += __distance(x1, y1, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}
// check with Red-Y-UB(y = 40), find X
nInfo.bound = R_y_H;
tmp_x = _chk_node(table, &nInfo, "R-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = R_y_H; //node(x, 40)
nRes.node_count++;
cost += __distance(x1, y1, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}
// check with Red-X-LB(x = 10), find y
nInfo.bound = R_x_L;
tmp_y = _chk_node(table, &nInfo, "R-X-LB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_L;
nRes.node_y[nRes.node_count] = tmp_y; //node(10, y)
nRes.node_count++;
cost += __distance(x1, y1, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}
// check with Red-X-UB(x = 30), find y
nInfo.bound = R_x_H;
tmp_y = _chk_node(table, &nInfo, "R-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_H;
nRes.node_y[nRes.node_count] = tmp_y; //node(30, y)
nRes.node_count++;
cost += __distance(x1, y1, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}
// find the node cross with Yellow
// check with Yellow-Y-UB(y = 60), find x
nInfo.bound = Y_y_H;
tmp_x = _chk_node(table, &nInfo, "Y-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = Y_y_H; //node(x, 60)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, 1, 2);
}
//check with Yellow-X-UB(x = 50), find y
nInfo.bound = Y_x_H;
tmp_y = _chk_node(table, &nInfo, "Y-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = Y_x_H;
nRes.node_y[nRes.node_count] = tmp_y; //node(50, y)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, 1, 2);
}
// in R->Y->G case, must have 2 node
printf("Total %d node\n", nRes.node_count);
if(nRes.node_count == 2){
cost += __distance(x2, y2, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}else{
printf("In R->Y->G case, there are 2 corss node, but only %d node\n", nRes.node_count);
return -1;
}
/* point1 = G, point2 = R */
}else if(mark[0] == GREEN && mark[1] == RED){
// only G->Y->R case
// First check Yellow boundary
// check with Yellow-Y-UB(y = 60), find x
nInfo.bound = Y_y_H;
tmp_x = _chk_node(table, &nInfo, "Y-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = Y_y_H; //node(x, 60)
nRes.node_count++;
cost += __distance(x1, y1, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}
// check with Yellow-X-UB(x = 50), find y
nInfo.bound = Y_x_H;
tmp_y = _chk_node(table, &nInfo, "Y-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = Y_x_H;
nRes.node_y[nRes.node_count] = tmp_y; //node(50, y)
nRes.node_count++;
cost += __distance(x1, y1, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}
// Secondly, check with Red bound
// check with Red-Y-LB(y = 10), find X
nInfo.bound = R_y_L;
tmp_x = _chk_node(table, &nInfo, "R-Y-LB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_x[nRes.node_count] = R_y_L; //node(x, 10)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1 ,nRes.node_count);
}
// check with Red-Y-UB(y = 40), find X
nInfo.bound = R_y_H;
tmp_x = _chk_node(table, &nInfo, "R-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = R_y_H; //node(x, 40)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
// check with Red-X-LB(x = 10), find y
nInfo.bound = R_x_L;
tmp_y = _chk_node(table, &nInfo, "R-X-LB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_L;
nRes.node_y[nRes.node_count] = tmp_y; //node(10, y)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
// check with Red-X-UB(x = 30), find y
nInfo.bound = R_x_H;
tmp_y = _chk_node(table, &nInfo, "R-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_H;
nRes.node_y[nRes.node_count] = tmp_y; //node(30, y)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
// in G->Y->R case , must have 2 node
printf("Total %d node\n", nRes.node_count);
if(nRes.node_count == 2){
cost += __distance(x2, y2, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}else{
printf("In G->Y->R case, there are 2 cross node, but only %d node\n", nRes.node_count);
return -1;
}
/* point1 = G, point2 = Y */
}else if(mark[0] == GREEN && mark[1] == YELLOW){
// G->Y->R->Y & G->Y case!
// First, check with Yellow boundary
/* !!! <Check Upper bound FIRST> !!!! */
// check with Yellow-Y-UB(y = 60), find x
nInfo.bound = Y_y_H;
tmp_x = _chk_node(table, &nInfo, "Y-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = Y_y_H; //node(x, 60)
nRes.node_count++;
cost += __distance(x1, y1, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}
// check with Yellow-X-UB(x = 50), find y
nInfo.bound = Y_x_H;
tmp_y = _chk_node(table, &nInfo, "Y-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = Y_x_H;
nRes.node_y[nRes.node_count] = tmp_y; //node(50, y)
nRes.node_count++;
cost += __distance(x1, y1, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}
// Secondly, check with Red boundary
// check with Red-Y-UB(y = 40), find X
nInfo.bound = R_y_H;
tmp_x = _chk_node(table, &nInfo, "R-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = R_y_H; //node(x, 40)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
// check with Red-X-UB(x = 30), find y
nInfo.bound = R_x_H;
tmp_y = _chk_node(table, &nInfo, "R-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_H;
nRes.node_y[nRes.node_count] = tmp_y; //node(30, y)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
// check with Red-Y-LB(y = 10), find X
nInfo.bound = R_y_L;
tmp_x = _chk_node(table, &nInfo, "R-Y-LB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_x[nRes.node_count] = R_y_L; //node(x, 10)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1 ,nRes.node_count);
}
// check with Red-X-LB(x = 10), find y
nInfo.bound = R_x_L;
tmp_y = _chk_node(table, &nInfo, "R-X-LB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_L;
nRes.node_y[nRes.node_count] = tmp_y; //node(10, y)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
printf("Total %d node\n", nRes.node_count);
// must have 1 or 3 node
if(nRes.node_count == 3 || nRes.node_count == 1){
cost += __distance(x2, y2, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}else{
printf("In G->Y/G->Y->R->Y case, must have 1 or 3 node!\n");
return -1;
}
// point1 = Y, point2 = G
}else if(mark[0] == YELLOW && mark[1] == GREEN){
// First, check with Red boundary
/* !!! <Check Lower bound FIRST> !!! */
// check with Red-Y-LB(y = 10), find X
nInfo.bound = R_y_L;
tmp_x = _chk_node(table, &nInfo, "R-Y-LB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_x[nRes.node_count] = R_y_L; //node(x, 10)
nRes.node_count++;
// first node need to calculate cost with endpoint
if(nRes.node_count == 1){
cost += __distance(x1, y1,
nRes.node_x[nRes.node_count-1], nRes.node_y[nRes.node_count-1]);
}else{
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
}
// check with Red-X-LB(x = 10), find y
nInfo.bound = R_x_L;
tmp_y = _chk_node(table, &nInfo, "R-X-LB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_L;
nRes.node_y[nRes.node_count] = tmp_y; //node(10, y)
nRes.node_count++;
// first node need to calculate cost with endpoint
if(nRes.node_count == 1){
cost += __distance(x1, y1,
nRes.node_x[nRes.node_count-1], nRes.node_y[nRes.node_count-1]);
}else{
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
}
// check with Red-Y-UB(y = 40), find x
nInfo.bound = R_y_H;
tmp_x = _chk_node(table, &nInfo, "R-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = R_y_H; //node(x, 40)
nRes.node_count++;
// first node need to calculate cost with endpoint
if(nRes.node_count == 1){
cost += __distance(x1, y1,
nRes.node_x[nRes.node_count-1], nRes.node_y[nRes.node_count-1]);
}else{
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
}
// check with Red-X-UB(x = 30), find y
nInfo.bound = R_x_H;
tmp_y = _chk_node(table, &nInfo, "R-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_H;
nRes.node_y[nRes.node_count] = tmp_y; //node(30, y)
nRes.node_count++;
// first node need to calculate cost with endpoint
if(nRes.node_count == 1){
cost += __distance(x1, y1,
nRes.node_x[nRes.node_count-1], nRes.node_y[nRes.node_count-1]);
}else{
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
}
// Secondly, check Yellow boundary
// check with Yellow-Y-UB(y = 60), find x
nInfo.bound = Y_y_H;
tmp_x = _chk_node(table, &nInfo, "Y-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = Y_y_H; //node(x, 60)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
// check with Yellow-X-UB(x = 50), find y
nInfo.bound = Y_x_H;
tmp_y = _chk_node(table, &nInfo, "Y-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = Y_x_H;
nRes.node_y[nRes.node_count] = tmp_y; //node(50, y)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
printf("Total %d node\n", nRes.node_count);
// must have 1 or 3 node
if(nRes.node_count == 3 || nRes.node_count == 1){
cost += __distance(x2, y2, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}else{
printf("In Y->G/Y->R->Y->G case, must have 1 or 3 node!\n");
return -1;
}
}else{
printf("F****n other case ???? F**K!!!\n");
return -1;
}
return cost;
}