int main(void)
{
const unsigned short MainCycle = 60;
Init(MainCycle);
int i;
Wait(1000);
for(i=0; i<N; i++)
{
go(length);
if(is_black_right()){
left_turn(2);
} else if(is_black_left()){
right_turn(2);
}
if(is_black()) {
break;
}
}
stop();
tyarumera();
return 0;
}
//test case for left_turn(int last_move)
int LEFT2 (){
START_TEST_CASE;
int i= M_SOUTH;
int result= left_turn(i);
SHOULD_BE(result==M_EAST);
END_TEST_CASE;
}
//test case for left_turn(int last_move)
int LEFT1 (){
START_TEST_CASE;
int i= M_NORTH;
int result= left_turn(i);
SHOULD_BE(result==M_WEST);
END_TEST_CASE;
}
void main(void)
{
vInitialize();
myInit();
while(!pushsw_get())
{
if(cnt1<100)
led_out(0x1);
else if(cnt1<200)
led_out(0x2);
else cnt1 = 0;
}
cnt1 = 0;
while(startbar_get())
{
if(cnt1<50)
led_out(0x1);
else if(cnt1<100)
led_out(0x2);
else cnt1 = 0;
}
led_out(0);
start();
pattern = 10;
while(1)
{
switch(pattern)
{
case 10: //直线
if(check_crossline()){ pattern = 30;break; }
if(check_blackArea()){ pattern = 40;break; }
straight_run(); //直线前行，弯道检测，盲区检测
break;
case 20: //弯道
switch(turn_direction)
{
case LEFT:
handle(-1*midDegree_st4);
left_turn();
break;
case RIGHT:
handle(midDegree_st4);
right_turn();
break;
default:break;
}
break;
case 30: //直角
rightAngle();
break;
case 40: //盲区
switch(blackArea_direction)
{
case LEFT:
left_blackArea();
break;
case RIGHT:
right_blackArea();
break;
default:break;
}
break;
default:break;
}//end switch
}//end while
}
bool build_convex_polygon(
Pmwx::Ccb_halfedge_circulator ccb,
vector<pair<Pmwx::Halfedge_handle, Pmwx::Halfedge_handle> >& sides,
const CoordTranslator2& trans,
Polygon2& metric_bounds,
double max_err_mtrs,
double min_side_len)
{
double e_sq = max_err_mtrs*max_err_mtrs;
sides.clear();
metric_bounds.clear();
Pmwx::Ccb_halfedge_circulator circ(ccb);
// Bbox2 bounds;
//
// do {
// bounds += cgal2ben(circ->source()->point());
// } while (++circ != ccb);
Pmwx::Ccb_halfedge_circulator start,next;
start = ccb;
do {
--start;
if(!sides_can_merge(start,ccb))
break;
if(!within_err_metric(start,ccb,trans,e_sq))
break;
} while(start != ccb);
++start;
// now we can go around.
circ = start;
//int ne = count_circulator(start);
//printf("Poly has %d sides.\n", ne);
do {
Pmwx::Ccb_halfedge_circulator stop(circ);
do {
++stop;
} while(sides_can_merge(circ,stop) && within_err_metric(circ,stop,trans,e_sq) && stop != start);
--stop;
//printf("Pushing side of %d, %d\n", circulator_distance_to(start, circ),circulator_distance_to(start,stop));
sides.push_back(pair<Pmwx::Halfedge_handle,Pmwx::Halfedge_handle>(circ, stop));
++stop;
circ = stop;
} while(circ != start);
if(sides.size() < 3)
{
//debug_mesh_point(bounds.centroid(),1,1,1);
return false;
}
int i, j, k;
vector<Segment2> msides;
for(i = 0; i < sides.size(); ++i)
{
j = (i + 1) % sides.size();
DebugAssert(sides[i].second->target() == sides[j].first->source());
msides.push_back(Segment2(
trans.Forward(cgal2ben(sides[i].first->source()->point())),
trans.Forward(cgal2ben(sides[i].second->target()->point()))));
}
vector<Segment2> debug(msides);
for(i = 0; i < sides.size(); ++i)
{
j = (i + 1) % sides.size();
Vector2 v1(msides[i].p1,msides[i].p2);
Vector2 v2(msides[j].p1,msides[j].p2);
v1.normalize();
v2.normalize();
if(v1.dot(v2) > 0.9998 ||
!v1.left_turn(v2))
{
//debug_mesh_point(trans.Reverse(msides[i].p2),1,0,0);
return false;
}
double w = width_for_he(sides[i].first);
if(w)
{
v1 = v1.perpendicular_ccw();
v1 *= w;
msides[i].p1 += v1;
msides[i].p2 += v1;
}
}
for(j = 0; j < sides.size(); ++j)
{
i = (j + sides.size() - 1) % sides.size();
Line2 li(msides[i]), lj(msides[j]);
Point2 p;
if(!li.intersect(lj,p))
{
Assert(!"Failure to intersect.\n");
return false;
}
metric_bounds.push_back(p);
}
for(i = 0; i < metric_bounds.size(); ++i)
{
j = (i + 1) % metric_bounds.size();
k = (i + 2) % metric_bounds.size();
if(metric_bounds.side(i).squared_length() < (min_side_len*min_side_len))
{
//debug_mesh_line(trans.Reverse(metric_bounds.side(i).p1),trans.Reverse(metric_bounds.side(i).p2),1,1,0,1,1,0);
return false;
}
if(!left_turn(metric_bounds[i],metric_bounds[j],metric_bounds[k]))
{
//debug_mesh_point(trans.Reverse(metric_bounds[j]),1,1,0);
return false;
}
if(Vector2(msides[i].p1,msides[i].p2).dot(Vector2(metric_bounds[i],metric_bounds[j])) < 0.0)
{
//debug_mesh_line(trans.Reverse(msides[i].p1),trans.Reverse(msides[i].p2),1,0,0,1,0,0);
return false;
}
}
DebugAssert(metric_bounds.size() == msides.size());
DebugAssert(msides.size() == sides.size());
return true;
}
/** the algorithm that computes the visibility graph
*/
void construct_visibility(struct Point *points, int num_points,
struct Line *lines, int num_lines,
struct Map_info *out)
{
struct Point *p, *p_r, *q, *z;
struct Point *p_infinity, *p_ninfinity;
int i;
p_ninfinity = (struct Point *)malloc(sizeof(struct Point));
p_infinity = (struct Point *)malloc(sizeof(struct Point));
p_ninfinity->x = PORT_DOUBLE_MAX;
p_ninfinity->y = -PORT_DOUBLE_MAX;
p_ninfinity->father = NULL;
p_ninfinity->left_brother = NULL;
p_ninfinity->right_brother = NULL;
p_ninfinity->rightmost_son = NULL;
p_infinity->x = PORT_DOUBLE_MAX;
p_infinity->y = PORT_DOUBLE_MAX;
p_infinity->father = NULL;
p_infinity->left_brother = NULL;
p_infinity->right_brother = NULL;
p_infinity->rightmost_son = NULL;
init_stack(num_points);
/* sort points in decreasing x order */
quickSort(points, 0, num_points - 1);
/* initialize the vis pointer of the vertices */
init_vis(points, num_points, lines, num_lines);
add_rightmost(p_ninfinity, p_infinity);
for (i = 0; i < num_points; i++) {
add_rightmost(&points[i], p_ninfinity);
}
push(&points[0]);
/* main loop */
while (!empty_stack()) {
p = pop();
p_r = right_brother(p);
q = father(p);
/* if the father is not -infinity, handle p and q */
if (q != p_ninfinity) {
handle(p, q, out);
}
z = left_brother(q);
remove_point(p);
/* remove and reattach p to the tree */
if (z == NULL || !left_turn(p, z, father(z))) {
add_leftof(p, q);
}
else {
while (rightmost_son(z) != NULL &&
left_turn(p, rightmost_son(z), z))
z = rightmost_son(z);
add_rightmost(p, z);
if (z == top())
z = pop();
}
/* if p not attached to infinity, then p has more points to visit */
if (left_brother(p) == NULL && father(p) != p_infinity) {
push(p);
}
/* and continue with the next one ( from left to right ) */
if (p_r != NULL) {
push(p_r);
}
}
G_free(p_infinity);
G_free(p_ninfinity);
}
/** for a pair (p, q) of points, add the edge pq if their are visible to each other
*/
void handle(struct Point *p, struct Point *q, struct Map_info *out)
{
/* if it's a point without segments, just report the edge */
if (segment1(q) == NULL && segment2(q) == NULL && before(p, q, p->vis)) {
report(p, q, out);
}
else if (segment1(p) != NULL && q == other1(p)) {
/* we need to check if there is another segment at q so it can be set to the vis */
if (segment1(q) == segment1(p) && segment2(q) != NULL &&
left_turn(p, q, other2(q))) {
p->vis = segment2(q);
}
else if (segment2(q) == segment1(p) && segment1(q) != NULL &&
left_turn(p, q, other1(q))) {
p->vis = segment1(q);
}
else
p->vis = q->vis;
report(p, q, out);
}
else if (segment2(p) != NULL && q == other2(p)) {
/* we need to check if there is another segment at q so it can be set to the vis */
if (segment1(q) == segment2(p) && segment2(q) != NULL &&
left_turn(p, q, other2(q))) {
p->vis = segment2(q);
}
else if (segment2(q) == segment2(p) && segment1(q) != NULL &&
left_turn(p, q, other1(q))) {
p->vis = segment1(q);
}
else
p->vis = q->vis;
report(p, q, out);
}
else if (segment1(q) == p->vis && segment1(q) != NULL) {
/* we need to check if there is another segment at q so it can be set to the vis */
if (segment2(q) != NULL && left_turn(p, q, other2(q)))
p->vis = segment2(q);
else
p->vis = q->vis;
/* check that p and q are not on the same boundary and that the edge pq is inside the boundary */
if (p->cat == -1 || p->cat != q->cat ||
!point_inside(p, (p->x + q->x) * 0.5, (p->y + q->y) * 0.5))
report(p, q, out);
}
else if (segment2(q) == p->vis && segment2(q) != NULL) {
/* we need to check if there is another segment at q so it can be set to the vis */
if (segment1(q) != NULL && left_turn(p, q, other1(q)))
p->vis = segment1(q);
else
p->vis = q->vis;
/* check that p and q are not on the same boundary and that the edge pq is inside the boundary */
if (p->cat == -1 || p->cat != q->cat ||
!point_inside(p, (p->x + q->x) * 0.5, (p->y + q->y) * 0.5))
report(p, q, out);
}
else if (before(p, q, p->vis)) {
/* if q only has one segment, then this is the new vis */
if (segment2(q) == NULL)
p->vis = segment1(q);
else if (segment1(q) == NULL)
p->vis = segment2(q);
/* otherwise take the one with biggest slope */
else if (left_turn(p, q, other1(q)) && !left_turn(p, q, other2(q)))
p->vis = segment1(q);
else if (!left_turn(p, q, other1(q)) && left_turn(p, q, other2(q)))
p->vis = segment2(q);
else if (left_turn(q, other2(q), other1(q)))
p->vis = segment1(q);
else
p->vis = segment2(q);
/* check that p and q are not on the same boundary and that the edge pq is inside the boundary */
if (p->cat == -1 || p->cat != q->cat ||
!point_inside(p, (p->x + q->x) * 0.5, (p->y + q->y) * 0.5))
report(p, q, out);
}
}
