Exemplo n.º 1
0
FuzzyIndices::CampelloIndices::CampelloIndices(FuzzyIndices& outer) :
		outer(outer) {
	std::shared_ptr<std::vector<int>> elements = outer.elements();
	a = compute_a(*elements.get());
	b = compute_b(*elements.get());
	c = compute_c(*elements.get());
	d = compute_d(*elements.get());
}
Exemplo n.º 2
0
CNDCGRankLoss::CNDCGRankLoss(CModel* &model, CVecData* &data)
  : CRankLoss(model, data) 
{
  truncation = 10;
  // do nothing serious here at the moment...                       
  std::cout << "In CNDCGRankLoss!" << std::endl;
  int num_query = _data->NumOfSubset();
  Scalar *Y_array = _data->labels().Data();
  max_subset_size = 0;
  for (int q=0;q<num_query;q++){
    int offset = _data->subset[q].startIndex;
    int subsetsize = _data->subset[q].size;
    if (max_subset_size < subsetsize){
      max_subset_size = subsetsize;
    }
    int length = subsetsize;
    vector<int> indices(length);
    iota(indices.begin(), indices.end(), 0);
    indirect_greater_than<Scalar> igt(&(Y_array[offset]));
    //indirect_less_than<Scalar> igt(&(Y_array[offset]));
    sort(indices.begin(), indices.end(),igt);
    sort_vectors.push_back(indices);

    // use this to compute bs
    vector<double> b;
    compute_b(offset, subsetsize, Y_array, indices, b);
    bs.push_back( b );
  }
  // use largest query information to compute c_i
  for (int i=0;i<max_subset_size;i++){
    //c.push_back( 1.0/pow((i+1.0),1.4) );
    c.push_back( max(truncation+1-i,0) );
  }
  pi = (int *) malloc(sizeof(int)*max_subset_size);
  compute_a(max_subset_size, a); 
  //cout << "finished init\n";
  //cout << "max_subset_size " << max_subset_size <<" \n";
  //cout << "a.size " << a.size() <<" \n";
  //cout << "c.size " << c.size() <<" \n";

}
Exemplo n.º 3
0
/*
 *if task_type == TASK_RUN,setup task_segList;
 */
