C++ Vx函数代码示例

/* Create a bounding box for a set of points */
BoundingBox CreateBoundingBox(const std::vector<v2_t> &points) 
{
    int num_points = (int) points.size();
    BoundingBox bb;

    bb.m_xmin = DBL_MAX;
    bb.m_xmax = -DBL_MAX;
    bb.m_ymin = DBL_MAX;
    bb.m_ymax = -DBL_MAX;

    if (num_points == 0) {
	printf("[CreateBoundingBox] No points given!\n");
	// bb.m_xmin = bb.m_xmax = bb.m_ymin = bb.m_ymin = 0.0;
	return bb;
    }

    for (int i = 0; i < num_points; i++) {
	bb.m_xmin = MIN(bb.m_xmin, Vx(points[i]));
	bb.m_xmax = MAX(bb.m_xmax, Vx(points[i]));
	bb.m_ymin = MIN(bb.m_ymin, Vy(points[i]));
	bb.m_ymax = MAX(bb.m_ymax, Vy(points[i]));
    }

    return bb;
}

void
ghid_invalidate_lr (int left, int right, int top, int bottom)
{
  int dleft, dright, dtop, dbottom;
  int minx, maxx, miny, maxy;
  GdkRectangle rect;

  dleft = Vx (left);
  dright = Vx (right);
  dtop = Vy (top);
  dbottom = Vy (bottom);

  minx = MIN (dleft, dright);
  maxx = MAX (dleft, dright);
  miny = MIN (dtop, dbottom);
  maxy = MAX (dtop, dbottom);

  rect.x = minx;
  rect.y = miny;
  rect.width = maxx - minx;
  rect.height = maxy - miny;

  redraw_region (&rect);
  ghid_screen_update ();
}

void align_affine_3D(int num_pts, v3_t *r_pts, v3_t *l_pts, double *Tout) {
    if (num_pts < 4) {
	printf("[align_affine_3D] System is underconstrained!\n");
	return;
    } else {
	int m = num_pts * 3; /* Rows of A */
	int n = 12;           /* Columns of A */
	int nrhs = 1;        /* Columns of X */

	double *A = malloc(sizeof(double) * m * n);    /* Left-hand matrix */
	double *B = malloc(sizeof(double) * m * nrhs); /* Right-hand matrix */
	// double *X = malloc(sizeof(double) * n * nrhs);

	int i, base;
	    
	for (i = 0; i < num_pts; i++) {
	    base = 3 * i * n;

	    /* Row 1 */
	    A[base + 0] = (double) Vx(l_pts[i]);
	    A[base + 1] = (double) Vy(l_pts[i]);
	    A[base + 2] = (double) Vz(l_pts[i]);
	    A[base + 3] = 1.0;
	    A[base + 4] = A[base + 5] = A[base + 6] = A[base + 7] = 0.0;
	    A[base + 8] = A[base + 9] = A[base + 10] = A[base + 11] = 0.0;

	    base = (3 * i + 1) * n;

	    /* Row 2 */
	    A[base + 0] = A[base + 1] = A[base + 2] = A[base + 3] = 0.0;
	    A[base + 4] = (double) Vx(l_pts[i]);
	    A[base + 5] = (double) Vy(l_pts[i]);
	    A[base + 6] = (double) Vz(l_pts[i]);
	    A[base + 7] = 1.0;
	    A[base + 8] = A[base + 9] = A[base + 10] = A[base + 11] = 0.0;

	    base = (3 * i + 2) * n;

	    /* Row 2 */
	    A[base + 0] = A[base + 1] = A[base + 2] = A[base + 3] = 0.0;
	    A[base + 4] = A[base + 5] = A[base + 6] = A[base + 7] = 0.0;
	    A[base + 8] = (double) Vx(l_pts[i]);
	    A[base + 9] = (double) Vy(l_pts[i]);
	    A[base + 10] = (double) Vz(l_pts[i]);
	    A[base + 11] = 1.0;

	    B[3 * i + 0] = (double) Vx(r_pts[i]);
	    B[3 * i + 1] = (double) Vy(r_pts[i]);
	    B[3 * i + 2] = (double) Vz(r_pts[i]);
	}

	/* Run the driver to dgelsy */
	dgelsy_driver(A, B, Tout, m, n, nrhs);
	Tout[12] = 0.0; Tout[13] = 0.0; Tout[14] = 0.0; Tout[15] = 1.0;

	free(A);
	free(B);
	// free(X);
    }
}

    // in global 3D coordinate system
    bp::tuple computeShearAndBending() {

        PolyVec Vx, Vy, Fz, Mx, My, Tz;
        beam->shearAndBending(Vx, Vy, Fz, Mx, My, Tz);


        bp::list Vx0, Vy0, Fz0, Mx0, My0, Tz0;

        int n = beam->getNumNodes() - 1;

        for(int i = 0; i < n; i++)
        {
            Vx0.append(Vx(i).eval(0.0));
            Vy0.append(Vy(i).eval(0.0));
            Fz0.append(Fz(i).eval(0.0));
            Mx0.append(-My(i).eval(0.0));  // translate back to global coordinates
            My0.append(Mx(i).eval(0.0));  // translate back to global coordinates
            Tz0.append(Tz(i).eval(0.0));
        }

        Vx0.append(Vx(n-1).eval(1.0));
        Vy0.append(Vy(n-1).eval(1.0));
        Fz0.append(Fz(n-1).eval(1.0));
        Mx0.append(-My(n-1).eval(1.0));  // translate back to global coordinates
        My0.append(Mx(n-1).eval(1.0));  // translate back to global coordinates
        Tz0.append(Tz(n-1).eval(1.0));


        return bp::make_tuple(bpn::array(Vx0), bpn::array(Vy0), bpn::array(Fz0), bpn::array(Mx0), bpn::array(My0), bpn::array(Tz0));

    }

