static inline int64_t verify(TupleGraph tg, GlobalAddress<G> g, int64_t root) {
VerificatorBase<G>::verify(tg,g,root);
// SSSP distances verification
forall(tg.edges, tg.nedge, [=](TupleGraph::Edge& e){
auto i = e.v0, j = e.v1;
/* Eliminate self loops from verification */
if ( i == j)
return;
/* SSSP specific checks */
auto ti = VerificatorBase<G>::get_parent(g,i), tj = VerificatorBase<G>::get_parent(g,j);
auto di = get_dist(g,i), dj = get_dist(g,j);
auto wij = get_edge_weight(g,i,j), wji = get_edge_weight(g,j,i);
CHECK(!((di < dj) && ((di + wij) < dj))) << "Error, distance of the nearest neighbor is too great :"
<< "(" << i << "," << di << ")" << "--" << wij << "-->" << "(" << j << "," << dj << ")" ;
CHECK(!((dj < di) && ((dj + wji) < di))) << "Error, distance of the nearest neighbor is too great : "
<< "(" << j << "," << dj << ")" << "--" << wji << "-->" << "(" << i << "," << di << ")" ;
CHECK(!((i == tj) && ((di + wij) != dj))) << "Error, distance of the child vertex is not equil to "
<< "sum of its parent distance and edge weight :"
<< "(" << i << "," << di << ")" << "--" << wij << "-->" << "(" << j << "," << dj << ")" ;
CHECK(!((j == ti) && ((dj + wji) != di))) << "Error, distance of the child vertex is not equil to "
<< "sum of its parent distance and edge weight :"
<< "(" << j << "," << dj << ")" << "--" << wji << "-->" << "(" << i << "," << di << ")" ;
});
// everything checked out!
VLOG(1) << "SSSP verified!\n";
return nedge_traversed;
}
void GridObject::save_anchors(clan::DomElement &e, clan::GUIComponent *comp)
{
clan::Rect boundary = grid_component->get_dialog_size();
clan::GUIComponent *tab_or_frame_parent = get_tab_or_frame_parent(comp);
if (tab_or_frame_parent)
{
boundary = tab_or_frame_parent->get_geometry().get_size();
}
clan::Rect g = get_geometry();
clan::Point tl = g.get_top_left();
clan::Point br = g.get_bottom_right();
clan::Point dist_tl = get_dist(anchor_tl, tl, boundary);
clan::Point dist_br = get_dist(anchor_br, br, boundary);
e.set_attribute("anchor_tl", clan::StringHelp::int_to_text(anchor_tl));
e.set_attribute("anchor_br", clan::StringHelp::int_to_text(anchor_br));
e.set_attribute("dist_tl_x", clan::StringHelp::int_to_text(dist_tl.x));
e.set_attribute("dist_tl_y", clan::StringHelp::int_to_text(dist_tl.y));
e.set_attribute("dist_br_x", clan::StringHelp::int_to_text(dist_br.x));
e.set_attribute("dist_br_y", clan::StringHelp::int_to_text(dist_br.y));
}
double calc_area(float lat, float lon, float cellsize)
{
double cell_area;
float start_lat,right_lng,left_lng;
double delta;
int d;
double dist;
lat = fabs(lat);
lon = fabs(lon);
start_lat = lat - cellsize / 2.;
right_lng = lon + cellsize / 2;
left_lng = lon - cellsize / 2;
delta = get_dist(lat,lon,lat+cellsize/10.,lon);
dist = 0.;
for ( d = 0; d < 10; d++ ) {
dist += get_dist(start_lat,left_lng,start_lat,right_lng) * delta;
start_lat += cellsize/10;
}
cell_area = dist;
return(cell_area);
}
// sorts the important vectors in closest to farthest order
void knn::sort_important( row * r )
{
int i, number_switched=1; // get into loop first time
double distance1, distance2;
double * temp_row;
// implement MERGE SORT... as many as needed times
//if we loop through the whole thing without having to switch any it is sorted
while( number_switched != 0 )
{
number_switched = 0; // initialize howmany switched first
for( i=0; i<(_IMPORTANT_SIZE-1); i++)
{
distance1 = get_dist( r->get_row_data(), _important[i] );
// get distance1
distance2 = get_dist( r->get_row_data(), _important[i+1]);
// get distance2
// if distance2 is greater then distance1 then switch the rows
if( distance2 < distance1 )
{
temp_row = _important[i];
_important[i] = _important[i+1];
_important[i+1] = temp_row;
number_switched ++ ;
}
}
// end of while loop
}
}
bool check(Point a, Point b) {
for (int i = 0; i < 4; ++ i) {
Point p = points[i];
if ((p == a) || (p == b)) {
continue;
}
double d1 = get_dist(a, b);
double d2 = get_dist(p, b);
double d3 = get_dist(p, a);
double alpha = acos((d1 * d1 + d2 * d2 - d3 * d3) / (2 * d1 * d2));
Point q = rotate(p, b, 2 * alpha);
if (sgn(dot(p - q, a - b)) != 0) {
q = rotate(p, b, -2 * alpha);
}
bool flag = false;
for (int i = 0; i < 4; ++ i) {
if (q == points[i]) {
flag = true;
break;
}
}
if (!flag) {
return false;
}
}
return true;
}
void simulate(t_x11 *x11,t_xhighway *xhw,
int ncars,t_car cars[],t_input *ir)
{
int i,n_bef,n_bef1,n_beh;
float dist,distf,distb;
for(i=0; (i<ncars); i++) {
cars[i].xold=cars[i].x;
cars[i].oldlane=cars[i].lane;
}
for(i=0; (i<ncars); i++) {
cars[i].bBrake=FALSE;
dist=get_dist(ncars,cars,i,xhw->bFog,
1,cars[i].lane,ir->metres,&n_bef);
if (dist < ir->min_dist) {
distf=get_dist(ncars,cars,i,xhw->bFog,
1,cars[i].lane+1,ir->metres,&n_bef1);
distb=get_dist(ncars,cars,i,xhw->bFog,
-1,cars[i].lane+1,ir->metres,&n_beh);
if ((cars[i].lane < ir->nlane-1) && (distb >= ir->min_dist) &&
(distf >= ir->min_dist))
cars[i].lane += 1;
else {
/* Use brakes */
cars[i].v -= cars[i].brake*ir->dt;
if (cars[i].v < 0)
cars[i].v = 0;
if (n_bef != -1)
if ((cars[i].v < cars[n_bef].v) && (dist > ir->min_dist/2))
cars[i].v=cars[n_bef].v;
cars[i].bBrake=TRUE;
}
}
else if ((cars[i].lane > 0) && (cars[i].v == cars[i].vwanted)) {
/* Check if I can go right again */
dist=get_dist(ncars,cars,i,xhw->bFog,
1,cars[i].lane-1,ir->metres,&n_bef);
distb=get_dist(ncars,cars,i,xhw->bFog,
-1,cars[i].lane-1,ir->metres,&n_beh);
if ((dist >= ir->min_dist) && (distb >= ir->min_dist))
cars[i].lane -= 1;
}
cars[i].x += cars[i].v*ir->dt;
if (cars[i].x > ir->metres)
cars[i].x -= ir->metres;
if (!cars[i].bBrake && (cars[i].v < cars[i].vwanted)) {
cars[i].v += cars[i].acc*ir->dt;
if (cars[i].v > cars[i].vwanted)
cars[i].v = cars[i].vwanted;
}
}
/* Detect Crashes */
/* Plot */
usleep(xhw->ir.sleep);
ExposeWin(x11->disp,xhw->win.self);
}
INLINE double
get_distortion_offset(
const distortion_lookup_t * const lookup,
const double * const img /*[NAXES]*/) {
double dist[NAXES];
double dist_floor[NAXES];
int dist_ifloor[NAXES];
double dist_weight[NAXES];
double dist_iweight[NAXES];
double result;
const unsigned int* naxis = lookup->naxis;
const float* data = lookup->data;
unsigned int i;
assert(lookup != NULL);
assert(img != NULL);
image_coords_to_distortion_coords(lookup, img, dist);
for (i = 0; i < NAXES; ++i) {
dist_floor[i] = floor(dist[i]);
dist_ifloor[i] = (int)dist_floor[i];
dist_weight[i] = dist[i] - dist_floor[i];
dist_iweight[i] = 1.0 - dist_weight[i];
}
/* If we may need to clamp the lookups, use this slower approach */
if (dist_ifloor[0] < 0 ||
dist_ifloor[1] < 0 ||
dist_ifloor[0] >= (long)lookup->naxis[0] - 1 ||
dist_ifloor[1] >= (long)lookup->naxis[1] - 1) {
result =
(double)get_dist_clamp(data, naxis, dist_ifloor[0], dist_ifloor[1]) * dist_iweight[0] * dist_iweight[1] +
(double)get_dist_clamp(data, naxis, dist_ifloor[0], dist_ifloor[1] + 1) * dist_iweight[0] * dist_weight[1] +
(double)get_dist_clamp(data, naxis, dist_ifloor[0] + 1, dist_ifloor[1]) * dist_weight[0] * dist_iweight[1] +
(double)get_dist_clamp(data, naxis, dist_ifloor[0] + 1, dist_ifloor[1] + 1) * dist_weight[0] * dist_weight[1];
/* Else, we don't need to clamp 4 times for each pixel */
} else {
result =
(double)get_dist(data, naxis, dist_ifloor[0], dist_ifloor[1]) * dist_iweight[0] * dist_iweight[1] +
(double)get_dist(data, naxis, dist_ifloor[0], dist_ifloor[1] + 1) * dist_iweight[0] * dist_weight[1] +
(double)get_dist(data, naxis, dist_ifloor[0] + 1, dist_ifloor[1]) * dist_weight[0] * dist_iweight[1] +
(double)get_dist(data, naxis, dist_ifloor[0] + 1, dist_ifloor[1] + 1) * dist_weight[0] * dist_weight[1];
}
return result;
}
static double is_collision(t_ray *ray, t_data_sphere *data,
t_obj *obj, t_rt *rt)
{
double ret;
t_pos u;
t_pos v;
t_pos e;
double tmp;
ret = -1.;
if (rt && ray && data)
{
u = ray->rd;
v = pos_vector(obj->sp.o, ray->ro);
e.x = pos_dot_product(u, u);
e.y = 2. * pos_dot_product(u, v);
e.z = pos_dot_product(v, v) - pow(data->radius, 2);
if ((tmp = delta(e)) < 0.)
ret = -1.;
else if (tmp == 0.)
ret = (-1. * e.y) / (2. * e.x);
else
ret = get_dist(e, tmp);
}
return (ret);
}
void draw_map(t_wf *game)
{
int x;
int color;
x = 0;
while (x < game->len_m)
{
game = init_camera(game, x);
game = get_dist(game);
game = detect_wall(game);
game = get_wall_length(game);
game->height_m = fabs((int)(game->map_h / game->wall_length));
game->draw_init = -game->height_m / 2 + game->map_h / 2;
if (game->draw_init < 0)
game->draw_init = 0;
game->draw_end = game->height_m / 2 + game->map_h / 2;
if (game->draw_end >= game->map_h)
game->draw_end = game->map_h - 1;
color = ft_color_tab(game);
if (game->w_side == 1)
color /= 2;
draw_line(x, game, color);
x++;
}
return ;
}
static int* calc_bank_map(world_skeleton_t *s, world_message_t *m, map_t *map)
{
int* bank_gravity = calloc(sizeof(int), map->num_nodes);
bank_value_line_t** nearest_bank = calloc(sizeof(bank_value_line_t **), map->num_nodes);
bank_value_line_t* bv;
int i;
for(i=0; i<map->num_nodes; i++)
{
bank_gravity[i] = INFTY;
for (bv = m->bank_values; bv != 0; bv = bv->next)
{
int bank_dist = get_dist(map, CHOOSE_FOOT, i, bv->node->index);
if(bank_dist < bank_gravity[i])
{
bank_gravity[i] = bank_dist;
nearest_bank[i] = bv;
}
}
bank_gravity[i]++;
int tmp = bank_gravity[i]*bank_gravity[i];
bank_gravity[i] = (nearest_bank[i]->value / tmp) / SCALE;
}
return bank_gravity;
}
void int_quad(t_inter *pt, void *e, t_ray ray, t_light *light)
{
t_quad quad;
double t;
t_vec inter;
quad = *((t_quad *)e);
t = get_dist(ray, quad);
if (t < 0)
return ;
if (pt->dist == NULL || *(pt->dist) > t)
{
if (pt->dist == NULL)
pt->dist = malloc(sizeof(double));
free_info(pt);
*(pt->dist) = t;
inter = get_inter(ray, t);
pt->normal = get_normal_quad(inter, quad);
if (scalar_prod(pt->normal, ray.dir) > 0)
pt->normal = mult_scalar(pt->normal, -1);
pt->refl = get_refl(ray, pt->normal);
pt->color = quad.color;
pt->inter = inter;
get_info(pt, light, inter);
}
}
void ai_eval_hexes( int x, int y, int range, int(*eval_func)(int,int,void*), void *ctx )
{
List *list = list_create( LIST_NO_AUTO_DELETE, LIST_NO_CALLBACK );
AI_Pos *pos;
int i, nx, ny;
/* gather and handle all positions by breitensuche.
use AUX mask to mark visited positions */
map_clear_mask( F_AUX );
list_add( list, ai_create_pos( x, y ) ); mask[x][y].aux = 1;
list_reset( list );
while ( list->count > 0 ) {
pos = list_dequeue( list );
if ( !eval_func( pos->x, pos->y, ctx ) ) {
free( pos );
break;
}
for ( i = 0; i < 6; i++ )
if ( get_close_hex_pos( pos->x, pos->y, i, &nx, &ny ) )
if ( !mask[nx][ny].aux && get_dist( x, y, nx, ny ) <= range ) {
list_add( list, ai_create_pos( nx, ny ) );
mask[nx][ny].aux = 1;
}
free( pos );
}
list_delete( list );
}
static int* calc_cop_map(world_skeleton_t *s, world_message_t *m, map_t *map)
{
int* gravity = calloc(sizeof(int), map->num_nodes);
int i;
int k;
for(i=0; i<map->num_nodes; i++)
{
gravity[i] = 0;
for (k = 0; k < NUM_COPS; ++k)
{
ptype_t cop_type;
node_line_t *cop_node;
int cop_dist;
cop_node = player_node(m, map, s->cops[k], &cop_type);
//cop_dist = get_combined_dist(map, (cop_type == PTYPE_COP_FOOT) ? CHOOSE_FOOT : CHOOSE_CAR,
// cop_node->index, i);
// ALERT
cop_dist = get_dist(map, (cop_type == PTYPE_COP_FOOT) ? CHOOSE_FOOT : CHOOSE_CAR,
cop_node->index, i);
cop_dist++;
gravity[i] += COPWORTH / (cop_dist*cop_dist) / SCALE;
}
}
return gravity;
}
static int* calc_score_map(world_skeleton_t *s, world_message_t *m, map_t *map, brain_t* brain)
{
int* score_map = calloc(sizeof(int), map->num_nodes);
int* bank_map = calc_bank_map(s, m, map);
int* cop_map = calc_cop_map(s, m, map);
char* poss_map = calc_possible_map(s, m, map, brain);
int i;
int away;
for(i=0; i<map->num_nodes; i++)
{
//away = get_combined_dist(map, CHOOSE_FROM_PTYPE(brain->my_ptype),
// brain->my_pos, i);
//ALERT
away = get_dist(map, CHOOSE_FROM_PTYPE(brain->my_ptype),
brain->my_pos, i);
score_map[i] = (SCORE_MAGIC1*poss_map[i]);
score_map[i] *= poss_map[i];
score_map[i] = (score_map[i]*bank_map[i]/(1+cop_map[i])) / (away+1);
}
free(bank_map);
free(cop_map);
free(poss_map);
return score_map;
}
long Breakpoint::overlap(Breakpoint * tmp) {
bool flag = false;
//flag = ((*tmp->get_coordinates().support.begin()).second.SV & DEL);
int max_dist = std::min(get_dist(tmp),get_dist(this)); // Parameter::Instance()->max_dist
if (flag) {
std::cout << "\t Overlap: " << max_dist << " start: " << tmp->get_coordinates().start.min_pos << " " << positions.start.min_pos << " stop :" << tmp->get_coordinates().stop.max_pos << " " << positions.stop.max_pos;
if ((*positions.support.begin()).second.SV & DEL) {
std::cout << " Is DEL";
} else if ((*positions.support.begin()).second.SV & INS) {
std::cout << " Is Ins ";
} else if ((*positions.support.begin()).second.SV & DUP) {
std::cout << " Is Dup ";
} else if ((*positions.support.begin()).second.SV & INV) {
std::cout << " Is Inv ";
}
std::cout << " Support: " << positions.support.size();
std::cout << std::endl;
}
//merging two robust calls:
if (is_same_strand(tmp) && (abs(tmp->get_coordinates().start.min_pos - positions.start.min_pos) < max_dist && abs(tmp->get_coordinates().stop.max_pos - positions.stop.max_pos) < max_dist)) {
if (flag) {
cout << "\tHIT" << endl;
}
return 0;
}
//extend Split read by noisy region: //not longer needed??
/* if (((tmp->get_types().is_Noise || this->get_types().is_Noise) && !(tmp->get_types().is_Noise && this->get_types().is_Noise))
&& (abs(tmp->get_coordinates().start.min_pos - positions.stop.min_pos) < max_dist / 2 || abs(tmp->get_coordinates().stop.max_pos - positions.start.max_pos) < max_dist / 2)) { //TODO maybe add SV type check!
if (flag) {
cout << "\tHIT Noise" << endl;
}
return 0;
}*/
//as abstraction lets try the start+stop coordinate!
long diff = (tmp->get_coordinates().start.min_pos - positions.start.min_pos);
//if (abs(diff) < max_dist) {
//return (tmp->get_coordinates().stop.max_pos - positions.stop.max_pos);
//}
if (diff == 0) {
return 1;
}
return diff; // + (tmp->get_coordinates().stop.max_pos - positions.stop.max_pos);
}
void tsp()
{
int i, j;
for (i = 0; i < COUNT_P; i++) {
printf("get_dist(p[%d]) = %lf\n", i, get_dist(p[i]));
if (get_dist(p[i]) < best_dist) {
for (j = 0; j < N; j++) {
result[j] = p[i][j];
}
map(p[i]);
test++;
break;
}
}
if (i < COUNT_P)
tsp();
else {
return;
}
}
int sh_quad(void *e, t_ray ray, double dist)
{
t_quad quad;
double t;
quad = *((t_quad *)e);
t = get_dist(ray, quad);
if (t < 0 || t > dist - 0.001)
return (0);
return (1);
}
int* calc_stinker_map(world_skeleton_t* s, world_message_t* m, map_t* map, brain_t* brain, int weite)
{
int* sm = calloc(sizeof(int), map->num_nodes);
int i = 0;
for(i=0; i<map->num_nodes; i++)
{
sm[i] = (get_dist(map, CHOOSE_FROM_PTYPE(brain->my_ptype), brain->my_pos, i)==weite);
}
return sm;
}
static char* calc_possible_map(world_skeleton_t *s, world_message_t *m, map_t *map, brain_t* brain)
{
char* poss_map = calloc(sizeof(int), map->num_nodes);
int i;
int count = 0;
for(i=0; i<brain->num_last; i++)
{
//int* dists = get_all_dists(map, CHOOSE_FOOT, brain->last_pos[i]);
int j;
// assert(sddhd);
for(j=0; j<map->num_nodes; j++)
{
if(ein_cop_is_here(s, m, map, j))
{
poss_map[j] = 0;
}
else
{
int dist = get_dist(map, CHOOSE_FOOT, j, brain->last_pos[i]);
if(dist < brain->rounds_ago)
{
count++;
// ALERT
poss_map[j] = THE_PROP * (SCORE_MAGIC2/(1+dist));
}
}
}
}
// PASS 2
count=0;
int dynamic = calc_dynamic_cutoff(s, m, map, brain);
for(i=0; i<map->num_nodes; i++)
{
if(poss_map[i] < dynamic)
{
poss_map[i] = 0;
}
else
{
count++;
}
}
return poss_map;
}
int main(void)
{
HCSR04LP sensor;
sensor.trigger_pin_number = TRIGGER_PIN;
sensor.echo_pin_number = ECHO_PIN;
sensor.power_pin_number = POWER_PIN;
hcsr04_init(&sensor);
turn_on(&sensor);
nrf_delay_us(1000000);
volatile double result = 0.0;
result = get_dist(&sensor);
turn_off(&sensor);
return (int)result;
}
Pilot get_next(Pilot first, Pilot second)
{
int i, dist, num_pilots = 2 * m, max_dist = -TRACK_SEGMENTS;
Pilot p, next = NULL;
for (i = 0; i < num_pilots; i++) {
p = pilots[i];
if (!p->is_finished && p != first && p != second) {
dist = get_dist(p);
if (dist > max_dist) {
next = p;
max_dist = dist;
}
else if (dist == max_dist && rand() % 2)
next = p;
}
}
return next;
}
void init()
{
srand((unsigned)time(NULL));
int i, j;
for (i = 0; i < N; i++){
point[i].x = rand()%100;
point[i].y = rand()%100;
printf("(%lf,%lf)\n", point[i].x, point[i].y);
}
for (i = 0; i < N; i++) {
for (j = 0; j < N; j++) {
if (i == j)
continue;
distance[i][j] = sqrt(pow((point[i].x - point[j].x), 2) + pow((point[i].y - point[j].y), 2));
printf("distance[%d][%d] = %lf ", i, j, distance[i][j]);
}
printf("\n");
}
for (i = 0; i < N; i++)
result[i] = i;
best_dist = get_dist(result);
map(result);
}
/*!
* Der Roboter aktualisiert kontinuierlich seine Karte
* @param *data der Verhaltensdatensatz
*/
void bot_scan_onthefly_behaviour(Behaviour_t * data) {
static int16_t last_location_x, last_location_y;
static int16_t last_dist_x, last_dist_y, last_dist_head;
static int16_t last_border_x, last_border_y, last_border_head;
static uint8_t index = 0;
(void) data; // kein warning
/* Verhalten je nach Cache-Fuellstand */
uint8_t cache_free = (uint8_t) (map_update_fifo.size - map_update_fifo.count);
if (cache_free < SCAN_OTF_CACHE_LEVEL_THRESHOLD) {
if (cache_free == 1) {
/* Cache ganz voll */
if (scan_otf_modes.data.map_mode &&
sensBorderL < BORDER_DANGEROUS && sensBorderR < BORDER_DANGEROUS) {
/* Stoppe den Bot, damit wir Zeit haben die Karte einzutragen
* aber nur, wenn kein Abgrund erkannt wurde */
motor_set(BOT_SPEED_STOP, BOT_SPEED_STOP);
#ifdef DEBUG_SCAN_OTF
LOG_DEBUG("Map-Cache voll, halte Bot an");
#endif
/* Halte alle Verhalten eine Weile an, weil sie ja sonst evtl. weiterfahren wuerden */
os_thread_sleep(SCAN_OTF_SLEEP_TIME);
} else {
/* Cache voll, neuen Eintrag verwerfen */
#ifdef DEBUG_SCAN_OTF
LOG_DEBUG("Map-Cache voll, verwerfe neuen Eintrag");
#endif
}
return;
}
/* Cache sehr voll */
if (v_enc_left == 0 && v_enc_right == 0) {
/* Falls Bot gerade steht, dann kleine Pause */
os_thread_sleep(SCAN_OTF_SLEEP_TIME);
return;
}
}
/* Cache updaten, falls sich der Bot weit genug bewegt hat. */
index++;
if (index == MAP_UPDATE_CACHE_SIZE) {
index = 0;
}
map_cache_t * cache_tmp = &map_update_cache[index];
cache_tmp->mode.raw = 0;
cache_tmp->dataL = 0;
cache_tmp->dataR = 0;
#ifdef MEASURE_POSITION_ERRORS_AVAILABLE
cache_tmp->loc_prob = (uint8_t) (pos_error_radius < MAP_MAX_ERROR_RADIUS ? 255 - (pos_error_radius * 256
/ MAP_MAX_ERROR_RADIUS) : 0);
#endif // MEASURE_POSITION_ERRORS_AVAILABLE
/*
* STANDFLAECHE
* Die Standflaeche tragen wir nur ein, wenn der Bot auch ein Stueck gefahren ist
*/
if (scan_otf_modes.data.location) {
// ermitteln, wie weit der Bot seit dem letzten Location-Update gefahren ist
uint16_t diff = (uint16_t) get_dist(x_pos, y_pos, last_location_x, last_location_y);
if (diff > (SCAN_OTF_RESOLUTION_DISTANCE_LOCATION * SCAN_OTF_RESOLUTION_DISTANCE_LOCATION)) {
// ist er weiter als SCAN_ONTHEFLY_DIST_RESOLUTION gefahren ==> Standflaeche aktualisieren
cache_tmp->mode.data.location = 1;
// Letzte Location-Update-Position sichern
last_location_x = x_pos;
last_location_y = y_pos;
}
}
/*
* DISTANZSENSOREN
* Die Distanzsensoren tragen wir beim Geradeausfahren selten ein,
* da sie viele Map-zellen ueberstreichen und das Eintragen teuer ist
* und sie auf der anderen Seite (beim Vorwaertsfahren) wenig neue Infos liefern
*/
if (scan_otf_modes.data.distance) {
// ermitteln, wie weit der Bot gedreht hat
int16_t turned = turned_angle(last_dist_head);
// ermitteln, wie weit der Bot seit dem letzten distance-update gefahren ist
uint16_t diff = (uint16_t) get_dist(x_pos, y_pos, last_dist_x, last_dist_y);
if ((turned > SCAN_OTF_RESOLUTION_ANGLE_DISTSENS) ||
(diff > (SCAN_OTF_RESOLUTION_DISTANCE_DISTSENS * SCAN_OTF_RESOLUTION_DISTANCE_DISTSENS))) {
// Hat sich der Bot mehr als SCAN_ONTHEFLY_ANGLE_RESOLUTION gedreht ==> Blickstrahlen aktualisieren
cache_tmp->mode.data.distance = 1;
cache_tmp->dataL = (uint8_t) (sensDistL / 5);
cache_tmp->dataR = (uint8_t) (sensDistR / 5);
// Letzte Distanz-Update-Position sichern
last_dist_x = x_pos;
last_dist_y = y_pos;
last_dist_head = heading_int;
}
}
/*
* ABGRUNDSENSOREN
* Wir werten diese nur aus, wenn der Bot entweder
* SCAN_OTF_RESOLUTION_DISTANCE_BORDER mm gefahren ist oder
* SCAN_OTF_RESOLUTION_ANGLE_BORDER Grad gedreht hat
*/
if (scan_otf_modes.data.border) {
// ermitteln, wie weit der Bot seit dem letzten border-update gefahren ist
uint16_t diff = (uint16_t) get_dist(x_pos, y_pos, last_border_x, last_border_y);
// ermitteln, wie weit der Bot gedreht hat
int16_t turned = turned_angle(last_border_head);
if (((diff > (SCAN_OTF_RESOLUTION_DISTANCE_BORDER * SCAN_OTF_RESOLUTION_DISTANCE_BORDER)) || (turned > SCAN_OTF_RESOLUTION_ANGLE_BORDER))
&& ((sensBorderL > BORDER_DANGEROUS) || (sensBorderR > BORDER_DANGEROUS))) {
cache_tmp->mode.data.border = 1;
cache_tmp->mode.data.distance = 0;
cache_tmp->dataL = (uint8_t) (sensBorderL > BORDER_DANGEROUS);
cache_tmp->dataR = (uint8_t) (sensBorderR > BORDER_DANGEROUS);
last_border_x = x_pos;
last_border_y = y_pos;
last_border_head = heading_int;
}
}
// ist ein Update angesagt?
if (cache_tmp->mode.data.distance || cache_tmp->mode.data.location || cache_tmp->mode.data.border) {
cache_tmp->x_pos = x_pos;
cache_tmp->y_pos = y_pos;
#ifdef MAP_USE_TRIG_CACHE
if (cache_tmp->mode.data.distance || cache_tmp->mode.data.border) {
cache_tmp->sin = heading_sin;
cache_tmp->cos = heading_cos;
}
#else
cache_tmp->heading = heading_10_int;
#endif // MAP_USE_TRIG_CACHE
#ifdef DEBUG_SCAN_OTF
LOG_DEBUG("neuer Eintrag: x=%d y=%d head=%f distance=%d loaction=%d border=%d", cache_tmp->x_pos, cache_tmp->y_pos, cache_tmp->heading / 10.0f, cache_tmp->mode.distance, cache_tmp->mode.location, cache_tmp->mode.border);
#endif
_inline_fifo_put(&map_update_fifo, index, False);
}
}
k_means_clustering()
{
double delta;
int i,j;
initialize_object();
//------- initialize cluster centers -----
for(i = 0; i < K; i++)
{
clusters[i].x = objects[i].x;
clusters[i].y = objects[i].y;
}
//---- intialize membership -------------
for(i = 0; i < N; i++)
{
if(i < K)
membership[i] = i;
else
{
double dist_min = get_dist(objects[i], clusters[0]);
int n = 0;
for(j = 0; j < K; j++)
{
double dist = get_dist(objects[i], clusters[j]);
if(dist < dist_min)
{
dist_min = dist;
n = j;
}
}
membership[i] = n;
}
}
printf("1st :\n");
printcluster();
delta = N;
while(delta > THRESHOLD)
{
//--- update to get new centers for each cluster---
for(i = 0; i < K; i++)
{
double new_center_x = 0;
double new_center_y = 0;
int cluster_num = 0;
for(j = 0; j < N; j++)
{
if(membership[j] == i)
{
new_center_x += objects[j].x;
new_center_y += objects[j].y;
cluster_num ++;
}
}
clusters[i].x = new_center_x/cluster_num;
clusters[i].y = new_center_y/cluster_num;
}
delta = 0.0;
//--- calculate to update each obj's membership ----
for(i = 0; i < N; i++)
{
double dist_min = get_dist(objects[i], clusters[0]);
int n = 0;
for(j = 0; j < K; j++)
{
double dist = get_dist(objects[i], clusters[j]);
if(dist < dist_min)
{
dist_min = dist;
n = j;
}
}
if (membership[i] != n)
{
delta += 1;
membership[i] = n;
}
}
}
printcluster();
}
int do_cum_mfd(void)
{
int r, c, dr, dc;
CELL is_swale;
DCELL value, valued, tci_div, sum_contour, cell_size;
int killer, threshold;
/* MFD */
int mfd_cells, stream_cells, swale_cells, astar_not_set, is_null;
double *dist_to_nbr, *contour, *weight, sum_weight, max_weight;
int r_nbr, c_nbr, r_max, c_max, ct_dir, np_side;
CELL ele, ele_nbr, aspect, is_worked;
double prop, max_val;
int workedon, edge, flat;
int asp_r[9] = { 0, -1, -1, -1, 0, 1, 1, 1, 0 };
int asp_c[9] = { 0, 1, 0, -1, -1, -1, 0, 1, 1 };
int this_index, down_index, nbr_index;
G_message(_("SECTION 3a: Accumulating Surface Flow with MFD."));
G_debug(1, "MFD convergence factor set to %d.", c_fac);
/* distances to neighbours, weights, contour lengths */
dist_to_nbr = (double *)G_malloc(sides * sizeof(double));
weight = (double *)G_malloc(sides * sizeof(double));
contour = (double *)G_malloc(sides * sizeof(double));
cell_size = get_dist(dist_to_nbr, contour);
flag_clear_all(worked);
workedon = 0;
if (bas_thres <= 0)
threshold = 60;
else
threshold = bas_thres;
for (killer = 1; killer <= do_points; killer++) {
G_percent(killer, do_points, 1);
this_index = astar_pts[killer];
seg_index_rc(alt_seg, this_index, &r, &c);
FLAG_SET(worked, r, c);
aspect = asp[this_index];
if (aspect) {
dr = r + asp_r[ABS(aspect)];
dc = c + asp_c[ABS(aspect)];
}
else
dr = dc = -1;
if (dr >= 0 && dr < nrows && dc >= 0 && dc < ncols) { /* if ((dr = astar_pts[killer].downr) > -1) { */
value = wat[this_index];
down_index = SEG_INDEX(wat_seg, dr, dc);
/* get weights */
max_weight = 0;
sum_weight = 0;
np_side = -1;
mfd_cells = 0;
astar_not_set = 1;
ele = alt[this_index];
is_null = 0;
edge = 0;
/* this loop is needed to get the sum of weights */
for (ct_dir = 0; ct_dir < sides; ct_dir++) {
/* get r, c (r_nbr, c_nbr) for neighbours */
r_nbr = r + nextdr[ct_dir];
c_nbr = c + nextdc[ct_dir];
weight[ct_dir] = -1;
if (dr == r_nbr && dc == c_nbr)
np_side = ct_dir;
/* check that neighbour is within region */
if (r_nbr >= 0 && r_nbr < nrows && c_nbr >= 0 &&
c_nbr < ncols) {
nbr_index = SEG_INDEX(wat_seg, r_nbr, c_nbr);
valued = wat[nbr_index];
ele_nbr = alt[nbr_index];
is_worked = FLAG_GET(worked, r_nbr, c_nbr);
if (is_worked == 0) {
is_null = Rast_is_c_null_value(&ele_nbr);
edge = is_null;
if (!is_null && ele_nbr <= ele) {
if (ele_nbr < ele) {
weight[ct_dir] =
mfd_pow(((ele -
ele_nbr) / dist_to_nbr[ct_dir]),
c_fac);
}
if (ele_nbr == ele) {
weight[ct_dir] =
mfd_pow((0.5 / dist_to_nbr[ct_dir]),
c_fac);
}
sum_weight += weight[ct_dir];
mfd_cells++;
if (weight[ct_dir] > max_weight) {
max_weight = weight[ct_dir];
}
if (dr == r_nbr && dc == c_nbr) {
astar_not_set = 0;
}
if (value < 0 && valued > 0)
wat[nbr_index] = -valued;
}
}
}
else
edge = 1;
if (edge)
break;
}
/* do not distribute flow along edges, this causes artifacts */
if (edge) {
continue;
}
/* honour A * path
* mfd_cells == 0: fine, SFD along A * path
* mfd_cells == 1 && astar_not_set == 0: fine, SFD along A * path
* mfd_cells > 0 && astar_not_set == 1: A * path not included, add to mfd_cells
*/
/* MFD, A * path not included, add to mfd_cells */
if (mfd_cells > 0 && astar_not_set == 1) {
mfd_cells++;
sum_weight += max_weight;
weight[np_side] = max_weight;
}
/* set flow accumulation for neighbours */
max_val = -1;
tci_div = sum_contour = 0.;
if (mfd_cells > 1) {
prop = 0.0;
for (ct_dir = 0; ct_dir < sides; ct_dir++) {
r_nbr = r + nextdr[ct_dir];
c_nbr = c + nextdc[ct_dir];
/* check that neighbour is within region */
if (r_nbr >= 0 && r_nbr < nrows && c_nbr >= 0 &&
c_nbr < ncols && weight[ct_dir] > -0.5) {
is_worked = FLAG_GET(worked, r_nbr, c_nbr);
if (is_worked == 0) {
nbr_index = SEG_INDEX(wat_seg, r_nbr, c_nbr);
weight[ct_dir] = weight[ct_dir] / sum_weight;
/* check everything adds up to 1.0 */
prop += weight[ct_dir];
if (atanb_flag) {
sum_contour += contour[ct_dir];
tci_div += get_slope_tci(ele, alt[nbr_index],
dist_to_nbr[ct_dir]) *
weight[ct_dir];
}
valued = wat[nbr_index];
if (value > 0) {
if (valued > 0)
valued += value * weight[ct_dir];
else
valued -= value * weight[ct_dir];
}
else {
if (valued < 0)
valued += value * weight[ct_dir];
else
valued = value * weight[ct_dir] - valued;
}
wat[nbr_index] = valued;
}
else if (ct_dir == np_side) {
/* check for consistency with A * path */
workedon++;
}
}
}
if (ABS(prop - 1.0) > 5E-6f) {
G_warning(_("MFD: cumulative proportion of flow distribution not 1.0 but %f"),
prop);
}
}
/* SFD-like accumulation */
else {
valued = wat[down_index];
if (value > 0) {
if (valued > 0)
valued += value;
else
valued -= value;
}
else {
if (valued < 0)
valued += value;
else
valued = value - valued;
}
wat[down_index] = valued;
if (atanb_flag) {
sum_contour = contour[np_side];
tci_div = get_slope_tci(ele, alt[down_index],
dist_to_nbr[np_side]);
}
}
/* topographic wetness index ln(a / tan(beta)) and
* stream power index a * tan(beta) */
if (atanb_flag) {
sca[this_index] = fabs(wat[this_index]) *
(cell_size / sum_contour);
tanb[this_index] = tci_div;
}
}
}
if (workedon)
G_warning(n_("MFD: A * path already processed when distributing flow: %d of %d cell",
"MFD: A * path already processed when distributing flow: %d of %d cells",
do_points),
workedon, do_points);
G_message(_("SECTION 3b: Adjusting drainage directions."));
for (killer = 1; killer <= do_points; killer++) {
G_percent(killer, do_points, 1);
this_index = astar_pts[killer];
seg_index_rc(alt_seg, this_index, &r, &c);
FLAG_UNSET(worked, r, c);
aspect = asp[this_index];
if (aspect) {
dr = r + asp_r[ABS(aspect)];
dc = c + asp_c[ABS(aspect)];
}
else
dr = dc = -1;
if (dr >= 0 && dr < nrows && dc >= 0 && dc < ncols) { /* if ((dr = astar_pts[killer].downr) > -1) { */
value = wat[this_index];
down_index = SEG_INDEX(wat_seg, dr, dc);
r_max = dr;
c_max = dc;
/* get max flow accumulation */
max_val = -1;
stream_cells = 0;
swale_cells = 0;
ele = alt[this_index];
is_null = 0;
edge = 0;
flat = 1;
for (ct_dir = 0; ct_dir < sides; ct_dir++) {
/* get r, c (r_nbr, c_nbr) for neighbours */
r_nbr = r + nextdr[ct_dir];
c_nbr = c + nextdc[ct_dir];
/* check that neighbour is within region */
if (r_nbr >= 0 && r_nbr < nrows && c_nbr >= 0 &&
c_nbr < ncols) {
nbr_index = SEG_INDEX(wat_seg, r_nbr, c_nbr);
/* check for swale or stream cells */
is_swale = FLAG_GET(swale, r_nbr, c_nbr);
if (is_swale)
swale_cells++;
valued = wat[nbr_index];
ele_nbr = alt[nbr_index];
edge = Rast_is_c_null_value(&ele_nbr);
if ((ABS(valued) + 0.5) >= threshold &&
ele_nbr > ele)
stream_cells++;
is_worked = !(FLAG_GET(worked, r_nbr, c_nbr));
if (is_worked == 0) {
if (ele_nbr != ele)
flat = 0;
is_null = Rast_is_c_null_value(&ele_nbr);
edge = is_null;
if (!is_null && ABS(valued) > max_val) {
max_val = ABS(valued);
r_max = r_nbr;
c_max = c_nbr;
}
}
}
else
edge = 1;
if (edge)
break;
}
/* do not distribute flow along edges, this causes artifacts */
if (edge) {
is_swale = FLAG_GET(swale, r, c);
if (is_swale && aspect > 0) {
aspect = -1 * drain[r - r_nbr + 1][c - c_nbr + 1];
asp[this_index] = aspect;
}
continue;
}
/* update asp */
if (dr != r_max || dc != c_max) {
aspect = drain[r - r_max + 1][c - c_max + 1];
if (asp[this_index] < 0)
aspect = -aspect;
asp[this_index] = aspect;
}
is_swale = FLAG_GET(swale, r, c);
/* start new stream */
value = ABS(value) + 0.5;
if (!is_swale && (int)value >= threshold && stream_cells < 1 &&
swale_cells < 1 && !flat) {
FLAG_SET(swale, r, c);
is_swale = 1;
}
/* continue stream */
if (is_swale) {
FLAG_SET(swale, r_max, c_max);
}
else {
if (er_flag && !is_swale)
slope_length(r, c, r_max, c_max);
}
}
}
G_free(astar_pts);
flag_destroy(worked);
G_free(dist_to_nbr);
G_free(weight);
return 0;
}
void beacon_calc(void *dummy)
{
static uint8_t a=0;
static int32_t local_rising, local_falling;
static int32_t middle;
static float size = 0;
int32_t local_angle;
int32_t local_dist;
int32_t local_count_diff_rising ;
int32_t local_count_diff_falling ;
int32_t local_beacon_coeff;
int32_t result_x=0;
int32_t result_y=0;
int32_t temp=0;
int32_t edge=0;
//int32_t total_size=0;
uint8_t flags;
//uint8_t i;
int8_t local_valid;
if(a)
LED4_ON();
else
LED4_OFF();
a = !a;
if (falling == -1){
/* 0.5 second timeout */
if (invalid_count < 25)
invalid_count++;
else {
IRQ_LOCK(flags);
beacon.opponent_x = I2C_OPPONENT_NOT_THERE;
IRQ_UNLOCK(flags);
}
return;
}
invalid_count = 0;
IRQ_LOCK(flags);
local_valid = valid_beacon;
local_count_diff_rising = count_diff_rising;
local_count_diff_falling = count_diff_falling ;
local_rising = rising;
local_falling = falling;
local_beacon_coeff = beacon_coeff;
IRQ_UNLOCK(flags);
if (local_valid){
invalid_count = 0;
//BEACON_DEBUG("rising= %ld\t",local_rising);
//BEACON_DEBUG("falling= %ld\r\n",local_falling);
/* recalculate number of pulse by adding the value of the counter, then put value back into motor's round range */
local_rising = ((local_rising + (local_count_diff_rising * local_beacon_coeff) / COEFF_MULT)) %(BEACON_STEP_TOUR);
local_falling = ((local_falling + (local_count_diff_falling * local_beacon_coeff) / COEFF_MULT)) %(BEACON_STEP_TOUR);
//BEACON_DEBUG("rising1= %ld\t",local_rising);
//BEACON_DEBUG("falling1= %ld\r\n",local_falling);
//BEACON_DEBUG("count diff rising= %ld\t",local_count_diff_rising);
//BEACON_DEBUG("count diff falling= %ld\r\n",local_count_diff_falling);
/* if around 360 deg, rising > falling, so invert both and recalculate size and middle */
if(local_falling < local_rising){
temp = local_rising;
local_rising = local_falling;
local_falling = temp;
size = BEACON_STEP_TOUR - local_falling + local_rising;
middle = (local_falling + ((int32_t)(size)/2) + BEACON_STEP_TOUR) %(BEACON_STEP_TOUR);
edge = local_falling;
}
/* else rising > falling */
else{
size = local_falling - local_rising;
middle = local_rising + (size / 2);
edge = local_rising;
}
//for(i=BEACON_MAX_SAMPLE-1;i>0;i--){
// beacon_sample_size[i] = beacon_sample_size[i-1];
// total_size += beacon_sample_size[i];
//}
//beacon_sample_size[0] = size;
//total_size += size;
//total_size /= BEACON_MAX_SAMPLE;
//BEACON_DEBUG("rising2= %ld\t",local_rising);
//BEACON_DEBUG("falling2= %ld\r\n",local_falling);
/* BEACON_DEBUG("size= %ld %ld\t",size, total_size); */
BEACON_DEBUG("size= %f\r\n",size);
//BEACON_DEBUG("middle= %ld\r\n",middle);
local_angle = get_angle(middle,0);
BEACON_NOTICE("opponent angle= %ld\t",local_angle);
local_dist = get_dist(size);
BEACON_NOTICE("opponent dist= %ld\r\n",local_dist);
beacon_angle_dist_to_x_y(local_angle, local_dist, &result_x, &result_y);
IRQ_LOCK(flags);
beacon.opponent_x = result_x;
beacon.opponent_y = result_y;
beacon.opponent_angle = local_angle;
beacon.opponent_dist = local_dist;
/* for I2C test */
//beacon.opponent_x = OPPONENT_POS_X;
//beacon.opponent_y = OPPONENT_POS_Y;
IRQ_UNLOCK(flags);
BEACON_NOTICE("opponent x= %ld\t",beacon.opponent_x);
BEACON_NOTICE("opponent y= %ld\r\n\n",beacon.opponent_y);
}
else {
BEACON_NOTICE("non valid\r\n\n");
}
falling = -1;
}
/*!
* Verhalten um sich entlang des Fahrweges relevante Koordinaten auf dem Stack zu merken; Verhalten ist nach Aktivierung via Botenroutine
* ein Endlosverhalten und sammelt bis zur Deaktivierung die Wegepunkte; deaktiviert wird es via Notaus oder direkt mit Befehl zum Zurueckfahren und
* damit Start des Stack-Fahrverhaltens
* @param *data Der Verhaltensdatensatz
*/
void bot_save_waypos_behaviour(Behaviour_t * data) {
static uint8_t skip_count = 0;
switch (waypos_state) {
case 0:
pos_store = pos_store_new_size(data, STACK_SIZE);
if (pos_store == NULL) {
exit_behaviour(data, BEHAVIOUR_SUBFAIL);
return;
}
set_pos_to_last(); // aktuelle Botposition wird zum ersten Stackeintrag und merken der Position
bot_push_pos(last_pos.x, last_pos.y);
waypos_state = 1;
break;
case 1: {
// Botpos wird in den Stack geschrieben zum Startzeitpunkt des Verhaltens (Stack noch leer) oder
// wenn die Entfernung zur zuletzt gemerkten Koordinate gewissen Abstand ueberschreitet oder
// wenn Entfernung noch nicht erreicht ist aber ein gewisser Drehwinkel erreicht wurde
uint8_t update = False;
// Abstand zur letzten Position ueberschritten
if (get_dist(last_pos.x, last_pos.y, x_pos, y_pos) > DIST_FOR_PUSH) {
// kein Push notwendig bei gerader Fahrt voraus zum Sparen des Stack-Speicherplatzes
if ((int16_t) heading != last_heading) {
update = True;
}
set_pos_to_last(); // Position jedenfalls merken
}
// bei Drehwinkelaenderung und Uberschreitung einer gewissen Groesse mit geringer Abstandsentfernung zum letzten Punkt kommt er in den Stack
if (turned_angle(last_heading) > ANGLE_FOR_PUSH && get_dist(last_pos.x, last_pos.y, x_pos, y_pos) > DIST_FOR_PUSH_TURN) {
set_pos_to_last();
update = True;
}
if (update) {
LOG_DEBUG("Position (%d|%d) kommt in den Stack", x_pos, y_pos);
position_t pos_0;
pos_0.x = x_pos / 16;
pos_0.y = y_pos / 16;
if (optimized_push >= 1) {
/* Positionen entfernen, die auf einer Linie liegen */
position_t pos_1, pos_2;
if (pos_store_top(pos_store, &pos_1, 1) == True && pos_store_top(pos_store, &pos_2, 2) == True) {
pos_1.x /= 16;
pos_1.y /= 16;
if (abs(pos_1.x - pos_0.x) <= 1) {
pos_0.x = pos_1.x;
}
pos_2.x /= 16;
pos_2.y /= 16;
if (abs(pos_2.x - pos_1.x) <= 1) {
pos_2.x = pos_1.x;
}
int8_t m_1 = (int8_t) (pos_1.x == pos_2.x ? 100 : abs((pos_1.y - pos_2.y) / (pos_1.x - pos_2.x)));
int8_t m = (int8_t) (pos_0.x == pos_1.x ? 100 : abs((pos_0.y - pos_1.y) / (pos_0.x - pos_1.x)));
LOG_DEBUG(" pos_2=(%d|%d)", pos_2.x, pos_2.y);
LOG_DEBUG(" pos_1=(%d|%d)", pos_1.x, pos_1.y);
LOG_DEBUG(" pos =(%d|%d)", pos_0.x, pos_0.y);
LOG_DEBUG(" m_1=%3d\tm=%3d", m_1, m);
LOG_DEBUG(" skip_count=%u", skip_count);
if (abs(m_1 - m) < MAX_GRADIENT) {
LOG_DEBUG("Neue Position auf einer Linie mit beiden Letzten");
if (skip_count < 3) {
LOG_DEBUG(" Verwerfe letzten Eintrag (%d|%d)", pos_1.x, pos_1.y);
pos_store_pop(pos_store, &pos_1);
skip_count++;
} else {
LOG_DEBUG(" Verwerfe Eintrag NICHT, skip_count=%u", skip_count);
skip_count = 0;
}
} else {
skip_count = 0;
}
}
if (optimized_push >= 2) {
/* Schleifen entfernen */
uint8_t i;
for (i=2; pos_store_top(pos_store, &pos_1, i); ++i) {
int32_t diff = get_dist(x_pos, y_pos, pos_1.x, pos_1.y);
LOG_DEBUG(" diff=%d", diff);
if (diff < MAX_POS_DIFF) {
LOG_DEBUG(" Position (%d|%d)@%u liegt in der Naehe", pos_1.x, pos_1.y, i);
uint8_t k;
for (k=i; k>1; --k) {
pos_store_pop(pos_store, &pos_1); // die Positionen bis zur i-ten zurueck loeschen
LOG_DEBUG(" Loesche (%d|%d) vom Stack", pos_1.x, pos_1.y);
}
i = 2;
}
}
}
}
bot_push_pos(x_pos, y_pos);
#if defined PC && defined DEBUG
pos_store_dump(pos_store);
#endif
#ifdef DEBUG
#ifdef MAP_2_SIM_AVAILABLE
command_write(CMD_MAP, SUB_MAP_CLEAR_LINES, 0, 0, 0);
position_t pos_1, pos_2;
pos_store_top(pos_store, &pos_0, 1);
uint16_t i;
for (i = 2; pos_store_top(pos_store, &pos_1, (uint8_t) i); ++i) {
map_draw_line_world(pos_0, pos_1, 0);
pos_0 = pos_1;
pos_1.x = pos_0.x - 16;
pos_1.y = pos_0.y;
pos_2.x = pos_0.x + 16;
pos_2.y = pos_0.y;
map_draw_line_world(pos_1, pos_2, 1);
pos_1.x = pos_0.x;
pos_1.y = pos_0.y - 16;
pos_2.x = pos_0.x;
pos_2.y = pos_0.y + 16;
map_draw_line_world(pos_1, pos_2, 1);
}
#endif // MAP_2_SIM_AVAILABLE
#endif // DEBUG
}
break;
} // case 1
} // switch
}
double get_sum( double* x, double* y ) {
double res = 0.0;
for ( int i = 0; i + 1 < N; ++ i )
res += get_dist(x[i], y[i], x[i + 1], y[i + 1]);
return res;
}
CubitStatus FBRetriangulate::add_bdry_edges()
{
// For each triangle edge, make a list of pairs of edge points and
// vertex numbers that touch the edge. Then sort the list by distance
// from the start of the edge. (This was what we really put in the
// pair, not the point itself.) Finally, go through this sorted list
// and add edges to the edge_list. If the edge_list is empty, we
// still must add in the triangle edge itself.
CubitStatus status;
FB_Edge *edge;
std::list< std::pair<double,int> > edge0_list, edge1_list, edge2_list;
std::list< std::pair<double,int> >::iterator dp;
std::vector<FB_Edge*>::iterator dpe, dpe_orig;
double vx0, vy0, vz0, vx1, vy1, vz1, vx2, vy2, vz2, dist;
std::pair<double,int> mypair;
status = CUBIT_SUCCESS;
vx0 = verts[my_tri->v0]->coord[0];
vy0 = verts[my_tri->v0]->coord[1];
vz0 = verts[my_tri->v0]->coord[2];
vx1 = verts[my_tri->v1]->coord[0];
vy1 = verts[my_tri->v1]->coord[1];
vz1 = verts[my_tri->v1]->coord[2];
vx2 = verts[my_tri->v2]->coord[0];
vy2 = verts[my_tri->v2]->coord[1];
vz2 = verts[my_tri->v2]->coord[2];
dpe = my_tri->edge_list.begin();
while ( dpe != my_tri->edge_list.end() ) {
edge = *dpe;
dpe++;
if ( (edge->v0_type == INTERIOR_VERT) &&
(edge->v1_type == INTERIOR_VERT) )
continue;
if ( edge->v0_type == EDGE_0 ) {
dist = get_dist(vx0,vy0,vz0,verts[edge->v0]->coord[0],
verts[edge->v0]->coord[1],verts[edge->v0]->coord[2]);
edge0_list.push_back(std::pair<double,int>(dist,edge->v0));
} else if ( edge->v0_type == EDGE_1 ) {
dist = get_dist(vx1,vy1,vz1,verts[edge->v0]->coord[0],
verts[edge->v0]->coord[1],verts[edge->v0]->coord[2]);
edge1_list.push_back(std::pair<double,int>(dist,edge->v0));
} else if ( edge->v0_type == EDGE_2 ) {
dist = get_dist(vx2,vy2,vz2,verts[edge->v0]->coord[0],
verts[edge->v0]->coord[1],verts[edge->v0]->coord[2]);
edge2_list.push_back(std::pair<double,int>(dist,edge->v0));
}
if ( edge->v1_type == EDGE_0 ) {
dist = get_dist(vx0,vy0,vz0,verts[edge->v1]->coord[0],
verts[edge->v1]->coord[1],verts[edge->v1]->coord[2]);
edge0_list.push_back(std::pair<double,int>(dist,edge->v1));
} else if ( edge->v1_type == EDGE_1 ) {
dist = get_dist(vx1,vy1,vz1,verts[edge->v1]->coord[0],
verts[edge->v1]->coord[1],verts[edge->v1]->coord[2]);
edge1_list.push_back(std::pair<double,int>(dist,edge->v1));
} else if ( edge->v1_type == EDGE_2 ) {
dist = get_dist(vx2,vy2,vz2,verts[edge->v1]->coord[0],
verts[edge->v1]->coord[1],verts[edge->v1]->coord[2]);
edge2_list.push_back(std::pair<double,int>(dist,edge->v1));
}
}
edge0_list.sort();
edge1_list.sort();
edge2_list.sort();
// Now we have to remove all BDRY_EDGEs because they will be made anew
// in what follows. Erasing elements from a vector is inefficient, but
// this shouldn't happen often.
dpe = my_tri->edge_list.begin();
dpe_orig = my_tri->edge_list.end();
while ( dpe != dpe_orig ) {
edge = *dpe;
if ( edge->edge_type == BDRY_EDGE ) {
dpe = my_tri->edge_list.erase(dpe);
dpe_orig = my_tri->edge_list.end();
} else {
dpe++;
}
}
int newv0, newv1, newv0_type, newv1_type;
newv0_type = newv1_type = EDGE_0;
newv0 = my_tri->v0;
dp = edge0_list.begin();
while ( dp != edge0_list.end() ) {
mypair = *dp;
newv1 = mypair.second;
// It is possible to get the same vert more than once in the list.
// After sorting, they will be adjacent. The following if statement
// causes duplicate verts to be used only once.
if ( newv0 != newv1 ) {
add_this_bdry_edge(newv0,newv1,newv0_type,newv1_type);
newv0 = newv1;
}
dp++;
}
newv1 = my_tri->v1;
if ( newv0 != newv1 )
add_this_bdry_edge(newv0,newv1,newv0_type,newv1_type);
newv0_type = newv1_type = EDGE_1;
newv0 = my_tri->v1;
dp = edge1_list.begin();
while ( dp != edge1_list.end() ) {
mypair = *dp;
newv1 = mypair.second;
if ( newv0 != newv1 ) {
add_this_bdry_edge(newv0,newv1,newv0_type,newv1_type);
newv0 = newv1;
}
dp++;
}
newv1 = my_tri->v2;
if ( newv0 != newv1 )
add_this_bdry_edge(newv0,newv1,newv0_type,newv1_type);
newv0_type = newv1_type = EDGE_2;
newv0 = my_tri->v2;
dp = edge2_list.begin();
while ( dp != edge2_list.end() ) {
mypair = *dp;
newv1 = mypair.second;
if ( newv0 != newv1 ) {
add_this_bdry_edge(newv0,newv1,newv0_type,newv1_type);
newv0 = newv1;
}
dp++;
}
newv1 = my_tri->v0;
if ( newv0 != newv1 )
add_this_bdry_edge(newv0,newv1,newv0_type,newv1_type);
return status;
}
void detect_ellipses(unsigned char** sobel_output, int w, int h, double threshold, int min_dist, EllipseList* ellipses_result_list) {
List* pixels = get_pixels(sobel_output, w, h);
if(pixels->size < 3) {
printf("To few pixels\n");
return;
}
//minor axis
double acc[BSIZE];
int i;
for(i=0; i<BSIZE; i++) {
acc[i] = 0.;
}
Node *i_node, *j_node, *k_node;
double center_x, center_y, a, b, d, f, alpha, cosine, cosine_sq, sin_sq, max_acc;
//
printf("przed przetw %d\n", pixels->size);
//
// print_list(pixels);
for(i_node = pixels->head; i_node->next; i_node = i_node->next) {
for(j_node = i_node->next; j_node; j_node = j_node->next) {
if(INODEX == JNODEX) {
if(get_dist(INODEX, INODEY, JNODEX, JNODEY) > min_dist) {
// printf("Rozwazane punkty: (%d, %d), (%d, %d)\n",
// INODEX, INODEY, JNODEX, JNODEY);
center_x = (INODEX + JNODEX)/2;
center_y = (INODEY + JNODEY)/2;
a = get_dist(INODEX, INODEY, JNODEX, JNODEY)/2;
if(JNODEX != INODEX)
alpha = atan((JNODEY - INODEY)/(JNODEX - INODEX));
// printf("a = %lf\n", a);
for(k_node = i_node->next; k_node != j_node; k_node = k_node->next) {
if((KNODEY != INODEY) && (KNODEY != JNODEY)) {
if((d = get_dist(KNODEX, KNODEY, center_x, center_y)) > min_dist) {
// printf("Rozwazane punkty: (%d, %d), (%d, %d), (%d, %d)\n",
// INODEX, INODEY, JNODEX, JNODEY, KNODEX, KNODEY);
f = get_dist(KNODEX, KNODEY, JNODEX, JNODEY);
cosine = (a*a + d*d - f*f)/(2*a*d);
cosine_sq = cosine*cosine;
sin_sq = 1 - cosine_sq;
double tmp2 = (a*a - d*d *cosine_sq);
double tmp1 = (a*a * d*d * sin_sq);
// printf("tmp1 = %lf, tmp2=%lf\t", tmp1, tmp2);
if(tmp1 < 0)
tmp1 = -tmp1;
if(tmp2 < 0)
tmp2 = -tmp2;
b = sqrt(tmp1/tmp2);
int round_b = round(b);
if(round_b < BSIZE && round_b > 0) {
k_node->b_ellipse = round_b;
acc[round_b]++;
} else {
// printf("BLAD!!\n");
// printf("Rozwazane punkty: (%d, %d), (%d, %d), (%d, %d)\n",
// INODEX, INODEY, JNODEX, JNODEY, KNODEX, KNODEY);
// printf("tmp1 = %lf, tmp2=%lf\n", tmp1, tmp2);
// printf("a = %lf\n", a);
// printf("b = %lf\n\n", b);
}
}
}
}
max_acc = 0.;
double max_wart = 0.;
for(i=0; i<BSIZE; i++) {
if(acc[i] > max_wart) {
// printf("%d ", (int)acc[i]);
max_wart = acc[i];
max_acc = i;
}
}
double obwod;
if(abs(a-max_acc) < 4)
obwod = M_PI*a*2/3;
else
obwod = M_PI*(3/2 * (a+max_acc) - sqrt(a*max_acc))*1/2;
if(max_wart >= obwod*4/5 && max_wart >= threshold && max_wart>0 && (int)max_acc>10) {
// printf("ELLIPSE FOUND!! %d %d %d %d\n", 512-(int)center_x, (int)center_y, (int)a, (int)max_acc);
// printf("(%d, %d), (%d, %d)\n\n", 512-INODEX, INODEY, 512-JNODEX, JNODEY);
add_first_ellipse(ellipses_result_list, (int)center_y, (int)center_x, (int)max_acc, (int)a);
drawEllipse("output_ellipse.bmp", (int)center_y, (int)center_x, (int)max_acc, (int)a, 512, 512);
removeEllipseFromImage(i_node, j_node, max_acc);
i_node->b_ellipse = 0.;
j_node->b_ellipse = 0.;
}
for(i=0; i<BSIZE; i++) {
acc[i] = 0.;
}
}
}
}
}
}