static void runTask_init(task t)
{
    mylogfd(-1,"[Control]/motor/runTask_init\n");
    int i = 0;;
    double rate = 1;
    int delta = 0;
    double r = 0 ;
    maxa = t.maxa;
    Point *circle;
    circle = (Point*)malloc(sizeof(struct _Point));
    for (i = 0; i < task_segNum; i++) {
        if (fabs((vleft + vright)/2 * (vleft + vright)/2)
                >=2*maxa*(task_segNum-i-1)*SEGDIS) {
            break;
        }
        task_segList[i].type = TASK_RUN;
        task_segList[i].start.x = t.trace->ps[i].x;
        task_segList[i].start.y = t.trace->ps[i].y;
        task_segList[i].end.x = t.trace->ps[i+1].x;
        task_segList[i].end.y = t.trace->ps[i+1].y;
        if ((task_segList[i].start.x == task_segList[i].end.x)
                &&(task_segList[i].start.y == task_segList[i].end.y)) {
            task_segList[i].type = TASK_ROTA;
            int rot = t.trace->direction[i+1]-t.trace->direction[i];
            if (rot > 0) {
                task_segList[i].lspeed = -ROTV;
                task_segList[i].rspeed = ROTV;
            } else {
                task_segList[i].lspeed = ROTV;
                task_segList[i].rspeed = -ROTV;
            }
            task_segList[i].rotangle = rot;
            task_segList[i].dur_time = compute_t(task_segList[i].rotangle,
                                                 DD/2,task_segList[i].lspeed);
            continue;
        }
        //直线
        if (t.trace->direction[i] == t.trace->direction[i+1]) {
            if (t.trace->inverse[i] == 1) { //反向
                if (vleft != vright) {
                    if (fabs(vleft) > fabs(vright)) {
                        if (vleft < 0)
                            vright = vleft;
                        else {
                            vleft = 0;
                            vright = 0;
                        }
                    } else {
                        if (vright < 0)
                            vleft = vright;
                        else {
                            vleft = 0;
                            vright = 0;
                        }
                    }
                }
                task_segList[i].lspeed = vleft - ADDV;
                //vleft = task_segList[i].lspeed;
                task_segList[i].rspeed = vright - ADDV;
                //vright = task_segList[i].rspeed;
            } else { //正向
                if (vleft != vright) {
                    if (fabs(vleft) > fabs(vright)) {
                        if (vleft > 0)
                            vright = vleft;
                        else {
                            vleft = 0;
                            vright = 0;
                        }
                    } else {
                        if (vright > 0)
                            vleft = vright;
                        else {
                            vleft = 0;
                            vright = 0;
                        }
                    }
                }
                task_segList[i].lspeed = vleft + ADDV;
                //vleft = task_segList[i].lspeed;
                task_segList[i].rspeed = vright + ADDV;
                //vright = task_segList[i].rspeed;
            }
            if (task_segList[i].lspeed > MAXV)
                task_segList[i].lspeed = MAXV;
            if (task_segList[i].lspeed < -MAXV)
                task_segList[i].lspeed = -MAXV;
            if (task_segList[i].rspeed > MAXV)
                task_segList[i].rspeed = MAXV;
            if (task_segList[i].rspeed < -MAXV)
                task_segList[i].rspeed = -MAXV;

            double d = sqrt((t.trace->ps[i].x - t.trace->ps[i+1].x)
                            *(t.trace->ps[i].x - t.trace->ps[i+1].x)
                            +(t.trace->ps[i].y - t.trace->ps[i+1].y)
                            *(t.trace->ps[i].y - t.trace->ps[i+1].y));
            task_segList[i].dur_time = fabs(d / task_segList[i].lspeed);
            compute_a(&task_segList[i].lspeed,&task_segList[i].rspeed,
                      task_segList[i].dur_time,t.trace->inverse[i],1);
            task_segList[i].dur_time = fabs(d / task_segList[i].lspeed);
        }

        else { //圆弧
            //计算圆心
            compute_c(circle,t.trace->ps[i],t.trace->ps[i+1],
                      t.trace->direction[i],t.trace->direction[i+1]);
            //计算半径
            r = t.trace->r[i];
            //速度比
            rate = (r+DD/2)/(r-DD/2);
            if (t.trace->direction[i] < t.trace->direction[i+1])
                rate = 1/rate;
            //右旋
            if (rate > 1) {
                if (t.trace->inverse[i] == 0) {
                    if (vleft == vright)
                        vright = vleft/rate;
                    task_segList[i].lspeed = vleft + ADDV;
                    //vleft = task_segList[i].lspeed;
                    task_segList[i].rspeed = task_segList[i].lspeed/rate;
                    //vright = task_segList[i].rspeed;
                } else {
                    if (vleft == vright)
                        vright = vleft/rate;
                    task_segList[i].lspeed = vleft - ADDV;
                    //vleft = task_segList[i].lspeed;
                    task_segList[i].rspeed = task_segList[i].lspeed/rate;
                    //vright = task_segList[i].rspeed;
                }
                delta = abs(t.trace->direction[i] - t.trace->direction[i+1]);
                task_segList[i].dur_time = compute_t(delta,(r + DD/2),
                                                     task_segList[i].lspeed);
                compute_a(&task_segList[i].lspeed,&task_segList[i].rspeed,
                          task_segList[i].dur_time,t.trace->inverse[i],rate);
                if (task_segList[i].lspeed > MAXV) {
                    task_segList[i].lspeed = MAXV;
                    task_segList[i].rspeed = MAXV/rate;
                }
                if (task_segList[i].lspeed < -MAXV)  {
                    task_segList[i].lspeed = -MAXV;
                    task_segList[i].rspeed = -MAXV/rate;
                }
                task_segList[i].dur_time = compute_t(delta,(r + DD/2),
                                                     task_segList[i].lspeed);
            }

            else { //左旋
                if (t.trace->inverse[i] == 0) {
                    if (vleft == vright)
                        vleft = vright*rate;
                    task_segList[i].rspeed = vright + ADDV;
                    //vright = task_segList[i].rspeed;
                    task_segList[i].lspeed = task_segList[i].rspeed*rate;
                    //vleft = task_segList[i].lspeed;
                } else  {
                    if (vleft == vright)
                        vleft = vright*rate;
                    task_segList[i].rspeed = vright - ADDV;
                    //vright = task_segList[i].rspeed;
                    task_segList[i].lspeed = task_segList[i].rspeed*rate;
                    //vleft = task_segList[i].lspeed;
                }
                delta = abs(t.trace->direction[i] - t.trace->direction[i+1]);
                task_segList[i].dur_time = compute_t(delta,(r + DD/2),
                                                     task_segList[i].rspeed);
                compute_a(&task_segList[i].lspeed,&task_segList[i].rspeed,
                          task_segList[i].dur_time,t.trace->inverse[i],rate);
                if (task_segList[i].rspeed > MAXV)  {
                    task_segList[i].rspeed = MAXV;
                    task_segList[i].lspeed = MAXV*rate;
                }
                if (task_segList[i].rspeed < -MAXV) {
                    task_segList[i].rspeed = -MAXV;
                    task_segList[i].lspeed = -MAXV*rate;
                }
                task_segList[i].dur_time = compute_t(delta,(r + DD/2),
                                                     task_segList[i].rspeed);
            }
        }
        task_segList[i].rotangle = 0;
        //mylogfd(1,"<%d,%d>",(int)task_segList[i].lspeed,(int)task_segList[i].rspeed);
    }
    for (; i < task_segNum-1; i++) {
        task_segList[i].type = TASK_RUN;
        task_segList[i].start.x = t.trace->ps[i].x;
        task_segList[i].start.y = t.trace->ps[i].y;
        task_segList[i].end.x = t.trace->ps[i+1].x;
        task_segList[i].end.y = t.trace->ps[i+1].y;
        //直线
        if (t.trace->direction[i] == t.trace->direction[i+1]) {
            if (t.trace->inverse[i] == 1) { //反向
                if (vleft != vright) {
                    if (fabs(vleft) > fabs(vright)) {
                        if (vleft < 0)
                            vright = vleft;
                        else {
                            vleft = 0;
                            vright = 0;
                        }
                    } else {
                        if (vright < 0)
                            vleft = vright;
                        else {
                            vleft = 0;
                            vright = 0;
                        }
                    }
                }
                task_segList[i].lspeed = vleft + ADDV;
                //vleft = task_segList[i].lspeed;
                task_segList[i].rspeed = task_segList[i].lspeed;
                //vright = task_segList[i].rspeed;
                if (task_segList[i].lspeed > -MIN_SPEED)
                    task_segList[i].lspeed = -MIN_SPEED;
                if (task_segList[i].rspeed > -MIN_SPEED)
                    task_segList[i].rspeed = -MIN_SPEED;
            }

            else { //正向
                if (vleft != vright) {
                    if (fabs(vleft) > fabs(vright)) {
                        if (vleft > 0)
                            vright = vleft;
                        else {
                            vleft = 0;
                            vright = 0;
                        }
                    } else {
                        if (vright > 0)
                            vleft = vright;
                        else {
                            vleft = 0;
                            vright = 0;
                        }
                    }
                }
                task_segList[i].lspeed = vleft - ADDV;
                //vleft = task_segList[i].lspeed;
                task_segList[i].rspeed = task_segList[i].lspeed;
                //vright = task_segList[i].rspeed;
                if (task_segList[i].lspeed < MIN_SPEED)
                    task_segList[i].lspeed = MIN_SPEED;
                if (task_segList[i].rspeed < MIN_SPEED)
                    task_segList[i].rspeed = MIN_SPEED;
            }
            if (task_segList[i].lspeed > MAXV)
                task_segList[i].lspeed = MAXV;
            if (task_segList[i].lspeed < -MAXV)
                task_segList[i].lspeed = -MAXV;
            if (task_segList[i].rspeed > MAXV)
                task_segList[i].rspeed = MAXV;
            if (task_segList[i].rspeed < -MAXV)
                task_segList[i].rspeed = -MAXV;

            double d = sqrt((t.trace->ps[i].x - t.trace->ps[i+1].x)
                            *(t.trace->ps[i].x - t.trace->ps[i+1].x)
                            +(t.trace->ps[i].y - t.trace->ps[i+1].y)
                            *(t.trace->ps[i].y - t.trace->ps[i+1].y));
            task_segList[i].dur_time = fabs(d / task_segList[i].lspeed);
            compute_a(&task_segList[i].lspeed,&task_segList[i].rspeed,
                      task_segList[i].dur_time,1-t.trace->inverse[i],1);
            task_segList[i].dur_time = fabs(d / task_segList[i].lspeed);
        }

        else { //圆弧
            //计算圆心
            compute_c(circle,t.trace->ps[i],t.trace->ps[i+1],
                      t.trace->direction[i],t.trace->direction[i+1]);
            //计算半径
            r = t.trace->r[i];
            //速度比
            rate = (r+DD/2)/(r-DD/2);
            if (t.trace->direction[i] < t.trace->direction[i+1])
                rate = 1/rate;
            //右旋
            if (rate > 1) {
                if (t.trace->inverse[i] == 0) {
                    if (vleft == vright)
                        vright = vleft/rate;
                    task_segList[i].lspeed = vleft - ADDV;
                    //vleft = task_segList[i].lspeed;
                    task_segList[i].rspeed = task_segList[i].lspeed/rate;
                    //vright = task_segList[i].rspeed;
                } else {
                    if (vleft == vright)
                        vright = vleft/rate;
                    task_segList[i].lspeed = vleft + ADDV;
                    //vleft = task_segList[i].lspeed;
                    task_segList[i].rspeed = task_segList[i].lspeed/rate;
                    //vright = task_segList[i].rspeed;
                }
                delta = abs(t.trace->direction[i] - t.trace->direction[i+1]);
                task_segList[i].dur_time = compute_t(delta,(r + DD/2),
                                                     task_segList[i].lspeed);
                compute_a(&task_segList[i].lspeed,&task_segList[i].rspeed,
                          task_segList[i].dur_time,1-t.trace->inverse[i],rate);
                if (task_segList[i].lspeed < MIN_SPEED) {
                    task_segList[i].lspeed = MIN_SPEED;
                    task_segList[i].rspeed = MIN_SPEED/rate;
                }
                if (task_segList[i].rspeed < MIN_SPEED/rate) {
                    task_segList[i].lspeed = MIN_SPEED;
                    task_segList[i].rspeed = MIN_SPEED/rate;
                }
                if (task_segList[i].lspeed > MAXV) {
                    task_segList[i].lspeed = MAXV;
                    task_segList[i].rspeed = MAXV/rate;
                }
                if (task_segList[i].lspeed < -MAXV)  {
                    task_segList[i].lspeed = -MAXV;
                    task_segList[i].rspeed = -MAXV/rate;
                }
                task_segList[i].dur_time = compute_t(delta,(r + DD/2),
                                                     task_segList[i].lspeed);
            }

            else { //左旋
                if (t.trace->inverse[i] == 0) {
                    if (vleft == vright)
                        vleft = vright*rate;
                    task_segList[i].rspeed = vright - ADDV;
                    //vright = task_segList[i].rspeed;
                    task_segList[i].lspeed = task_segList[i].rspeed*rate;
                    //vleft = task_segList[i].lspeed;
                } else  {
                    if (vleft == vright)
                        vleft = vright*rate;
                    task_segList[i].rspeed = vright - ADDV;
                    //vright = task_segList[i].rspeed;
                    task_segList[i].lspeed = task_segList[i].rspeed*rate;
                    //vleft = task_segList[i].lspeed;
                }
                delta = abs(t.trace->direction[i] - t.trace->direction[i+1]);
                task_segList[i].dur_time = compute_t(delta,(r + DD/2),
                                                     task_segList[i].rspeed);
                compute_a(&task_segList[i].lspeed,&task_segList[i].rspeed,
                          task_segList[i].dur_time,1-t.trace->inverse[i],rate);
                if (task_segList[i].lspeed < MIN_SPEED*rate) {
                    task_segList[i].lspeed = MIN_SPEED*rate;
                    task_segList[i].rspeed = MIN_SPEED;
                }
                if (task_segList[i].rspeed < MIN_SPEED) {
                    task_segList[i].lspeed = MIN_SPEED*rate;
                    task_segList[i].rspeed = MIN_SPEED;
                }
                if (task_segList[i].rspeed > MAXV)  {
                    task_segList[i].rspeed = MAXV;
                    task_segList[i].lspeed = MAXV*rate;
                }
                if (task_segList[i].rspeed < -MAXV) {
                    task_segList[i].rspeed = -MAXV;
                    task_segList[i].lspeed = -MAXV*rate;
                }
                task_segList[i].dur_time = compute_t(delta,(r + DD/2),
                                                     task_segList[i].rspeed);
            }
        }
        task_segList[i].rotangle = 0;
        /* mylogfd(1,"(%d,%d)<%d,%d>{%lf}",circle->x,circle->y,(int)task_segList[i].lspeed,(int)task_segList[i].rspeed,rate); */
    }
    task_segList[i].type = TASK_RUN;
    task_segList[i].start.x = t.trace->ps[i].x;
    task_segList[i].start.y = t.trace->ps[i].y;
    task_segList[i].end.x = t.trace->ps[i+1].x;
    task_segList[i].end.y = t.trace->ps[i+1].y;
    task_segList[i].lspeed = 0;
    task_segList[i].rspeed = 0;
    task_segList[i].dur_time = 0;
    task_segList[i].rotangle = 0;
}
Exemplo n.º 4
0
static void dotshootTask_init(task t)
{
    mylogfd (MOTOFD, "dotshootTask_init\n");
    int i = 0;
    double rate = 1;
    int delta = 0;
    double r = 0;
    maxa = t.maxa;
    for (i = 0; i < task_segNum - 2; i++) {
        task_segList[i].type = TASK_RUN;
        task_segList[i].start.x = t.trace->ps[i].x;
        task_segList[i].start.y = t.trace->ps[i].y;
        task_segList[i].end.x = t.trace->ps[i+1].x;
        task_segList[i].end.y = t.trace->ps[i+1].y;
        if ((task_segList[i].start.x == task_segList[i].end.x)
                &&(task_segList[i].start.y == task_segList[i].end.y)) {
            task_segList[i].type = TASK_ROTA;
            int rot = t.trace->direction[i+1]-t.trace->direction[i];
            if (rot > 0) {
                task_segList[i].lspeed = 2*-ROTV;
                task_segList[i].rspeed = 2*ROTV;
            } else {
                task_segList[i].lspeed = 2*ROTV;
                task_segList[i].rspeed = 2*-ROTV;
            }
            task_segList[i].rotangle = rot;
            task_segList[i].dur_time = compute_t(task_segList[i].rotangle,
                                                 DD/2,task_segList[i].lspeed);
            continue;
        }
        if (t.trace->direction[i] == t.trace->direction[i+1]) {
            if (t.trace->inverse[i] == 1) { //反向
                if (vleft != vright) {
                    if (fabs(vleft) > fabs(vright)) {
                        if (vleft < 0)
                            vright = vleft;
                        else {
                            vleft = 0;
                            vright = 0;
                        }
                    } else {
                        if (vright < 0)
                            vleft = vright;
                        else {
                            vleft = 0;
                            vright = 0;
                        }
                    }
                }
                task_segList[i].lspeed = vleft - 2*ADDV;
                //vleft = task_segList[i].lspeed;
                task_segList[i].rspeed = task_segList[i].lspeed;
                //vright = task_segList[i].rspeed;
            } else { //正向
                if (vleft != vright) {
                    if (fabs(vleft) > fabs(vright)) {
                        if (vleft > 0)
                            vright = vleft;
                        else {
                            vleft = 0;
                            vright = 0;
                        }
                    } else {
                        if (vright > 0)
                            vleft = vright;
                        else {
                            vleft = 0;
                            vright = 0;
                        }
                    }
                }
                task_segList[i].lspeed = vleft + ADDV;
                //vleft = task_segList[i].lspeed;
                task_segList[i].rspeed = task_segList[i].lspeed;
                //vright = task_segList[i].rspeed;
            }
            double d = sqrt((t.trace->ps[i].x - t.trace->ps[i+1].x)
                            *(t.trace->ps[i].x - t.trace->ps[i+1].x)
                            +(t.trace->ps[i].y - t.trace->ps[i+1].y)
                            *(t.trace->ps[i].y - t.trace->ps[i+1].y));
            task_segList[i].dur_time = fabs(d / task_segList[i].lspeed);
            compute_a(&task_segList[i].lspeed,&task_segList[i].rspeed,
                      task_segList[i].dur_time,t.trace->inverse[i],1);
            if (task_segList[i].lspeed > MAXV)
                task_segList[i].lspeed = MAXV;
            if (task_segList[i].lspeed < -MAXV)
                task_segList[i].lspeed = -MAXV;
            if (task_segList[i].rspeed > MAXV)
                task_segList[i].rspeed = MAXV;
            if (task_segList[i].rspeed < -MAXV)
                task_segList[i].rspeed = -MAXV;
            task_segList[i].dur_time = fabs(d / task_segList[i].lspeed);
        } else { //圆弧
            //计算圆心

            Point *circle;
            circle = (Point*)malloc(sizeof(struct _Point));
            compute_c(circle,t.trace->ps[i],t.trace->ps[i+1],
                      t.trace->direction[i],t.trace->direction[i+1]);
            //计算半径
            r = t.trace->r[i];
            //速度比
            rate = (r+DD/2)/(r-DD/2);
            mylogfd(MOTOFD,"get in arc r=%f rate=%f\n",r,rate);
            if (t.trace->direction[i] < t.trace->direction[i+1])
                rate = 1/rate;
            //右旋
            if (rate > 1) {
                if (t.trace->inverse[i] == 0) {
                    if (vleft < 0)
                        vleft = -vleft;
                    if (vleft == vright)
                        vright = vleft/rate;
                    task_segList[i].lspeed = vleft + ADDV;
                    //vleft = task_segList[i].lspeed;
                    task_segList[i].rspeed = task_segList[i].lspeed/rate;
                    //vright = task_segList[i].rspeed;
                } else {
                    if (vleft > 0)
                        vleft = -vleft;
                    if (vleft == vright)
                        vright = vleft/rate;
                    task_segList[i].lspeed = vleft - ADDV;
                    //vleft = task_segList[i].lspeed;
                    task_segList[i].rspeed = task_segList[i].lspeed/rate;
                    //vright = task_segList[i].rspeed;
                }
                delta = abs(t.trace->direction[i] - t.trace->direction[i+1]);
                task_segList[i].dur_time = compute_t(delta,(r + DD/2),
                                                     task_segList[i].lspeed) - 0.07;
                compute_a(&task_segList[i].lspeed,&task_segList[i].rspeed,
                          task_segList[i].dur_time,t.trace->inverse[i],rate);
                if (task_segList[i].lspeed > MAXV) {
                    task_segList[i].lspeed = MAXV;
                    task_segList[i].rspeed = MAXV/rate;
                }
                if (task_segList[i].lspeed < -MAXV)  {
                    task_segList[i].lspeed = -MAXV;
                    task_segList[i].rspeed = -MAXV/rate;
                }
                task_segList[i].dur_time = compute_t(delta,(r + DD/2),
                                                     task_segList[i].lspeed);
                task_segList[i].dur_time = task_segList[i].dur_time * 13.0f / 12 - 0.07;
            }

            else { //左旋
                if (t.trace->inverse[i] == 0) {
                    if (vleft < 0)
                        vleft = -vleft;
                    if (vleft == vright)
                        vleft = vright*rate;
                    task_segList[i].rspeed = vright + ADDV;
                    //vright = task_segList[i].rspeed;
                    task_segList[i].lspeed = task_segList[i].rspeed*rate;
                    //vleft = task_segList[i].lspeed;
                } else  {
                    if (vleft > 0)
                        vleft = -vleft;
                    if (vleft == vright)
                        vleft = vright*rate;
                    task_segList[i].rspeed = vright - ADDV;
                    //vright = task_segList[i].rspeed;
                    task_segList[i].lspeed = task_segList[i].rspeed*rate;
                    //vleft = task_segList[i].lspeed;
                }
                delta = abs(t.trace->direction[i] - t.trace->direction[i+1]);
                task_segList[i].dur_time = compute_t(delta,(r + DD/2),
                                                     task_segList[i].rspeed);
                compute_a(&task_segList[i].lspeed,&task_segList[i].rspeed,
                          task_segList[i].dur_time,t.trace->inverse[i],rate);
                if (task_segList[i].rspeed > MAXV)  {
                    task_segList[i].rspeed = MAXV;
                    task_segList[i].lspeed = MAXV*rate;
                }
                if (task_segList[i].rspeed < -MAXV) {
                    task_segList[i].rspeed = -MAXV;
                    task_segList[i].lspeed = -MAXV*rate;
                }
                task_segList[i].dur_time = compute_t(delta,(r + DD/2),
                                                     task_segList[i].rspeed);
                task_segList[i].dur_time = task_segList[i].dur_time * 13.0f / 12 - 0.07;
            }
        }
        task_segList[i].rotangle = 0;
        //mylogfd(-1,"<%d,%d>",(int)task_segList[i].lspeed,(int)task_segList[i].rspeed);
    }
    task_segList[i].type = TASK_RUN;
    task_segList[i].start.x = 0;
    task_segList[i].start.y = 0;
    task_segList[i].end.x = 0;
    task_segList[i].end.y = 0;
    task_segList[i].lspeed = task_segList[i-1].lspeed;
    //vleft = 0;
    task_segList[i].rspeed = task_segList[i-1].rspeed;
    //vright = 0;
    task_segList[i].rotangle = 0;
    task_segList[i].dur_time = MOTO_SHOOT_DIS/task_segList[i].lspeed;
    i++;
    task_segList[i].type = TASK_STOP;
    task_segList[i].start.x = 0;
    task_segList[i].start.y = 0;
    task_segList[i].end.x = 0;
    task_segList[i].end.y = 0;
    task_segList[i].lspeed = task_segList[i-1].lspeed;
    vleft = 0;
    task_segList[i].rspeed = task_segList[i-1].rspeed;
    vright = 0;
    task_segList[i].rotangle = 0;
    task_segList[i].dur_time = 0;
    mylogfd (MOTOFD, "dotshootTask_init over\n");
}
Exemplo n.º 5
0
/* y = ax^b + c 
 * 	return 0 if success, -1 otherwise
 * 	if success, a, b and c are modified
 * */