void img_dmap_write(FILE *f, img_dmap_t *dmap) {
    int count;
    short int has_uppers;

    /* Write the identifier */
    write_word((u_int32_t *)"DMAP", f);

    /* Write the width and height */
    write_short(&dmap->w, f);
    write_short(&dmap->h, f);

    if (dmap->uppers == NULL)
	has_uppers = 0;
    else
	has_uppers = 1;
    
    write_short(&has_uppers, f);

    /* Write the distances */
    for (count = 0; count < dmap->w * dmap->h; count++)
	write_double(&(dmap->dists[count]), f);

    /* Write each vector */
    for (count = 0; count < dmap->w * dmap->h; count++) {
	write_double(&(Vx(dmap->nns[count])), f);
	write_double(&(Vy(dmap->nns[count])), f);
    }

    if (dmap->uppers != NULL) {
	for (count = 0; count < dmap->w * dmap->h; count++) {
	    write_short(&Vx(dmap->uppers[count]), f);
	    write_short(&Vy(dmap->uppers[count]), f);
	}
    }
}

static void
homography_resids(int *m, int *n, double *x, double *fvec, int *iflag)
{
  int i;

  double resids = 0.0;

  double H[9];
  memcpy(H, x, 8 * sizeof(double));
  H[8] = 1.0;

  if (*iflag == 0 && global_num_pts > 4)
    {
      printf("[Round %d]\n", global_round);
      printf(
          "  H=(%0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.1f)\n",
          H[0], H[1], H[2], H[3], H[4], H[5], H[6], H[7], H[8]);
      global_round++;
    }

  for (i = 0; i < global_num_pts; i++)
    {
      double p[3], q[3];

      p[0] = Vx(global_l_pts[i]);
      p[1] = Vy(global_l_pts[i]);
      p[2] = Vz(global_l_pts[i]);

      if (*iflag == 0 && global_num_pts > 4)
        printf("    p=(%0.3f, %0.3f, %0.3f)\n", p[0], p[1], p[2]);

      matrix_product(3, 3, 3, 1, H, p, q);

      if (*iflag == 0 && global_num_pts > 4)
        printf("    q=(%0.3f, %0.3f, %0.3f)\n", q[0], q[1], q[2]);

      q[0] /= q[2];
      q[1] /= q[2];

      fvec[2 * i + 0] = q[0] - Vx(global_r_pts[i]);
      fvec[2 * i + 1] = q[1] - Vy(global_r_pts[i]);

      if (*iflag == 0 && global_num_pts > 4)
        printf("    (%0.3f, %0.3f) ==> (%0.3f, %0.3f)\n", q[0], q[1],
            Vx(global_r_pts[i]), Vy(global_r_pts[i]));
    }

  for (i = 0; i < 2 * global_num_pts; i++)
    {
      resids += fvec[i] * fvec[i];
    }

  if (*iflag == 0 && global_num_pts > 4)
    printf("resids = %0.3f\n", resids);
}

