-#include <stdio.h>
-#include <string.h>
-#include <inttypes.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/error.h>
-
-#include <stdint.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <math.h>
-
-#include <vect_base.h>
-#include <lines.h>
-#include <polygon.h>
-
-#include "img_processing.h"
-
-
-#define debug_printf(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__)
-
-#ifndef HOST_VERSION
-
-#include <aversive.h>
-#include <pwm_ng.h>
-#include <pid.h>
-#include <time.h>
-#include <quadramp.h>
-#include <blocking_detection_manager.h>
-#include <rdline.h>
-
-#include <control_system_manager.h>
-#include <adc.h>
-#include <spi.h>
-#include <ax12.h>
-
-#include "main.h"
-
-#define IMG_DEBUG(args...) DEBUG(E_USER_IMGPROCESS, args)
-#define IMG_NOTICE(args...) NOTICE(E_USER_IMGPROCESS, args)
-#define IMG_ERROR(args...) ERROR(E_USER_IMGPROCESS, args)
-
-#else
-
-#define IMG_DEBUG(args...) debug_printf(args)
-#define IMG_NOTICE(args...) debug_printf(args)
-#define IMG_ERROR(args...) debug_printf(args)
-
-#endif
-
-
-
-
-#define OBJECT_MINIMUM_DEMI_PERIMETER (2*3)
-/* store object in pool if never seen
- * returns:
- * 1 if new object
- * 0 if already known
- */
-int store_obj(Object_bb* tab_o, int total, Object_bb*o)
-{
- uint8_t i;
-
- if (o->x_max - o->x_min + o->y_max - o->y_min < OBJECT_MINIMUM_DEMI_PERIMETER)
- return 0;
-
- for (i=0;i<total;i++){
- if (!memcmp(&tab_o[i], o, sizeof(Object_bb)))
- return 0;
-
- if (tab_o[i].x_min==0 && tab_o[i].x_max==0 &&
- tab_o[i].y_min==0 && tab_o[i].y_max==0){
- memcpy(&tab_o[i], o, sizeof(Object_bb));
- return 1;
- }
-
- }
- return 0;
-}
-
-/* step around object of given color and compute its bounding box */
-void object_find_bounding_box(unsigned char* data,
- int16_t x_in, int16_t y_in,
- int16_t start_x, int16_t start_y,
- int16_t color, int16_t color_w,Object_bb* o)
-{
- int16_t pos_x = start_x;
- int16_t pos_y = start_y;
- int16_t start =0;
- int16_t len = 0;
- int16_t count_stand = 0;
-
- vect_t v, vi;
-
-
- v.x = 1;
- v.y = 0;
-
- vi = v;
-
- o->x_max = 0;
- o->y_max = 0;
- o->x_min = x_in;
- o->y_min = y_in;
-
-
- while(1){
- if (pos_x == start_x && pos_y == start_y){
- count_stand++;
- if (count_stand>4)
- break;
- if( v.x == vi.x && v.y == vi.y && start)
- break;
- }
-
- if (pos_x<o->x_min)
- o->x_min = pos_x;
- if (pos_y<o->y_min)
- o->y_min = pos_y;
- if (pos_x>o->x_max)
- o->x_max = pos_x;
- if (pos_y>o->y_max)
- o->y_max = pos_y;
-
- /* is next pixel is good color */
- if (data[(pos_y+v.y)*x_in + pos_x+v.x] != color){
- pos_x = pos_x+v.x;
- pos_y = pos_y+v.y;
- len++;
- vect_rot_retro(&v);
- start = 1;
- continue;
- }
- vect_rot_trigo(&v);
- }
-
- o->len = len;
-}
-
-
-
-
-
-/* step around object of given color and computes its polygon */
-void object_find_poly(unsigned char* data,
- int16_t x_in, int16_t y_in,
- int16_t start_x, int16_t start_y,
- int16_t color, int16_t color_w, Object_poly* o)
-{
- int16_t pos_x = start_x;
- int16_t pos_y = start_y;
- int16_t start =0;
- int16_t len = 0;
- int16_t count_stand = 0;
- uint16_t pt_step, pt_num;
- vect_t v, vi;
-
- v.x = 1;
- v.y = 0;
-
- vi = v;
-
- pt_step = o->len/POLY_MAX_PTS + 1;
-
- pt_num = 0;
-
- while(1){
- if (pos_x == start_x && pos_y == start_y){
- count_stand++;
- if (count_stand>4)
- break;
- if( v.x == vi.x && v.y == vi.y && start)
- break;
- }
-
- /* is next pixel is good color */
- if (data[(pos_y+v.y)*x_in + pos_x+v.x] != color){
- pos_x = pos_x+v.x;
- pos_y = pos_y+v.y;
- len++;
-
- if (len >pt_num*pt_step){
- o->pts[pt_num].x = pos_x;
- o->pts[pt_num].y = pos_y;
- pt_num+=1;
- if (pt_num>=POLY_MAX_PTS)
- break;
- }
-
- vect_rot_retro(&v);
- start = 1;
- continue;
- }
- vect_rot_trigo(&v);
- }
-
- o->pts_num = pt_num;
-
-}
-
-#define PT_LEFT 0
-#define PT_RIGHT 2
-#define PT_TOP 1
-#define PT_DOWN 3
-
-/* return most left/right/top/down pts indexes of given polygon */
-void object_find_extrem_points_index(Object_poly* o,
- unsigned int *pts)
-{
- unsigned int i;
- for (i=0;i<4;i++)
- pts[i] = 0;
-
-
- for (i=1;i<o->pts_num;i++){
- if (o->pts[i].x < o->pts[pts[PT_LEFT]].x)
- pts[PT_LEFT] = i;
- if (o->pts[i].x > o->pts[pts[PT_RIGHT]].x)
- pts[PT_RIGHT] = i;
- if (o->pts[i].y < o->pts[pts[PT_TOP]].y)
- pts[PT_TOP] = i;
- if (o->pts[i].y > o->pts[pts[PT_DOWN]].y)
- pts[PT_DOWN] = i;
- }
-
-}
-
-
-#define NUM_CALIPERS 4
-/* for debug purpose: display a vector on image */
-void draw_pt_vect(unsigned char *buf, int16_t x_in, int16_t y_in,
- vect_t *v, point_t p)
-{
- unsigned int i;
- float n;
- int16_t x, y;
- float coef=1.0;
-
- if (!v->x && !v->y)
- return;
-
- n = vect_norm(v);
- coef = 1/n;
- for (i=0;i<5;i++){
- x = p.x;
- y = p.y;
-
- x+=(float)((v->x)*(float)i*(float)coef);
- y+=(float)((v->y)*(float)i*(float)coef);
- if ((x== p.x) && (y == p.y))
- buf[y*x_in+x] = 0x0;
- else
- buf[y*x_in+x] = 0x0;
- }
-
-}
-
-#define CAL_X 3
-#define CAL_Y 0
-
-
-/* compute minimum rectangle area including the given convex polygon */
-void object_poly_get_min_ar(Object_poly *o, unsigned int *pts_index_out,
- vect_t *v_out, vect_t *r1, vect_t*r2)
-{
- vect_t calipers[NUM_CALIPERS];
- vect_t edges[NUM_CALIPERS];
-
- unsigned int i;
- unsigned int calipers_pts_index[NUM_CALIPERS];
- float angles[NUM_CALIPERS];
- float min_angle;
- float total_rot_angle = 0;
- int caliper_result_index;
-
- vect_t res1, res2;
- float ps, n1, n2, caliper_n;
-
- float aera_tmp;
- /* XXX hack sould be max*/
- float aera_min=0x100000;
-
- object_find_extrem_points_index(o, calipers_pts_index);
-
- calipers[0].x = 0;
- calipers[0].y = 1;
-
- calipers[1].x = -1;
- calipers[1].y = 0;
-
- calipers[2].x = 0;
- calipers[2].y = -1;
-
- calipers[3].x = 1;
- calipers[3].y = 0;
-
-
- while (total_rot_angle <= M_PI/2){
-
- for (i=0;i<NUM_CALIPERS;i++){
- /* compute polygon edge vector */
- edges[i].x = o->pts[(calipers_pts_index[i] + 1)%o->pts_num].x -
- o->pts[calipers_pts_index[i]].x;
- edges[i].y = o->pts[(calipers_pts_index[i] + 1)%o->pts_num].y -
- o->pts[calipers_pts_index[i]].y;
-
- /* compute angle between caliper and polygon edge */
- angles[i] = vect_get_angle(&edges[i], &calipers[i]);
- }
-
- /* find min angle */
- min_angle = angles[0];
- caliper_result_index = 0;
- for (i=1;i<NUM_CALIPERS;i++){
- if (angles[i]<min_angle){
- min_angle = angles[i];
- caliper_result_index = i;
- }
- }
-
- /* rotate calipers */
- calipers[caliper_result_index] = edges[caliper_result_index];
-
- for (i=caliper_result_index; i<caliper_result_index + o->pts_num; i++){
- calipers[(i+1) % NUM_CALIPERS] = calipers[i % NUM_CALIPERS];
- vect_rot_trigo(&calipers[(i+1) % NUM_CALIPERS]);
- }
-
- /* update calipers point */
- for (i=0;i<NUM_CALIPERS;i++){
- if (angles[i]==min_angle)
- calipers_pts_index[i] = (calipers_pts_index[i] + 1) % o->pts_num;
- }
-
- res1.x = o->pts[calipers_pts_index[2]].x - o->pts[calipers_pts_index[0]].x;
- res1.y = o->pts[calipers_pts_index[2]].y - o->pts[calipers_pts_index[0]].y;
-
- res2.x = o->pts[calipers_pts_index[3]].x - o->pts[calipers_pts_index[1]].x;
- res2.y = o->pts[calipers_pts_index[3]].y - o->pts[calipers_pts_index[1]].y;
-
- ps = vect_pscal(&res1, &calipers[CAL_X]);
- n1 = vect_norm(&res1);
- caliper_n = vect_norm(&calipers[CAL_X]);
- caliper_n*=caliper_n;
-
- res1 = calipers[CAL_X];
-
- res1.x *= ps/(caliper_n);
- res1.y *= ps/(caliper_n);
-
-
- ps = vect_pscal(&res2, &calipers[CAL_Y]);
- n1 = vect_norm(&res2);
-
- res2 = calipers[CAL_Y];
-
- res2.x *= ps/(caliper_n);
- res2.y *= ps/(caliper_n);
-
- n1 = vect_norm(&res1);
- n2 = vect_norm(&res2);
-
- aera_tmp = n1*n2;
-
- if (aera_min >aera_tmp){
- aera_min = aera_tmp;
- for (i=0;i<NUM_CALIPERS;i++){
- pts_index_out[i] = calipers_pts_index[i];
- }
- *v_out = calipers[0];
- *r1 = res1;
- *r2 = res2;
-
- }
- total_rot_angle+=min_angle;
- }
-
- return;
-}
-
-#define COEF_CALIP 1
-/* transform caliper to rectangle coordinates */
-int object_poly_caliper_to_rectangle(Object_poly *o,
- unsigned int *pts_index_out, vect_t* caliper,
- vect_t *r1, vect_t*r2, point_t *p)
-{
- line_t l1, l2;
- vect_t caliper_tmp;
- int ret, i;
- double mp_x, mp_y;
- point_t p_int1;//, p_int2;
-
- point_t p1, p2;
-
- caliper_tmp = *caliper;
-
- //IMG_DEBUG("cal: %" PRIi32 " %" PRIi32 "", caliper_tmp.x, caliper_tmp.y);
-
- mp_x = 0;
- mp_y = 0;
-
- /* to be precise, calc 4 intersection of 4 calipers */
- for (i=0;i<NUM_CALIPERS;i++){
- p1.x = o->pts[pts_index_out[i]].x;
- p1.y = o->pts[pts_index_out[i]].y;
-
- p2.x = p1.x+COEF_CALIP*caliper_tmp.x;
- p2.y = p1.y+COEF_CALIP*caliper_tmp.y;
-
- pts2line(&p1, &p2, &l1);
-
- vect_rot_trigo(&caliper_tmp);
-
- p1.x = o->pts[pts_index_out[(i+1)%NUM_CALIPERS]].x;
- p1.y = o->pts[pts_index_out[(i+1)%NUM_CALIPERS]].y;
-
- p2.x = p1.x+COEF_CALIP*caliper_tmp.x;
- p2.y = p1.y+COEF_CALIP*caliper_tmp.y;
-
- pts2line(&p1, &p2, &l2);
-
- ret = intersect_line(&l1, &l2, &p_int1);
- if (ret!=1)
- return 0;
- //IMG_DEBUG("int1 (%d): %" PRIi32 " %" PRIi32 " ", ret, p_int1.x, p_int1.y);
-
- mp_x+=p_int1.x;
- mp_y+=p_int1.y;
-
- }
-
-
-
- p->x = lround(mp_x/NUM_CALIPERS);
- p->y = lround(mp_y/NUM_CALIPERS);
-
-
- return 1;
-
-}
-
-#define OBJECT_DIM 5
-/* split zone in many column's sized area */
-int split_rectangle(point_t *p, vect_t *r1, vect_t* r2, uint8_t max_zone, zone* zones, uint8_t color)
-{
- int n1, n2;
- int i, j;
- int index=0;
- int r1_s, r2_s;
- point_t ptmp;
-
-
- n1 = vect_norm(r1);
- n2 = vect_norm(r2);
-
- r1_s = n1/OBJECT_DIM;
- r2_s = n2/OBJECT_DIM;
-
- if (!r1_s || ! r2_s)
- return 0;
-
- ptmp.x = p->x - r1->x/2 - r2->x/2 + (r1->x/(r1_s*2))+(r2->x/(r2_s*2));
- ptmp.y = p->y - r1->y/2 - r2->y/2 + (r1->y/(r1_s*2))+(r2->y/(r2_s*2));
-
- for(i=0;i<r1_s;i++){
- for(j=0;j<r2_s;j++){
- zones[index].p.x = ptmp.x + (i*r1->x)/r1_s+(j*r2->x)/r2_s;
- zones[index].p.y = ptmp.y + (i*r1->y)/r1_s+(j*r2->y)/r2_s;
- zones[index].h = color;
- zones[index].valid = 1;
-
- index++;
- if (index>=max_zone)
- return index;
-
-
- }
- }
-
- return index;
-}
-
-#define OBJECT_SEMI_DIM (OBJECT_DIM/2)
-#define MIN_SURFACE_PERCENT 50
-#define HIGHER_MAX_PIXEL 5
-
-
-int zone_has_enought_pixels(unsigned char* data, int16_t x_in, int16_t y_in, zone* z)
-{
- int x, y;
- uint16_t count, total_pix, higher_pixels;
-
- count = 0;
- total_pix=0;
- higher_pixels = 0;
-
- for (x = -OBJECT_SEMI_DIM;
- (x <= OBJECT_SEMI_DIM) && (higher_pixels < HIGHER_MAX_PIXEL);
- x++){
- for (y = -OBJECT_SEMI_DIM;
- (y <= OBJECT_SEMI_DIM) && (higher_pixels < HIGHER_MAX_PIXEL);
- y++){
- total_pix++;
- if (data[x_in * (y + z->p.y) + x + z->p.x] == z->h)
- count++;
-
- if (data[x_in * (y + z->p.y) + x + z->p.x] > z->h)
- higher_pixels++;
-
- }
-
- }
-
- IMG_DEBUG("has enougth pixel (h: %d x %"PRIi32": y:%"PRIi32") total: %d/%d (tt: %d, hmax: %d)", z->h, z->p.x, z->p.y,
- count, (total_pix * MIN_SURFACE_PERCENT) / 100, total_pix, higher_pixels);
-
- if ((count > (total_pix * MIN_SURFACE_PERCENT) / 100) &&
- (higher_pixels <HIGHER_MAX_PIXEL))
- return 1;
-
- return 0;
-}
-
-int zone_filter_min_surface(unsigned char* data, int16_t x_in, int16_t y_in,
- uint8_t color, unsigned int zones_num, zone* p)
-{
- int i;
-
- for (i = 0; i < zones_num ; i++){
- if (zone_has_enought_pixels(data, x_in, y_in, &p[i]))
- continue;
-
- p[i].valid = 0;
- }
-
- IMG_NOTICE("num zone after min surf: %d", zones_num);
-
- return zones_num;
-
-}
-
-/* center is 15 cm radius*/
-#define CENTER_RADIUS 15
-
-/* the complete column must be in the drop zone*/
-#define CENTER_MAX_DIST (15-3)
-
-/* the column must not be too close from center*/
-#define CENTER_MIN_DIST (8)
-
-int zone_filter_center(unsigned int zones_num, zone* p, int16_t center_x, int16_t center_y, int tweak_min_margin)
-{
- int i;
- vect_t v;
-
- for (i = 0; i < zones_num; i++){
-
- v.x = p[i].p.x - center_x;
- v.y = p[i].p.y - center_y;
- IMG_DEBUG("square dist to center %"PRIi32" (%d %d)",
- v.x*v.x + v.y*v.y, (CENTER_MIN_DIST+tweak_min_margin) * (CENTER_MIN_DIST+tweak_min_margin), CENTER_MAX_DIST * CENTER_MAX_DIST);
-
- if (v.x*v.x + v.y*v.y < CENTER_MAX_DIST * CENTER_MAX_DIST &&
- v.x*v.x + v.y*v.y > (CENTER_MIN_DIST+tweak_min_margin) * (CENTER_MIN_DIST+tweak_min_margin))
- continue;
-
- p[i].valid = 0;
-
- }
-
- return zones_num;
-}
-
-#define MAX_DIST_TO_ZONE 2
-
-unsigned int zone_filter_zone_rect(unsigned int zones_num, zone* p, int16_t center_x, int16_t center_y , uint8_t working_zone)
-{
- int i;
-
- for (i = 0; i < zones_num; i++){
-
- IMG_DEBUG("rct x:%"PRIi32" y:%"PRIi32" (centerx: %d)",p[i].p.x , p[i].p.y, center_x);
-
- if ((p[i].p.x > center_x - MAX_DIST_TO_ZONE) && (p[i].h > working_zone))
- continue;
-
- p[i].valid = 0;
- }
-
- return zones_num;
-}
-
-
-/* delete point to render polygon convex */
-int object_poly_to_convex(Object_poly *o)
-{
- unsigned int i, j;
- vect_t v, w;
- int16_t z;
- unsigned int del_pts_num = 0;
-
- for (i=0;i<o->pts_num;){
- v.x = o->pts[(i + o->pts_num - 1)%o->pts_num].x - o->pts[i].x;
- v.y = o->pts[(i + o->pts_num - 1)%o->pts_num].y - o->pts[i].y;
-
- w.x = o->pts[(i+1)%o->pts_num].x - o->pts[i].x;
- w.y = o->pts[(i+1)%o->pts_num].y - o->pts[i].y;
-
- z = vect_pvect(&v, &w);
- if (z>0){
- i+=1;
- continue;
- }
-
- /* found a convex angle (or colinear points) */
- for (j = i; j < o->pts_num-1; j++){
- o->pts[j] = o->pts[j+1];
- }
- if (i!=0)
- i-=1;
- o->pts_num--;
- del_pts_num++;
- }
-
- return del_pts_num;
-}
-
-
-
-
-
-#define DEFAULT_COLOR 255
-/* scan all image and find objects*/
-unsigned char *parcour_img(unsigned char* data, int16_t x_in, int16_t y_in,
- Object_bb *sac_obj, Object_poly *sac_obj_poly, int16_t max_obj)
-{
- int16_t i, obj_num;
-
- uint8_t in_color=0;
- int16_t etat;
-
- Object_bb o;
- int ret;
-
- obj_num = 0;
- /*
- first, delete borders
- */
- for (i=0;i<x_in;i++){
- data[i] = 0;
- data[(y_in - 1) * x_in + i] = 0;
- }
-
- for (i=0;i<y_in;i++){
- data[i * x_in] = 0;
- data[i * x_in + x_in - 1] = 0;
- }
-
-
-
- etat = 0;
- /*
- 0 look for color (object or edge)
- 1 look for edge end
- */
-
- for (i=1;i<x_in*y_in;i++){
- switch(etat){
- case 0:
- //we are in the dark
- switch(data[i]){
- case 0:
- //look for in dark
- break;
- /*
- case 1:
- case 2:
- case 0x15:
- case 0x24:
- */
- default:
- in_color = data[i];
- etat = 1;
- // we found an object
- object_find_bounding_box(data, x_in, y_in, (i-1)%x_in, (i-1)/x_in, data[i], 255, &o);
-
- ret = store_obj(sac_obj, max_obj, &o);
- /* if new object, process rotating calipers */
- if (ret){
- sac_obj_poly[obj_num].len = o.len;
- object_find_poly(data, x_in, y_in,
- (i-1)%x_in, (i-1)/x_in,
- data[i], 255, &sac_obj_poly[obj_num]);
- IMG_DEBUG("%d",sac_obj_poly[obj_num].pts_num);
- object_poly_to_convex(&sac_obj_poly[obj_num]);
- /*
- for (j=0;j<sac_obj_poly[obj_num].pts_num;j++){
- data[sac_obj_poly[obj_num].pts[j].y*x_in + sac_obj_poly[obj_num].pts[j].x] = 8;
- }*/
- sac_obj_poly[obj_num].color = data[i];
- obj_num++;
- }
-
- break;
-
- case DEFAULT_COLOR:
- //we must out of color
- break;
- }
- break;
-
- case 1:
- if (data[i] != in_color){
- i--;
- etat = 0;
- }
- /*
- we are in a color and want to go out of it
- */
- break;
-
- }
-
-
-
- }
-
-
- return data;
-
-
-}
-/* space between twin tower is 13 pixels*/
-#define SPACE_INTER_TWIN_TOWER (13)
-
-#define SPACE_INTER_TWIN_TOWER_TOLERENCE 3
-
-
-
-/* find best twin tower for each zone */
-void find_twin_tower(uint8_t zones_num, zone* zones, int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE],
- int16_t center_x, int16_t center_y)
-{
-
- uint8_t i, j;
- uint8_t z_n;
- int n1, n2;
- unsigned int z1, z2, z3;
- //int32_t scal1, scal2;
- vect_t v, v1, v2;
- line_t l;
- point_t p;
- unsigned int good_zone;
- double dist, dist2;
- unsigned int current_sister;
-
- /* init dummy sisters */
- for (i = 0; i < zones_num; i++)
- for (j = 0; j < MAX_SISTER_PER_ZONE; j++)
- sisters[i][j] = -1;
-
-
-
- for (z_n = 0; z_n < zones_num; z_n++){
- if (!zones[z_n].valid)
- continue;
-
- current_sister = 0;
-
- for (i = 0; i < zones_num; i++){
-
-
- /* we already have max sisters */
- if (current_sister >= MAX_SISTER_PER_ZONE)
- break;
-
-
- if (!zones[i].valid)
- continue;
-
-
- if (i == z_n)
- continue;
-
- /* twin tower must have same high */
- if (zones[i].h != zones[z_n].h)
- continue;
-
- IMG_DEBUG("test sisters (%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")",
- zones[z_n].p.x, zones[z_n].p.y,
- zones[i].p.x, zones[i].p.y);
-
-
- dist = sqrt( (zones[i].p.x - zones[z_n].p.x) * (zones[i].p.x - zones[z_n].p.x) +
- (zones[i].p.y - zones[z_n].p.y) * (zones[i].p.y - zones[z_n].p.y) );
-
-
- IMG_DEBUG("sister space is %2.2f may be near %d", dist, SPACE_INTER_TWIN_TOWER);
-
- /*
- twin tower must be close/far enought to drop lintel
- */
- if (ABS(dist - SPACE_INTER_TWIN_TOWER) > SPACE_INTER_TWIN_TOWER_TOLERENCE)
- continue;
-
-
- pts2line(&zones[i].p, &zones[z_n].p, &l);
-
- /*
- test the paralelism of the temple:
- zone may be on same distance from center
- */
-
-
- v1.x = zones[z_n].p.x - center_x;
- v1.y = zones[z_n].p.y - center_y;
-
- dist = vect_norm(&v1);
-
- v2.x = zones[i].p.x - center_x;
- v2.y = zones[i].p.y - center_y;
-
- dist2 = vect_norm(&v2);
-
- IMG_DEBUG("zone dist %2.2f %2.2f", dist, dist2);
- if (ABS(dist-dist2) > 3){
- IMG_DEBUG("bad parallelism %2.2f", ABS(dist-dist2));
- continue;
- }
-
-
-
-
- /* no other aligned tower to avoid dropping on a lintel
- * (3 aligned zone may mean lintel)
- */
-
- good_zone = 1;
-
- for (j = 0; j < zones_num; j++){
- if (j==i ||j == z_n)
- continue;
-
- /* if third zone, but lower */
- if (zones[j].h <= zones[i].h)
- continue;
-
- /*
- check distance from dropping zone to
- line formed by twin towers
- */
-
- proj_pt_line(&zones[j].p, &l, &p);
-
-
- /* test if projected point is in the segement */
-
-
- v.x = zones[z_n].p.x - zones[i].p.x;
- v.y = zones[z_n].p.y - zones[i].p.y;
-
- v1.x = p.x - zones[i].p.x;
- v1.y = p.y - zones[i].p.y;
-
- n1 = vect_pscal_sign(&v, &v1);
-
- v.x = -v.x;
- v.y = -v.y;
-
- v1.x = p.x - zones[z_n].p.x;
- v1.y = p.y - zones[z_n].p.y;
-
-
- n2 =vect_pscal_sign(&v, &v1);
-
- v.x = p.x - zones[j].p.x;
- v.y = p.y - zones[j].p.y;
-
- dist = vect_norm(&v);
- IMG_DEBUG("dist pt h %d n: (%d %d) (%"PRIi32" %"PRIi32") to line %2.2f", zones[j].h, n1, n2, zones[j].p.x, zones[j].p.y, dist);
-
-
- if ((n1>=0 && n2>=0) && dist < OBJECT_DIM+2.){
- good_zone = 0;
- break;
- }
-
-
- /* test if zone is far from points*/
-
- v1.x = zones[j].p.x - zones[z_n].p.x;
- v1.y = zones[j].p.y - zones[z_n].p.y;
-
- dist = vect_norm(&v1);
- IMG_DEBUG("dist pt to z1 %2.2f", dist);
-
- if (dist < OBJECT_DIM){
- good_zone = 0;
- break;
- }
-
- v2.x = zones[j].p.x - zones[i].p.x;
- v2.y = zones[j].p.y - zones[i].p.y;
-
-
- dist = vect_norm(&v2);
- IMG_DEBUG("dist pt to z2 %2.2f", dist);
-
- if (dist < OBJECT_DIM){
- good_zone = 0;
- break;
- }
-
-
-
- z1 = i;
- z2 = z_n;
- z3 = j;
-
-
-
-
-
- /*
- XXX may be a lintel on lintel !!
- */
-
- }
-
- if (!good_zone)
- continue;
-
- IMG_DEBUG("sisters ok (%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")",
- zones[z_n].p.x, zones[z_n].p.y,
- zones[i].p.x, zones[i].p.y);
-
-
- sisters[z_n][current_sister] = i;
- current_sister++;
- }
- }
-}
-
-/* test if a higher zone is too close */
-int test_close_zone(uint8_t zones_num, zone* zones, unsigned int z_n)
-{
- uint8_t i;
- vect_t v;
- double dist;
-
- for (i = 0; i < zones_num; i++){
- if (i == z_n)
- continue;
- if (zones[i].h <= zones[z_n].h)
- continue;
-
- v.x = zones[i].p.x - zones[z_n].p.x;
- v.y = zones[i].p.y - zones[z_n].p.y;
-
- dist = vect_norm(&v);
- //IMG_DEBUG("dist pt to pt %2.2f", dist);
-
- if (dist < OBJECT_DIM){
- return 1;
- }
-
- }
-
- return 0;
-}
-
-#define MAX_COLUMN 4
-#define MAX_LINTEL 2
-
-drop_column_zone drop_c[MAX_COLUMN];
-drop_lintel_zone drop_l[MAX_LINTEL];
-
-
-void reset_drop_zone(void)
-{
- memset(drop_c, 0, sizeof(drop_c));
- memset(drop_l, 0, sizeof(drop_l));
-
-}
-
-
-void display_drop_zones(uint8_t n_columns, uint8_t n_lintels, zone* zones)
-{
- unsigned int i;
-
- for (i=0;i<n_columns;i++)
- IMG_NOTICE("c %d:(h:%d) (%"PRIi32" %"PRIi32") valid=%d",
- i, drop_c[i].h,
- zones[drop_c[i].z].p.x, zones[drop_c[i].z].p.y,
- drop_c[i].valid);
-
- for (i=0;i<n_lintels;i++)
- IMG_NOTICE("l %d:(h:%d) (%"PRIi32" %"PRIi32") "
- "(%"PRIi32" %"PRIi32") valid=%d",
- i, drop_l[i].h,
- zones[drop_l[i].z1].p.x, zones[drop_l[i].z1].p.y,
- zones[drop_l[i].z2].p.x, zones[drop_l[i].z2].p.y, drop_l[i].valid);
-
-}
-
-#define MY_MAX(a, b) ((a)>(b)?(a):(b))
-
-
-#if 0
-#define MAX_DROP_HIGH 8
-
-
-/*
- recursive function to maximize points during object
- dropping, given lintel/column number
- working zone may be 1, 2 or 3
- */
-unsigned int solve_objects_dropping(unsigned int points, unsigned int points_max,
- uint8_t n_columns, uint8_t n_lintels,
- uint8_t zones_num, zone* zones, int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE], uint8_t working_zone)
-{
-
- uint8_t i, j;
- unsigned int points_calc;
- //unsigned int points_added = 0;
- int sister;
- int ret;
-
-
- /* if no more objects, return points */
- if (n_columns == 0 && n_lintels == 0)
- return MY_MAX(points, points_max);
-
- /* start by putting columns if so */
- for (i = 0; i < zones_num; i++){
- if (zones[i].h >= MAX_DROP_HIGH)
- continue;
-
- if (n_columns){
-
- ret = test_close_zone(zones_num, zones, i);
- if (ret)
- continue;
-
- zones[i].h++;
- points_calc = solve_objects_dropping(points + zones[i].h, points_max,
- n_columns - 1, n_lintels,
- zones_num, zones, sisters, working_zone);
-
- if (points_calc > points_max){
- points_max = points_calc;
- drop_c[n_columns - 1].z = i;
- drop_c[n_columns - 1].h = zones[i].h;
- drop_c[n_columns - 1].valid = 1;
-
- }
- zones[i].h--;
- }
- /* we must depose all columns before dropping lintels */
- else if (n_lintels){
-
- /* dont drop lintel on ground */
- if (zones[i].h <= working_zone)
- continue;
-
- /* XXX todo need get second zone near selected one */
- //ret = find_twin_tower(zones_num, zones, i, &sister);
-
- for (j = 0; j < MAX_SISTER_PER_ZONE; j++){
- sister = sisters[i][j];
- if (sister == -1)
- break;
- if (zones[i].h != zones[sister].h){
- sister = -1;
- }
-
- }
-
- if (sister == -1)
- continue;
- //IMG_DEBUG("sister found: %d %d (h=%d %p)", i, sister, zones[i].h, &zones[i].h);
-
- zones[i].h++;
- zones[sister].h++;
-
-
- points_calc = solve_objects_dropping(points + zones[i].h * 3, points_max,
- n_columns, n_lintels - 1,
- zones_num, zones, sisters, working_zone);
-
- if (points_calc > points_max){
- points_max = points_calc;
-
- drop_l[n_lintels - 1].z1 = i;
- drop_l[n_lintels - 1].z2 = sister;
- drop_l[n_lintels - 1].h = zones[i].h;
- drop_l[n_lintels - 1].valid = 1;
-
- }
-
-
- zones[sister].h--;
- zones[i].h--;
- }
- }
-
- return MY_MAX(points, points_max);
-}
-#endif
-
-
-/* */
-int find_column_dropzone(uint8_t zones_num, zone* zones)
-{
- uint8_t i;
-
- uint8_t z_n = 0;
-
- if (zones_num <= 0)
- return -1;
-
- for (i = 0; i < zones_num; i++){
- if (!zones[i].valid)
- continue;
- if (zones[i].h > zones[z_n].h)
- z_n = i;
- }
-
-
- /*
- now, chose dropzone closest to robot
- meaning little x, big y
- so maximise y-x
- */
- for (i = 0; i < zones_num; i++){
- if (zones[i].h != zones[z_n].h)
- continue;
- if (!zones[i].valid)
- continue;
- if (zones[i].p.y - zones[i].p.x > zones[z_n].p.y - zones[z_n].p.x)
- z_n = i;
- }
-
-
-
- return z_n;
-}
-
-
-
-uint8_t color2h(uint8_t color)
-{
- return (0x100-color)/0x20;
-}
-
-uint8_t h2color(uint8_t color)
-{
- return color*0x20;
-}
-
-
-#define NUM_ZONE_GENERATE 8
-#define DIST_ZONE_GEN 9
-
-/*
- remove zone at ground level, and generate zones on
- a circle at X cm from center
- */
-unsigned int generate_center_ground_zones(unsigned char* data, int16_t x_in, int16_t y_in,
- zone * zones, unsigned int zones_num, uint8_t max_zones, int16_t center_x, int16_t center_y)
-{
- double c_a, s_a;
- uint8_t i, j;
- double px1, py1, px2, py2;
-
-
- /* first del zone at level 2 */
- for (i = 0; i < zones_num; ){
- if (zones[i].h!=2){
- i++;
- continue;
- }
-
- for (j = i; j < zones_num-1; j++)
- zones[j] = zones[j+1];
-
- zones_num--;
-
- }
-
- /* generate zones around circle */
-
- c_a = cos(2*M_PI/NUM_ZONE_GENERATE);
- s_a = sin(2*M_PI/NUM_ZONE_GENERATE);
-
- px1 = DIST_ZONE_GEN;
- py1 = 0;
-
- for (i = 0; i < NUM_ZONE_GENERATE; i++){
-
- zones[zones_num].p.x = center_x + px1;
- zones[zones_num].p.y = center_y + py1;
- zones[zones_num].h = 2;
- zones[zones_num].valid = 1;
-
-
- px2 = px1*c_a + py1*s_a;
- py2 = -px1*s_a + py1*c_a;
-
- px1 = px2;
- py1 = py2;
-
- /* skip zone if it is not in img */
- if (zones[zones_num].p.x < 0 || zones[zones_num].p.y < 0 ||
- zones[zones_num].p.x >= x_in || zones[zones_num].p.y > y_in)
- continue;
-
- /* skip zone if not enougth pixels */
- if (!zone_has_enought_pixels(data, x_in, y_in, &zones[zones_num]))
- continue;
-
- zones_num++;
- if (zones_num >= max_zones)
- break;
-
- }
-
- return zones_num;
-
-
-}
-
-
-/*
- remove zone at ground level, and generate zones on
- a line at X cm from robot
- */
-unsigned int generate_rectangle_ground_zones(unsigned char* data, int16_t x_in, int16_t y_in,
- zone * zones, unsigned int zones_num, uint8_t max_zones, int16_t center_x, int16_t center_y,
- uint8_t working_zone)
-{
- uint8_t i, j;
- uint8_t y;
-
- /* first del zone at level i */
- for (i = 0; i < zones_num; ){
- if (zones[i].h != working_zone ){
- i++;
- continue;
- }
-
- for (j = i; j < zones_num-1; j++)
- zones[j] = zones[j+1];
-
- zones_num--;
- }
-
-
- /* generate zones on a line */
- for (y = OBJECT_DIM; y < y_in; y+=OBJECT_DIM){
-
- zones[zones_num].p.x = center_x;
- zones[zones_num].p.y = y;
- zones[zones_num].h = working_zone ;
- zones[zones_num].valid = 1;
-
- if (!zone_has_enought_pixels(data, x_in, y_in, &zones[zones_num]))
- continue;
- zones_num++;
-
- if (zones_num >= max_zones)
- break;
-
- }
- return zones_num;
-
-}
-
-
-#define MAX_DECAL_LINE 5
-#define ENOUGHT_ZONE_PIXEL 2
-
-void recal_img_y(unsigned char* buffer, int16_t x_in, int16_t y_in,
- uint8_t working_zone)
-{
- uint8_t i, j;
- uint8_t cpt;
-
- /* recal img only for central zone */
- if (working_zone !=2)
- return;
-
- for (i = 0; i < MAX_DECAL_LINE; i++){
- cpt = 0;
- for (j = 0; j < x_in; j++){
- if (buffer[i*x_in + j] ==2)
- cpt++;
- }
-
- if (cpt >= ENOUGHT_ZONE_PIXEL)
- break;
- }
-
- memmove(buffer, &buffer[i * x_in], x_in * y_in - i*x_in);
- memset(&buffer[x_in * y_in - i * x_in], 0, i*x_in);
-}
-
-
-#define MAX_OBJECTS 20
-
-#define MAX_ZONES_PER_OBJECT 20
-
-
-uint8_t g_zone_num;
-zone g_all_zones[MAX_ZONES];
-
-uint8_t process_img(unsigned char *buffer, int16_t x_in, int16_t y_in,
- zone * all_zones, uint8_t max_zones)
-{
-
- int ret;
- int i, j;
-
- zone zones[MAX_ZONES_PER_OBJECT];
-
- vect_t caliper;
- unsigned int pts_cal[4];
- point_t ptmp;
- vect_t r1, r2;
- int zone_len;
-
-
- uint8_t zone_num = 0;
-
- Object_bb sac_obj[MAX_OBJECTS];
- Object_poly sac_obj_poly[MAX_OBJECTS];
-
-
-
- /*
- XXX fix: only decal for working zone 2/(1?)
- but we dont have info yet
- */
- recal_img_y(buffer, x_in, y_in, 2);
-
- memset(sac_obj, 0, sizeof(sac_obj));
- memset(sac_obj_poly, 0, sizeof(sac_obj_poly));
-
-
- /* first, find polygons*/
- parcour_img(buffer, x_in, y_in, sac_obj, sac_obj_poly, MAX_OBJECTS);
-
- /* enclose each poygon in the min area polygon
- then, split each rectangle in dropping zone
- */
- for (i=0;i<MAX_OBJECTS;i++){
-
- if (!sac_obj_poly[i].pts_num)
- continue;
-
- IMG_DEBUG("obj: %d %d %d %d %d",
- i,
- sac_obj[i].x_min,
- sac_obj[i].y_min,
- sac_obj[i].x_max,
- sac_obj[i].y_max);
-
- //IMG_DEBUG("poly pts_num: %d", sac_obj_poly[i].pts_num);
-
- object_poly_get_min_ar(&sac_obj_poly[i], &pts_cal[0], &caliper, &r1, &r2);
-
- ret = object_poly_caliper_to_rectangle(&sac_obj_poly[i], &pts_cal[0], &caliper,
- &r1, &r2, &ptmp);
-
- if (!ret)
- continue;
-
- /*
- IMG_DEBUG("r: (%3"PRIi32" %3"PRIi32") "
- "(%3"PRIi32" %3"PRIi32")",
- r1.x, r1.y, r2.x, r2.y);
- IMG_DEBUG("intersection: %"PRIi32" %"PRIi32"",
- ptmp.x, ptmp.y);
- */
-
- zone_len = split_rectangle(&ptmp, &r1, &r2,
- MAX_ZONES_PER_OBJECT, &zones[0], sac_obj_poly[i].color);
- //IMG_DEBUG("split ok %d", zone_len);
-
- zone_len = zone_filter_min_surface(buffer, x_in, y_in,
- sac_obj_poly[i].color,
- zone_len, &zones[0]);
-
- for (j = 0; j < zone_len && zone_num < max_zones; zone_num++, j++)
- all_zones[zone_num] = zones[j];
-
- }
-
-
- IMG_NOTICE("num zones end: %d", zone_num);
-
- return zone_num;
-}
-
-
-void process_img_to_zone(unsigned char *buffer, int16_t x_in, int16_t y_in)
-{
- g_zone_num = process_img(buffer, x_in, y_in,
- g_all_zones, MAX_ZONES);
-}
-
-uint8_t filter_zones(unsigned char *buffer, int16_t x_in, int16_t y_in,
- zone * all_zones, uint8_t zone_num, uint8_t max_zones,
- uint8_t working_zone, int16_t center_x, int16_t center_y,
- int tweak_min_margin)
-{
- uint8_t i;
-
- /* first valid all zones */
- for (i = 0; i < zone_num; i++){
- all_zones[i].valid = 1;
-
- /* filter zone lower thatn working zone */
- if (all_zones[i].h < working_zone)
- all_zones[i].valid = 0;
- }
-
-
- /*
- generate correct zone at ground level
- (depending on working zone)
- */
- if (working_zone == 2)
- zone_num = generate_center_ground_zones(buffer, x_in, y_in,
- all_zones, zone_num, max_zones, center_x, center_y);
- else
- zone_num = generate_rectangle_ground_zones(buffer, x_in, y_in,
- all_zones, zone_num, max_zones, center_x, center_y,
- working_zone);
-
- /* filter zone position, depending on workingzone */
- if (working_zone == 2)
- zone_num = zone_filter_center(zone_num, all_zones, center_x, center_y, tweak_min_margin);
- else
- zone_num = zone_filter_zone_rect(zone_num, all_zones, center_x, center_y , working_zone);
-
-
-
- /* display zones (debug purpose) */
-
- for (i = 0; i < zone_num; i++){
- //buffer[all_zones[i].p.y*x_in+all_zones[i].p.x] = 0x3;
- IMG_NOTICE("h:%d (v:%d) x:%"PRIi32" y:%"PRIi32"", all_zones[i].h, all_zones[i].valid, all_zones[i].p.x, all_zones[i].p.y);
-
- }
-
- IMG_NOTICE("num zones: %d", zone_num);
-
-
-
-
- return zone_num;
-}
-
-
-/*
- return -1 if not column dropzone is found
- return column hight if found
-*/
-int8_t get_column_dropzone(unsigned char *buffer, int16_t x_in, int16_t y_in,
- uint8_t working_zone, int16_t center_x, int16_t center_y,
- int16_t * dropzone_x, int16_t * dropzone_y)
-{
- uint8_t zone_num;
- int c_drop_zone;
-
-
-
- zone_num = filter_zones(buffer, x_in, y_in,
- g_all_zones, g_zone_num, MAX_ZONES,
- working_zone, center_x, center_y,
- 0);
-
- c_drop_zone = find_column_dropzone(zone_num, g_all_zones);
-
- if (c_drop_zone<0)
- return -1;
-
- *dropzone_x = g_all_zones[c_drop_zone].p.x;
- *dropzone_y = g_all_zones[c_drop_zone].p.y;
-
- *dropzone_x = (*dropzone_x) * PIXEL2CM;
- *dropzone_y = (*dropzone_y) * PIXEL2CM + 30;
-
- return g_all_zones[c_drop_zone].h;
-}
-
-#define ROBOT_SEMI_INTERCOLUMN_SPACE 75
-
-uint8_t is_temple_there(unsigned char * buffer, int16_t x_in, int16_t y_in,
- uint8_t h, int16_t center_x, int16_t center_y)
-{
- zone z;
- int ret;
- int i;
-
-
- IMG_NOTICE("test z (mm) : x:%d y:%d", center_x, center_y);
- IMG_NOTICE("test z:(pix): x:%d y:%d", (int)(center_x/ PIXEL2CM), (int)(center_y/ PIXEL2CM));
-
-
- z.p.x = center_x / PIXEL2CM;
- z.p.y = (center_y - ROBOT_SEMI_INTERCOLUMN_SPACE) / PIXEL2CM ;
- z.h = h;
- z.valid = 1;
- ret = zone_has_enought_pixels(buffer, x_in, y_in, &z);
- IMG_NOTICE("test z1: %d", ret);
-
- if (!ret)
- return 0;
-
- z.p.x = center_x / PIXEL2CM;
- z.p.y = (center_y + ROBOT_SEMI_INTERCOLUMN_SPACE)/ PIXEL2CM;
- z.h = h;
- z.valid = 1;
- ret = zone_has_enought_pixels(buffer, x_in, y_in, &z);
- IMG_NOTICE("test z2: %d", ret);
- if (!ret)
- return 0;
-
-
- /*
- if middle zone is less or egual to tested temple
- */
- z.p.x = center_x / PIXEL2CM;
- z.p.y = center_y / PIXEL2CM;
-
- for (i = h; i > 0; i--){
- z.h = i;
- z.valid = 1;
- ret = zone_has_enought_pixels(buffer, x_in, y_in, &z);
- IMG_NOTICE("test z3:(h=%d) %d", i, ret);
-
- if (ret)
- return 1;
- }
-
- return 0;
-
-}
-
-
-int8_t find_temple_dropzone(unsigned char *buffer, int16_t x_in, int16_t y_in,
- uint8_t working_zone, int16_t center_x, int16_t center_y,
- int16_t * dropzone_x, int16_t * dropzone_y)
-{
- uint8_t zone_num;
- int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE];
- int8_t z_n = -1;
- uint8_t i, j;
-
-
- /* find all drop zone */
- zone_num = filter_zones(buffer, x_in, y_in,
- g_all_zones, g_zone_num, MAX_ZONES,
- working_zone, center_x, center_y,
- -2);
-
- /* precompute possible twin towers */
- find_twin_tower(zone_num, g_all_zones, sisters, center_x, center_y);
-
-
- for (i=0; i< zone_num; i++){
- IMG_DEBUG("all sisters: %d", i);
- for (j=0;j<MAX_SISTER_PER_ZONE;j++){
- IMG_DEBUG("s: %d", sisters[i][j]);
- }
- }
-
- /* only use first sister of each zone */
- for (i=0; i< zone_num; i++){
-
- /* if zone doesn't have twin tower */
- if (sisters[i][0] == -1)
- continue;
-
- if (!g_all_zones[i].valid)
- continue;
-
- if (z_n == -1){
- z_n = i;
- continue;
- }
-
- /* if we found higher twin tower */
- if (g_all_zones[z_n].h > g_all_zones[i].h)
- continue;
-
- z_n = i;
- }
-
- IMG_NOTICE("twin tower found :z_n=%d", z_n);
- if (z_n == -1)
- return -1;
-
- IMG_NOTICE("(%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")",
- g_all_zones[z_n].p.x, g_all_zones[z_n].p.y,
- g_all_zones[sisters[z_n][0]].p.x, g_all_zones[sisters[z_n][0]].p.y);
-
-
- *dropzone_x = ((double)(g_all_zones[z_n].p.x + g_all_zones[sisters[z_n][0]].p.x)*PIXEL2CM ) / 2;
- *dropzone_y = ((double)(g_all_zones[z_n].p.y + g_all_zones[sisters[z_n][0]].p.y)*PIXEL2CM ) / 2 + 30;
-
-
- return g_all_zones[z_n].h;
-
-}
-
git.droids-corp.org / aversive.git / blobdiff