int _starpu_regression_non_linear_power(struct starpu_history_list_t *ptr, double *a, double *b, double *c)
{
	unsigned n = find_list_size(ptr);

	unsigned *x = malloc(n*sizeof(unsigned));
	STARPU_ASSERT(x);

	double *y = malloc(n*sizeof(double));
	STARPU_ASSERT(y);

	dump_list(x, y, ptr);

	double cmin = 0.0;
	double cmax = find_list_min(y, n);
	
	unsigned iter;

	double err = 100000.0;

	for (iter = 0; iter < MAXREGITER; iter++)
	{
		double c1, c2;
		double r1, r2;
		
		double radius = 0.01;

		c1 = cmin + (0.5-radius)*(cmax - cmin);
		c2 = cmin + (0.5+radius)*(cmax - cmin);

		r1 = test_r(c1, n, x, y);
		r2 = test_r(c2, n, x, y);

		double err1, err2;
		err1 = fabs(1.0 - r1);
		err2 = fabs(1.0 - r2);

		if (err1 < err2)
		{
			cmax = (cmin + cmax)/2;
		}
		else {
			/* 2 is better */
			cmin = (cmin + cmax)/2;
		}

		if (fabs(err - STARPU_MIN(err1, err2)) < EPS)
		{
			err = STARPU_MIN(err1, err2);
			break;
		}

		err = STARPU_MIN(err1, err2);
	}

	*c = (cmin + cmax)/2;

	*b = compute_b(*c, n, x, y); 
	*a = exp(compute_a(*c, *b, n, x, y));

	free(x);
	free(y);

	return 0;
}