/* Compute the centroid of an array of 2D vectors */
v2_t v2_compute_centroid(v2_t *pts, int num_pts) {
    int i;
    v2_t centroid = v2_new(0.0, 0.0);

    for (i = 0; i < num_pts; i++) {
	Vx(centroid) += Vx(pts[i]);
	Vy(centroid) += Vy(pts[i]);
    }

    return v2_scale(1.0 / ((double) num_pts), centroid);
}

double v2_angle(v2_t u, v2_t v) {
    double angle = atan2(Vy(v),Vx(v)) - atan2(Vy(u),Vx(u));

    /* Get in range [-pi,pi] */
    if (angle > M_PI)
        angle -= 2.0 * M_PI;
    if (angle < -M_PI)
        angle += 2.0 * M_PI;

    return angle;
}

Type objective_function<Type>::operator() ()
{
	DATA_VECTOR(times);
	DATA_VECTOR(obs);
	
	PARAMETER(log_R0);
	PARAMETER(m);
	PARAMETER(log_theta);
	PARAMETER(log_sigma);
	Type theta=exp(log_theta);
	Type sigma=exp(log_sigma);
	Type R0=exp(log_R0);
	
	int n1=times.size();
	int n2=2;//mean and variance
	vector<Type> Dt(n1-1);
	vector<Type> Ex(n1-1);
	vector<Type> Vx(n1-1);
	Type nll=0;
	m=0;
	Dt=diff(times);
	Ex=theta*(Type(1)-exp(-R0*Dt)) + obs.segment(0, n1-1)*exp(-R0*Dt);
	Vx=Type(0.5)*sigma*sigma*(Type(1)-exp(Type(-2)*R0*Dt))/R0;
	
	for(int i=0; i<n1-1; i++)
	{
		nll-= dnorm(obs[i+1], Ex[i], sqrt(Vx[i]), true);
	}
	return nll;
}

/* Scale the vector so that the 3rd coordinate is 1 */
v3_t v3_homogenize(const v3_t v) {
    if (Vz(v) == 0.0) {
	return v3_new(DBL_MAX, DBL_MAX, 1.0);
    } else {
	return v3_new(Vx(v) / Vz(v), Vy(v) / Vz(v), 1.0);
    }
}

v3_t FindRobustMean(const std::vector<v3_t> &points) 
{
    int num_points = (int) points.size();
    double best_sum = DBL_MAX;
    int best_idx = -1;

    for (int i = 0; i < num_points; i++) {
	double sum = 0.0;

	for (int j = 0; j < num_points; j++) {
	    v3_t diff = v3_sub(points[i], points[j]);
	    
	    sum += fabs(Vx(diff)) + fabs(Vy(diff)) + fabs(Vz(diff));
	}

	if (sum < best_sum) {
	    best_sum = sum;
	    best_idx = i;
	}
    }

    if (best_idx == -1)
	return v3_new(0.0, 0.0, 0.0);
    
    return points[best_idx];
}

/* Produces an image of the flow map */
img_t *img_dmap_render_flow(img_dmap_t *dmap) {
    
    int w = dmap->w, h = dmap->h;
    double *dists = dmap->dists;
    v2_t *nns = dmap->nns;

    img_t *img_out = img_new(w, h);
    int idx, x, y;

    double max_dist = 0.0;

    /* Fill in the pixels */
    for (y = 0; y < h; y++) {
	for (x = 0; x < w; x++) {
	    idx = y * dmap->w + x;
	    v2_t nn = nns[idx];

#define FACTOR 6.0
	    double dx = CLAMP(FACTOR * (Vx(nn) - x) + 127.0, 0.0, 255.0);
	    double dy = CLAMP(FACTOR * (Vy(nn) - y) + 127.0, 0.0, 255.0);
#undef FACTOR

	    if (dists[idx] == DBL_MAX) {
		img_out->pixels[idx].r = img_out->pixels[idx].g = 0;
		img_out->pixels[idx].b = 0;
	    } else {
		img_out->pixels[idx].r = (int) rint(dx);
		img_out->pixels[idx].g = (int) rint(dy);
		img_out->pixels[idx].b = 0.0;
	    }
	}
    }

    return img_out;    
}

void TwoFrameModel::Write(FILE *f) const
{
    fprintf(f, "%d\n", m_num_points);

    fprintf(f, "%0.9f\n", m_angle);

    fprintf(f, "%0.9f\n", m_error);

    for (int i = 0; i < m_num_points; i++) {
        int tr = -1, k1 = -1, k2 = -1;
            
        if (m_tracks != NULL)
            tr = m_tracks[i];
        
        if (m_keys1 != NULL)
            k1 = m_keys1[i];
        
        if (m_keys2 != NULL)
            k2 = m_keys2[i];
        
        fprintf(f, "%d %d %d %0.16e %0.16e %0.16e\n", tr, k1, k2,
                Vx(m_points[i]), Vy(m_points[i]), Vz(m_points[i]));
    }
    
    WriteCamera(f, m_camera0);
    WriteCamera(f, m_camera1);
    
    WriteVector(f, 9, m_C0);
    WriteVector(f, 9, m_C1);
}

img_dmap_t *img_dmap_read(FILE *f) {
    int count;
    u_int16_t w, h;
    char id[5];
    img_dmap_t *dmap;
    short int has_uppers;

    /* Read the identifier */
    read_word((u_int32_t *)id, f);
    id[4] = 0;

    if (strcmp(id, "DMAP") != 0) {
	printf("[img_read_distance_map] Invalid distance map file\n");
	return NULL;
    }

    /* Read the width and height */
    read_short(&w, f);
    read_short(&h, f);
    read_short(&has_uppers, f);

    /* Initialize the map */
    dmap = img_dmap_new(w, h);

    /* Read the distances */
    for (count = 0; count < w * h; count++) {
	read_double(&(dmap->dists[count]), f);
    }
    
    /* Read the nearest neighbors */
    for (count = 0; count < w * h; count++) {
	read_double(&Vx(dmap->nns[count]), f);
	read_double(&Vy(dmap->nns[count]), f);
    }

    if (has_uppers) {
	dmap->uppers = (iv2_t *) malloc(w * h * sizeof(iv2_t));

	/* Read the uppers */
	for (count = 0; count < w * h; count++) {
	    read_short(&(Vx(dmap->uppers[count])), f);
	    read_short(&(Vy(dmap->uppers[count])), f);
	}
    }

    return dmap;
}

/* Compute the pixel that would be at location (x, y) if the given
 * image were transformed with the inverse of the given transform */
fcolor_t pixel_transform(img_t *img, trans2D_t *Tinv, int x, int y) {
    double Tp[2];
    
    int w = img->w, h = img->h;

    transform_point(Tinv, x, y, Tp+0, Tp+1);

    /* Check if the result is in range */
    if (Tp[0] < Vx(img->origin) || Tp[1] < Vy(img->origin) || 
	Tp[0] > Vx(img->origin) + w - 1 || Tp[1] > Vy(img->origin) + h - 1) 
	{
	    fcolor_t black = { 0.0, 0.0, 0.0 };
	    return black;
	}
    
    return pixel_lerp(img, Tp[0] - Vx(img->origin), Tp[1] - Vy(img->origin));
}