예제 #1
0
파일: mthrowu.c 프로젝트: FredrIQ/nhfourk
/* monster attempts ranged weapon attack against a square */
void
thrwmq(struct monst *mtmp, int xdef, int ydef)
{
    struct obj *otmp, *mwep;
    schar skill;
    int multishot;
    const char *onm;

    /* Rearranged beginning so monsters can use polearms not in a line */
    if (mtmp->weapon_check == NEED_WEAPON || !MON_WEP(mtmp)) {
        mtmp->weapon_check = NEED_RANGED_WEAPON;
        /* mon_wield_item resets weapon_check as appropriate */
        if (mon_wield_item(mtmp) != 0)
            return;
    }

    /* Pick a weapon */
    otmp = select_rwep(mtmp);
    if (!otmp)
        return;

    if (is_pole(otmp)) {
        int dam, hitv;

        if (otmp != MON_WEP(mtmp))
            return;  /* polearm must be wielded */

        /* TODO: LOE function between two arbitrary points. */
        if (dist2(mtmp->mx, mtmp->my, xdef, ydef) > POLE_LIM ||
            (xdef == u.ux && ydef == u.uy && !couldsee(mtmp->mx, mtmp->my)))
            return;     /* Out of range, or intervening wall */

        if (mon_visible(mtmp)) {
            onm = singular(otmp, xname);
            pline("%s thrusts %s.", Monnam(mtmp),
                  obj_is_pname(otmp) ? the(onm) : an(onm));
        }

        if (xdef == u.ux && ydef == u.uy) {

            dam = dmgval(otmp, &youmonst);
            hitv = 3 - distmin(u.ux, u.uy, mtmp->mx, mtmp->my);
            if (hitv < -4)
                hitv = -4;
            if (bigmonst(youmonst.data))
                hitv++;
            hitv += 8 + otmp->spe;
            if (objects[otmp->otyp].oc_class == WEAPON_CLASS ||
                objects[otmp->otyp].oc_class == VENOM_CLASS)
                hitv += objects[otmp->otyp].oc_hitbon;
            if (dam < 1)
                dam = 1;

            thitu(hitv, dam, otmp, NULL);
            action_interrupted();

        } else if (MON_AT(level, xdef, ydef))
            (void)ohitmon(m_at(level, xdef, ydef), otmp, 0, FALSE);
        else if (mon_visible(mtmp))
            pline("But it misses wildly.");

        return;
    }

    if (!qlined_up(mtmp, xdef, ydef, FALSE, FALSE) ||
        !ai_use_at_range(BOLT_LIM - distmin(mtmp->mx, mtmp->my, xdef, ydef)))
        return;

    skill = objects[otmp->otyp].oc_skill;
    mwep = MON_WEP(mtmp);       /* wielded weapon */

    /* Multishot calculations */
    multishot = 1;
    if ((ammo_and_launcher(otmp, mwep) || skill == P_DAGGER || skill == -P_DART
         || skill == -P_SHURIKEN) && !mtmp->mconf) {
        /* Assumes lords are skilled, princes are expert */
        if (is_prince(mtmp->data))
            multishot += 2;
        else if (is_lord(mtmp->data))
            multishot++;

        switch (monsndx(mtmp->data)) {
        case PM_RANGER:
            multishot++;
            break;
        case PM_ROGUE:
            if (skill == P_DAGGER)
                multishot++;
            break;
        case PM_NINJA:
        case PM_SAMURAI:
            if (otmp->otyp == YA && mwep && mwep->otyp == YUMI)
                multishot++;
            break;
        default:
            break;
        }
        /* racial bonus */
        if ((is_elf(mtmp->data) && otmp->otyp == ELVEN_ARROW && mwep &&
             mwep->otyp == ELVEN_BOW) || (is_orc(mtmp->data) &&
                                          otmp->otyp == ORCISH_ARROW && mwep &&
                                          mwep->otyp == ORCISH_BOW))
            multishot++;

        if ((long)multishot > otmp->quan)
            multishot = (int)otmp->quan;
        if (multishot < 1)
            multishot = 1;
        else
            multishot = rnd(multishot);
    }

    if (mon_visible(mtmp)) {
        if (multishot > 1) {
            /* "N arrows"; multishot > 1 implies otmp->quan > 1, so xname()'s
               result will already be pluralized */
            onm = msgprintf("%d %s", multishot, xname(otmp));
        } else {
            /* "an arrow" */
            onm = singular(otmp, xname);
            onm = obj_is_pname(otmp) ? the(onm) : an(onm);
        }
        m_shot.s = ammo_and_launcher(otmp, mwep) ? TRUE : FALSE;
        pline("%s %s %s!", Monnam(mtmp), m_shot.s ? "shoots" : "throws", onm);
        m_shot.o = otmp->otyp;
    } else {
        m_shot.o = STRANGE_OBJECT;      /* don't give multishot feedback */
    }

    m_shot.n = multishot;
    for (m_shot.i = 1; m_shot.i <= m_shot.n; m_shot.i++) {
        m_throw(mtmp, mtmp->mx, mtmp->my, sgn(tbx), sgn(tby),
                distmin(mtmp->mx, mtmp->my, xdef, ydef), otmp, TRUE);
        /* conceptually all N missiles are in flight at once, but
           if mtmp gets killed (shot kills adjacent gas spore and
           triggers explosion, perhaps), inventory will be dropped
           and otmp might go away via merging into another stack;
           if we then use it, we could cause undefined behavior */
        if (mtmp->mhp <= 0 && m_shot.i < m_shot.n) {
            /* cancel pending shots (ought to give a message here since
               we gave one above about throwing/shooting N missiles) */
            break;  /* endmultishot(FALSE); */
        }
    }
    m_shot.n = m_shot.i = 0;
    m_shot.o = STRANGE_OBJECT;
    m_shot.s = FALSE;

    action_interrupted();
}
예제 #2
0
// this method will also construct the triangles/polygons in the new mesh
// if we accept planar polygons, we just save them
// also, we could just create new vertices every time, and merge only in the end;
// could be too expensive, and the tolerance for merging could be an
// interesting topic
int Intx2MeshOnSphere::findNodes(EntityHandle red, int nsRed, EntityHandle blue, int nsBlue,
    double * iP, int nP)
{
  // first of all, check against red and blue vertices
  //
  if (dbg_1)
  {
    std::cout << "red, blue, nP, P " << mb->id_from_handle(red) << " "
        << mb->id_from_handle(blue) << " " << nP << "\n";
    for (int n = 0; n < nP; n++)
      std::cout << " \t" << iP[2 * n] << "\t" << iP[2 * n + 1] << "\n";

  }

  // get the edges for the red triangle; the extra points will be on those edges, saved as
  // lists (unordered)
  std::vector<EntityHandle> redEdges(nsRed);//
  int i = 0;
  for (i = 0; i < nsRed; i++)
  {
    EntityHandle v[2] = { redConn[i], redConn[(i + 1) % nsRed] };
    std::vector<EntityHandle> adj_entities;
    ErrorCode rval = mb->get_adjacencies(v, 2, 1, false, adj_entities,
        Interface::INTERSECT);
    if (rval != MB_SUCCESS || adj_entities.size() < 1)
      return 0; // get out , big error
    redEdges[i] = adj_entities[0]; // should be only one edge between 2 nodes
  }
  // these will be in the new mesh, mbOut
  // some of them will be handles to the initial vertices from blue or red meshes (lagr or euler)

  EntityHandle * foundIds = new EntityHandle[nP];
  for (i = 0; i < nP; i++)
  {
    double * pp = &iP[2 * i]; // iP+2*i
    // project the point back on the sphere
    CartVect pos;
    reverse_gnomonic_projection(pp[0], pp[1], R, plane, pos);
    int found = 0;
    // first, are they on vertices from red or blue?
    // priority is the red mesh (mb2?)
    int j = 0;
    EntityHandle outNode = (EntityHandle) 0;
    for (j = 0; j < nsRed && !found; j++)
    {
      //int node = redTri.v[j];
      double d2 = dist2(pp, &redCoords2D[2 * j]);
      if (d2 < epsilon_1)
      {

        foundIds[i] = redConn[j]; // no new node
        found = 1;
        if (dbg_1)
          std::cout << "  red node j:" << j << " id:"
              << mb->id_from_handle(redConn[j]) << " 2d coords:" << redCoords2D[2 * j] << "  "
              << redCoords2D[2 * j + 1] << " d2: " << d2 << " \n";
      }
    }

    for (j = 0; j < nsBlue && !found; j++)
    {
      //int node = blueTri.v[j];
      double d2 = dist2(pp, &blueCoords2D[2 * j]);
      if (d2 < epsilon_1)
      {
        // suspect is blueConn[j] corresponding in mbOut

        foundIds[i] = blueConn[j]; // no new node
        found = 1;
        if (dbg_1)
          std::cout << "  blue node " << j << " "
              << mb->id_from_handle(blueConn[j]) << " d2:" << d2 << " \n";
      }

    }
    if (!found)
    {
      // find the edge it belongs, first, on the red element
      //
      for (j = 0; j < nsRed; j++)
      {
        int j1 = (j + 1) % nsRed;
        double area = area2D(&redCoords2D[2 * j], &redCoords2D[2 * j1], pp);
        if (dbg_1)
          std::cout << "   edge " << j << ": "
              << mb->id_from_handle(redEdges[j]) << " " << redConn[j] << " "
              << redConn[j1] << "  area : " << area << "\n";
        if (fabs(area) < epsilon_1/2)
        {
          // found the edge; now find if there is a point in the list here
          //std::vector<EntityHandle> * expts = extraNodesMap[redEdges[j]];
          int indx = -1;
          indx = RedEdges.index(redEdges[j]);
          std::vector<EntityHandle> * expts = extraNodesVec[indx];
          // if the points pp is between extra points, then just give that id
          // if not, create a new point, (check the id)
          // get the coordinates of the extra points so far
          int nbExtraNodesSoFar = expts->size();
          CartVect * coords1 = new CartVect[nbExtraNodesSoFar];
          mb->get_coords(&(*expts)[0], nbExtraNodesSoFar, &(coords1[0][0]));
          //std::list<int>::iterator it;
          for (int k = 0; k < nbExtraNodesSoFar && !found; k++)
          {
            //int pnt = *it;
            double d2 = (pos - coords1[k]).length_squared();
            if (d2 < epsilon_1)
            {
              found = 1;
              foundIds[i] = (*expts)[k];
              if (dbg_1)
                std::cout << " found node:" << foundIds[i] << std::endl;
            }
          }
          if (!found)
          {
            // create a new point in 2d (at the intersection)
            //foundIds[i] = m_num2dPoints;
            //expts.push_back(m_num2dPoints);
            // need to create a new node in mbOut
            // this will be on the edge, and it will be added to the local list
            mb->create_vertex(pos.array(), outNode);
            (*expts).push_back(outNode);
            foundIds[i] = outNode;
            found = 1;
            if (dbg_1)
              std::cout << " new node: " << outNode << std::endl;
          }
          delete[] coords1;
        }
      }
    }
    if (!found)
    {
      std::cout << " red quad: ";
      for (int j1 = 0; j1 < nsRed; j1++)
      {
        std::cout << redCoords2D[2 * j1] << " " << redCoords2D[2 * j1 + 1] << "\n";
      }
      std::cout << " a point pp is not on a red quad " << *pp << " " << pp[1]
          << " red quad " << mb->id_from_handle(red) << " \n";
      delete[] foundIds;
      return 1;
    }
  }
  if (dbg_1)
  {
    std::cout << " candidate polygon: nP" << nP <<  " plane: " << plane << "\n";
    for (int i1 = 0; i1 < nP; i1++)
            std::cout << iP[2 * i1] << " " << iP[2 * i1 + 1] << " " << foundIds[i1] << "\n";
  }
  // first, find out if we have nodes collapsed; shrink them
  // we may have to reduce nP
  // it is possible that some nodes are collapsed after intersection only
  // nodes will always be in order (convex intersection)
  correct_polygon(foundIds, nP);
  // now we can build the triangles, from P array, with foundIds
  // we will put them in the out set
  if (nP >= 3)
  {
    EntityHandle polyNew;
    mb->create_element(MBPOLYGON, foundIds, nP, polyNew);
    mb->add_entities(outSet, &polyNew, 1);

    // tag it with the index ids from red and blue sets
    int id = rs1.index(blue); // index starts from 0
    mb->tag_set_data(blueParentTag, &polyNew, 1, &id);
    id = rs2.index(red);
    mb->tag_set_data(redParentTag, &polyNew, 1, &id);

    static int count=0;
    count++;
    mb->tag_set_data(countTag, &polyNew, 1, &count);

    if (dbg_1)
    {

      std::cout << "Count: " << count << "\n";
      std::cout << " polygon " << mb->id_from_handle(polyNew) << "  nodes: " << nP << " :";
      for (int i1 = 0; i1 < nP; i1++)
        std::cout << " " << mb->id_from_handle(foundIds[i1]);
      std::cout << " plane: " << plane << "\n";
      std::vector<CartVect> posi(nP);
      mb->get_coords(foundIds, nP, &(posi[0][0]));
      for (int i1 = 0; i1 < nP; i1++)
        std::cout << foundIds[i1]<< " " << posi[i1] << "\n";

      std::stringstream fff;
      fff << "file0" <<  count<< ".vtk";
          mb->write_mesh(fff.str().c_str(), &outSet, 1);
    }

  }
  delete[] foundIds;
  foundIds = NULL;
  return 0;
}
예제 #3
0
/*****************************************************************************
 *FUNCTION -- dist                                                           *
 *                                                                           *
 *INPUTS:  x, y, z    (position of first point in space)                     *
 *         x2, y2, z2 (position of second point in space)                    *
 *                                                                           *
 *RETURNS: length of the vector from (x,y,z) to (x2, y2, z2)                 *
 *                                                                           *
 *****************************************************************************/
double dist (double x, double y, double z, double x2, double y2, double z2)
{
      return sqrt(dist2(x, y, z, x2, y2, z2));
} /* end function dist */
예제 #4
0
// compute center of circle given two points and radius
PT ComputeCircleCenter(PT a, PT b, double r) {
	double det = r * r / dist2(a, b) - 0.25;
	if (det < 0) return PT(INF,INF); // does not exist
	return (a + b) * 0.5 + PT(a.y - b.y, b.x - a.x) * sqrt(det);
}
예제 #5
0
int SingleSLAM::fastPoseUpdate3D() {
	propagateFeatureStates();
//get the feature points corresponding to the map points
	std::vector<Track2DNode*> nodes;
	int num = getStaticMappedTrackNodes(nodes);
	if (num < 5) {
		repErr(
				"[camera id:%d]intra-camera pose update failed! less than five static map points (%d)",
				camId, num);
		return -1;
	}

//choose the feature points for pose estimation
	std::vector<FeaturePoint*> featPts;
	int iChoose = chooseStaticFeatPts(featPts);
//test
	logInfo("number of chosen features :%d\n", iChoose);

	std::vector<FeaturePoint*> mappedFeatPts;
	std::vector<FeaturePoint*> unmappedFeatPts;

	mappedFeatPts.reserve(nRowBlk * nColBlk * 2);
	unmappedFeatPts.reserve(nRowBlk * nColBlk * 2);

	for (size_t i = 0; i < featPts.size(); i++) {
		if (featPts[i]->mpt)
			mappedFeatPts.push_back(featPts[i]);
		else if (featPts[i]->preFrame)
			unmappedFeatPts.push_back(featPts[i]);
	}

	//get the 2D-3D corresponding points
	int n3D2Ds = mappedFeatPts.size();
	Mat_d ms(n3D2Ds, 2), Ms(n3D2Ds, 3), repErrs(n3D2Ds, 1);
	for (int i = 0; i < n3D2Ds; i++) {
		FeaturePoint* fp = mappedFeatPts[i];
		ms.data[2 * i] = fp->x;
		ms.data[2 * i + 1] = fp->y;

		Ms.data[3 * i] = fp->mpt->x;
		Ms.data[3 * i + 1] = fp->mpt->y;
		Ms.data[3 * i + 2] = fp->mpt->z;

		repErrs.data[i] = fp->reprojErr;
	}

	//get the 2D-2D corresponding points
	int n2D2Ds = unmappedFeatPts.size();
	Mat_d ums(n2D2Ds, 2), umspre(n2D2Ds, 2), Rpre(n2D2Ds, 9), tpre(n2D2Ds, 3);
	for (int i = 0; i < n2D2Ds; i++) {
		FeaturePoint* fp = unmappedFeatPts[i];

		ums.data[2 * i] = fp->x;
		ums.data[2 * i + 1] = fp->y;

		//travel back to the frame of the first appearance
		//while (fp->preFrame) {
		fp = fp->preFrame;
		//}
		assert(fp);

		umspre.data[2 * i] = fp->x;
		umspre.data[2 * i + 1] = fp->y;

		doubleArrCopy(Rpre.data, i, fp->cam->R, 9);
		doubleArrCopy(tpre.data, i, fp->cam->t, 3);
	}

//estimate the camera pose using both 2D-2D and 3D-2D correspondences
	double R[9], t[3];
	double* cR = m_camPos.current()->R;
	double* cT = m_camPos.current()->t;

//	//test
//	logInfo("==============start of camId:%d=================\n", camId);
//	logInfo("n3D2D:%d, n2D2D:%d\n", n3D2Ds, n2D2Ds);
//
//	write(K, "/home/tsou/data/K.txt");
//	write(cR, 3, 3, "/home/tsou/data/%d_R0.txt", camId);
//	write(cT, 3, 1, "/home/tsou/data/%d_T0.txt", camId);
//	write(repErrs, "/home/tsou/data/%d_errs.txt", camId);
//	write(Ms, "/home/tsou/data/%d_Ms.txt", camId);
//	write(ms, "/home/tsou/data/%d_ms.txt", camId);
//	write(Rpre, "/home/tsou/data/%d_Rpre.txt", camId);
//	write(tpre, "/home/tsou/data/%d_tpre.txt", camId);
//	write(umspre, "/home/tsou/data/%d_umspre.txt", camId);
//	write(ums, "/home/tsou/data/%d_ums.txt", camId);
//
//	//test
//	printMat(3, 3, cR);
//	printMat(3, 1, cT);

	IntraCamPoseOption opt;
	double R_tmp[9], t_tmp[3];
	intraCamEstimate(K.data, cR, cT, n3D2Ds, repErrs.data, Ms.data, ms.data, 6,
			R_tmp, t_tmp, &opt);

	if (getCameraDistance(R_tmp, t_tmp, Rpre.data, tpre.data) > 1000) {
		opt.verboseLM = 1;
		intraCamEstimateEpi(K.data, R_tmp, t_tmp, n3D2Ds, repErrs.data, Ms.data,
				ms.data, n2D2Ds, 0, Rpre.data, tpre.data, umspre.data, ums.data,
				6, R, t, &opt);
	} else {
		doubleArrCopy(R, 0, R_tmp, 9);
		doubleArrCopy(t, 0, t_tmp, 3);
	}

//	printMat(3, 3, cR);
//	printMat(3, 1, cT);
//	printMat(3, 3, R);
//	printMat(3, 1, cT);
//	logInfo("==============end of camId:%d=================\n", camId);
//	intraCamEstimate(K.data,cR,cT,n3D2Ds, repErrs.data,Ms.data,ms.data,6.0,R,t,&opt);
//	find outliers

	int numOut = 0;
	double rm[2], var[4], ivar[4];
	for (int i = 0; i < num; i++) {
		double* pM = nodes[i]->pt->mpt->M;
		double* pCov = nodes[i]->pt->mpt->cov;
		project(K, R, t, pM, rm);
		getProjectionCovMat(K, R, t, pM, pCov, var, Const::PIXEL_ERR_VAR);
		mat22Inv(var, ivar);
		double err = mahaDist2(rm, nodes[i]->pt->m, ivar);
		if (err < 1) { //inlier
			nodes[i]->pt->reprojErr = err;
			seqTriangulate(K, R, t, nodes[i]->pt->m, pM, pCov,
					Const::PIXEL_ERR_VAR);
			project(K, R, t, pM, rm);
			getProjectionCovMat(K, R, t, pM, pCov, var, Const::PIXEL_ERR_VAR);
			mat22Inv(var, ivar);
			err = mahaDist2(rm, nodes[i]->pt->m, ivar);
			if (err >= 1) {
				nodes[i]->pt->mpt->setFalse();
			}
		} else {
			//outliers
			numOut++;
			double repErr = dist2(rm, nodes[i]->pt->m);
			nodes[i]->pt->reprojErr = repErr;
			nodes[i]->pt->mpt->setUncertain();
		}
	}
	CamPoseItem* camPos = m_camPos.add(currentFrame(), camId, R, t);
	updateCamParamForFeatPts(K, camPos);

	return num;
}
예제 #6
0
// TEST 10 - SHOT MODIFICATION - SIMILARITY
///////////////////////////////////////////////////////////////////////////////
bool test10(vcg::Shotd shot1, vcg::Shotd shot2, vcg::Point3d p1, vcg::Point3d p2)
{
  vcg::Point2d p1proj = shot1.Project(p1);

  // store data
  vcg::Matrix44d Rorig = shot1.Extrinsics.Rot();
  vcg::Point3d Torig = shot1.Extrinsics.Tra();

  // pure translation
  vcg::Matrix44d T;
  T.SetIdentity();
  T.ElementAt(0,3) = 10.0;
  T.ElementAt(1,3) = 10.0;
  T.ElementAt(2,3) = 10.0;
  T.ElementAt(3,3) = 1.0;

  vcg::Point3d tr(T.ElementAt(0,3), T.ElementAt(1,3), T.ElementAt(2,3));

  // pure rotation
  vcg::Matrix44d R;
  R.SetZero();
  R.ElementAt(0,2) = 1.0;
  R.ElementAt(1,1) = 1.0;
  R.ElementAt(2,0) = -1.0;
  R.ElementAt(3,3) = 1.0;

  // scaling
  vcg::Matrix44d S;
  double scale = 10.0;
  S.SetIdentity();
  S *= scale;
  S.ElementAt(3,3) = 1.0;

  vcg::Point3d psim = R * S * p1 + tr;

  vcg::Matrix44d SRT = T * R * S;

  shot1.ApplySimilarity(SRT);
  vcg::Point2d psimproj = shot1.Project(psim);

  if (dist2(p1proj, psimproj) > precision)
    return false;

  // restore the original reference frame to test another transformation
  shot1.Extrinsics.SetTra(Torig);
  shot1.Extrinsics.SetRot(Rorig);

  vcg::Similarityd sm;
  double pihalf = 3.1415926535897932384626433832795 / 2.0;
  sm.SetRotate(pihalf, vcg::Point3d(0.0,1.0,0.0));
  sm.sca = scale;
  sm.tra = tr;

  shot1.ApplySimilarity(sm);
  psimproj = shot1.Project(psim);

  if (dist2(p1proj, psimproj) > precision)
    return false;

  return true;
}
예제 #7
0
// determine if point is on the boundary of a polygon
bool PointOnPolygon(const vector<PT> &p, PT q) {
	for (int i = 0; i < p.size(); i++)
		if (dist2(ProjectPointSegment(p[i], p[(i+1)%p.size()], q), q) < EPS)
			return true;
	return false;
}
예제 #8
0
// Approximation to Gaussian...  Used in filtering
static inline float wt(const point &p1, const point &p2, float invsigma2)
{
	float d2 = invsigma2 * dist2(p1, p2);
	return (d2 >= 9.0f) ? 0.0f : exp(-0.5f*d2);
	//return (d2 >= 25.0f) ? 0.0f : exp(-0.5f*d2);
}
예제 #9
0
 double dist() const { return sqrt((double)dist2()); }
예제 #10
0
void think(){
	
	//think
	for (int i=0; i<max_flares; i++) {
#ifdef COLLISION	
		for (int j=i+1; j<max_flares; j++) {
			//if(i==j) continue;
			float d = dist(i,j);
			if(d<flare_size){
				float dx = flares[j].x - flares[i].x;
				float dy = flares[j].y - flares[i].y;
				dx /= d / 0.8;
				dy /= d / 0.8;
				flares[i].vx -= dx;
				flares[i].vy -= dy;
				flares[j].vx += dx;
				flares[j].vy += dy;
			}
		}
#endif
		float tx = targets[flares[i].target].x;
		float ty = targets[flares[i].target].y;
		float d = dist2(tx, ty, flares[i].x, flares[i].y);
		float dx = tx - flares[i].x;
		float dy = ty - flares[i].y;
		dx /= d / 0.5;
		dy /= d / 0.5;
		flares[i].vx += dx;
		flares[i].vy += dy;
		
	}
	//move
	for (int i=0; i<max_flares; i++) {

            flares[i].x+=flares[i].vx;
            flares[i].y+=flares[i].vy;
            if (flares[i].x<0) {
		flares[i].x = 0;
		flares[i].vx += 5;
            }
            if (flares[i].x>width) {
		flares[i].x = width;
		flares[i].vx -= 5;
            }
            if (flares[i].y<0) {
		flares[i].y = 0;
		flares[i].vy += 5;
            }
            if (flares[i].y>height) {
		flares[i].y = height;
		flares[i].vy -= 5;
            }

		//damping
		flares[i].vx *= DAMPING;	
		flares[i].vy *= DAMPING;	
	
        }

	
	
}
예제 #11
0
char* TessPDFRenderer::GetPDFTextObjects(TessBaseAPI* api,
                                         double width, double height,
                                         int page_number) {
  double ppi = api->GetSourceYResolution();
  STRING pdf_str("");
  double old_x = 0.0, old_y = 0.0;
  int old_pointsize = 0;

  // TODO(jbreiden) Slightly cleaner from an abstraction standpoint
  // if this were to live inside a separate text object.
  pdf_str += "q ";
  pdf_str.add_str_double("", prec(width));
  pdf_str += " 0 0 ";
  pdf_str.add_str_double("", prec(height));
  pdf_str += " 0 0 cm /Im1 Do Q\n";

  ResultIterator *res_it = api->GetIterator();

  while (!res_it->Empty(RIL_BLOCK)) {
    if (res_it->IsAtBeginningOf(RIL_BLOCK)) {
      pdf_str += "BT\n3 Tr\n";  // Begin text object, use invisible ink
      old_pointsize = 0.0;      // Every block will declare its font
    }

    int line_x1, line_y1, line_x2, line_y2;
    if (res_it->IsAtBeginningOf(RIL_TEXTLINE)) {
      res_it->Baseline(RIL_TEXTLINE,
                       &line_x1, &line_y1, &line_x2, &line_y2);
      double rise = abs(line_y2 - line_y1) * 72 / ppi;
      double run = abs(line_x2 - line_x1) * 72 / ppi;
      // There are some really stupid PDF viewers in the wild, such as
      // 'Preview' which ships with the Mac. They might do a better
      // job with text selection and highlighting when given perfectly
      // straight text instead of very slightly tilted text. I chose
      // this threshold large enough to absorb noise, but small enough
      // that lines probably won't cross each other if the whole page
      // is tilted at almost exactly the clipping threshold.
      if (rise < 2.0 && 2.0 < run)
        line_y1 = line_y2 = (line_y1 + line_y2) / 2;
    }

    if (res_it->Empty(RIL_WORD)) {
      res_it->Next(RIL_WORD);
      continue;
    }

    int word_x1, word_y1, word_x2, word_y2;
    res_it->Baseline(RIL_WORD, &word_x1, &word_y1, &word_x2, &word_y2);

    // The critical one is writing_direction
    tesseract::Orientation orientation;
    tesseract::WritingDirection writing_direction;
    tesseract::TextlineOrder textline_order;
    float deskew_angle;
    res_it->Orientation(&orientation, &writing_direction,
                        &textline_order, &deskew_angle);

    // Unlike Tesseract, we always want the word baseline in reading order.
    if (writing_direction == WRITING_DIRECTION_RIGHT_TO_LEFT) {
      Swap(&word_x1, &word_x2);
      Swap(&word_y1, &word_y2);
    }

    // Viewers like evince can get really confused during copy-paste
    // when the baseline wanders around. I've decided to force every
    // word to match the (straight) baseline.  The math below is just
    // projecting the word origin onto the baseline.  All numbers are
    // in the native PDF coordinate system, which has the origin in
    // the bottom left and the unit is points, which is 1/72 inch.
    double word_length;
    double x, y;
    {
      int px = word_x1;
      int py = word_y1;
      double l2 = dist2(line_x1, line_y1, line_x2, line_y2);
      if (l2 == 0) {
        x = line_x1;
        y = line_y1;
      } else {
        double t = ((px - line_x2) * (line_x2 - line_x1) +
                    (py - line_y2) * (line_y2 - line_y1)) / l2;
        x = line_x2 + t * (line_x2 - line_x1);
        y = line_y2 + t * (line_y2 - line_y1);
      }
      word_length = sqrt(static_cast<double>(dist2(word_x1, word_y1,
                                                   word_x2, word_y2)));
      word_length = word_length * 72.0 / ppi;
      x = x * 72 / ppi;
      y = height - (y * 72.0 / ppi);
    }

    int pointsize = 0;
    if (res_it->IsAtBeginningOf(RIL_TEXTLINE)) {
      // Calculate the rotation angle in the PDF cooordinate system,
      // which has the origin in the bottom left. The Tesseract
      // coordinate system has the origin in the upper left.
      //
      // PDF is kind of a like turtle graphics, and we orient the
      // turtle (errr... initial cursor position) with an affine
      // transformation.
      //
      //                                Rotate              RTL    Translate
      //
      // [ x' y' 1 ]  = [ x y 1 ] [ cos𝜃 -sin𝜃 0 ]  [ -1 0 0 ] [ 1 0 0 ]
      //                          [ sin𝜃  cos𝜃 0 ]  [  0 1 0 ] [ 0 1 0 ]
      //                          [   0    0   1 ]  [  0 0 1 ] [ x y 1 ]
      //
      double theta = atan2(static_cast<double>(line_y1 - line_y2),
                           static_cast<double>(line_x2 - line_x1));
      double a, b, c, d;
      a = cos(theta);
      b = sin(theta);
      c = -sin(theta);
      d = cos(theta);
      switch(writing_direction) {
        case WRITING_DIRECTION_RIGHT_TO_LEFT:
          a = -a;
          b = -b;
          c = -c;
          break;
        case WRITING_DIRECTION_TOP_TO_BOTTOM:
          // TODO(jbreiden) Consider switching PDF writing mode to vertical.
          break;
        default:
          break;
      }

      pdf_str.add_str_double("",  prec(a));  // . This affine matrix
      pdf_str.add_str_double(" ", prec(b));  // . sets the coordinate
      pdf_str.add_str_double(" ", prec(c));  // . system for all
      pdf_str.add_str_double(" ", prec(d));  // . text in the entire
      pdf_str.add_str_double(" ", prec(x));  // . line.
      pdf_str.add_str_double(" ", prec(y));  // .
      pdf_str += (" Tm ");                   // Place cursor absolutely
    } else {
      double offset = sqrt(static_cast<double>(dist2(old_x, old_y, x, y)));
      pdf_str.add_str_double(" ", prec(offset));  // Delta x in pts
      pdf_str.add_str_double(" ", 0);             // Delta y in pts
      pdf_str += (" Td ");                        // Relative moveto
    }
    old_x = x;
    old_y = y;

    // Adjust font size on a per word granularity. Pay attention to
    // pointsize, old_pointsize, and pdf_str. We've found that for
    // in Arabic, Tesseract will happily return a pointsize of zero,
    //  so we make up a default number to protect ourselves.
    {
      bool bold, italic, underlined, monospace, serif, smallcaps;
      int font_id;
      res_it->WordFontAttributes(&bold, &italic, &underlined, &monospace,
                                 &serif, &smallcaps, &pointsize, &font_id);
      const int kDefaultPointSize = 8;
      if (pointsize <= 0)
        pointsize = kDefaultPointSize;
      if (pointsize != old_pointsize) {
        char textfont[20];
        snprintf(textfont, sizeof(textfont), "/f-0-0 %d Tf ", pointsize);
        pdf_str += textfont;
        old_pointsize = pointsize;
      }
    }

    bool last_word_in_line = res_it->IsAtFinalElement(RIL_TEXTLINE, RIL_WORD);
    bool last_word_in_block = res_it->IsAtFinalElement(RIL_BLOCK, RIL_WORD);
    STRING pdf_word("");
    int pdf_word_len = 0;
    do {
      const char *grapheme = res_it->GetUTF8Text(RIL_SYMBOL);
      if (grapheme && grapheme[0] != '\0') {
        // TODO(jbreiden) Do a real UTF-16BE conversion
        // http://en.wikipedia.org/wiki/UTF-16#Example_UTF-16_encoding_procedure
        string_32 utf32;
        CubeUtils::UTF8ToUTF32(grapheme, &utf32);
        char utf16[20];
        for (int i = 0; i < static_cast<int>(utf32.length()); i++) {
          snprintf(utf16, sizeof(utf16), "<%04X>", utf32[i]);
          pdf_word += utf16;
          pdf_word_len++;
        }
      }
      delete []grapheme;
      res_it->Next(RIL_SYMBOL);
    } while (!res_it->Empty(RIL_BLOCK) && !res_it->IsAtBeginningOf(RIL_WORD));
    if (word_length > 0 && pdf_word_len > 0 && pointsize > 0) {
      double h_stretch =
          kCharWidth * prec(100.0 * word_length / (pointsize * pdf_word_len));
      pdf_str.add_str_double("", h_stretch);
      pdf_str += " Tz";          // horizontal stretch
      pdf_str += " [ ";
      pdf_str += pdf_word;       // UTF-16BE representation
      pdf_str += " ] TJ";        // show the text
    }
    if (last_word_in_line) {
      pdf_str += " \n";
    }
    if (last_word_in_block) {
      pdf_str += "ET\n";         // end the text object
    }
  }
  char *ret = new char[pdf_str.length() + 1];
  strcpy(ret, pdf_str.string());
  delete res_it;
  return ret;
}
예제 #12
0
파일: naiprim.c 프로젝트: jturney/aquarius
/*
 * Calculate NAIs with the Rys Polynomial algorithm of Ishida
 *  K. Ishida, J. Chem. Phys. 95, 5198-205 (1991)
 *  Ishida, K., J. Chem. Phys., 98, 2176 (1993)
 */
void naiprim(int la, const int lb, const double* posa, const double* posb, const double* posc,
             double za, double zb, double charge, double* integrals,
             double* gtable)
{
    const double PI = 3.1415926535897932384626433832795;
    int vmax = la + lb;

    double posp[3];
    double zp;
    double afac[3], bfac[3], cfac[3];
    double sfac;
    double A0, Z;
    int i, nint;
    int ax, ay, bx, by;
    int ainc, binc, iinc, jinc;
    double* table1;
    double* table2;
    double* target;
    double* integral;

    ainc = (vmax + 1);
    binc = ainc * (la + 1);

    iinc = 1;
    jinc = iinc * (la + 1) * (la + 2) / 2;
    nint = jinc * (lb + 1) * (lb + 2) / 2;

    zp = za + zb;

    posp[0] = (posa[0] * za + posb[0] * zb) / zp;
    posp[1] = (posa[1] * za + posb[1] * zb) / zp;
    posp[2] = (posa[2] * za + posb[2] * zb) / zp;

    afac[0] = posp[0] - posa[0];
    afac[1] = posp[1] - posa[1];
    afac[2] = posp[2] - posa[2];
    bfac[0] = posp[0] - posb[0];
    bfac[1] = posp[1] - posb[1];
    bfac[2] = posp[2] - posb[2];
    cfac[0] = posp[0] - posc[0];
    cfac[1] = posp[1] - posc[1];
    cfac[2] = posp[2] - posc[2];
    sfac = 0.5 / zp;

    A0 = -charge * 2 * PI * exp(-za * zb * dist2(posa, posb) / zp) / zp;
    Z = dist2(posp, posc) * zp;

    //fmrecursive(Z, vmax, (double*)gtable);
    for (i = 0;i <= vmax;i++)
        gtable[i] = fm(Z, i) * A0;

    target = &gtable[la * ainc + lb * binc];

    // fill table with x
    fillgtable(&gtable[0], la, lb, afac[0], bfac[0], cfac[0], sfac, ainc, binc);

    // loop over all possible distributions of x momenta
    table1 = target;
    integral = &integrals[nint - 1];
    for (bx = lb;bx >= 0;bx--)
    {
        for (ax = la;ax >= 0;ax--)
        {
            // and fill remainder with y from that point
            fillgtable(table1, la - ax, lb - bx, afac[1], bfac[1], cfac[1], sfac, ainc, binc);

            // loop over all possible distributions of y momenta given x
            table2 = target;
            for (by = lb - bx;by >= 0;by--)
            {
                for (ay = la - ax;ay >= 0;ay--)
                {
                    // and fill remainder with z from that point
                    fillgtable(table2, la - ax - ay, lb - bx - by, afac[2], bfac[2], cfac[2], sfac, ainc, binc);

                    *integral += *target;

                    table2 -= ainc;
                    integral -= iinc;
                }
                table2 -= binc - ainc * (la - ax + 1);
                integral -= jinc - iinc * (la - ax + 1);
            }
            table1 -= ainc;
            integral -= iinc * (la - ax + 1) - jinc * (lb - bx + 1);
        }
        table1 -= binc - ainc * (la + 1);
        integral -= jinc * (lb - bx + 1) - iinc * (la + 1) * (la + 2) / 2;
    }
}
예제 #13
0
/*
 * Make the mail daemon run through the dungeon.  The daemon will run over
 * any monsters that are in its path, but will replace them later.  Return
 * FALSE if the md gets stuck in a position where there is a monster.  Return
 * TRUE otherwise.
 */
STATIC_OVL boolean
md_rush(struct monst *md, register int tx, register int ty)
                 
                    		/* destination of mail daemon */
{
    struct monst *mon;			/* displaced monster */
    register int dx, dy;		/* direction counters */
    int fx = md->mx, fy = md->my;	/* current location */
    int nfx = fx, nfy = fy,		/* new location */
        d1, d2;				/* shortest distances */

    /*
     * It is possible that the monster at (fx,fy) is not the md when:
     * the md rushed the hero and failed, and is now starting back.
     */
    if (m_at(fx, fy) == md) {
        remove_monster(fx, fy);		/* pick up from orig position */
        newsym(fx, fy);
    }

    /*
     * At the beginning and exit of this loop, md is not placed in the
     * dungeon.
     */
    while (1) {
        /* Find a good location next to (fx,fy) closest to (tx,ty). */
        d1 = dist2(fx,fy,tx,ty);
        for (dx = -1; dx <= 1; dx++) for(dy = -1; dy <= 1; dy++)
                if ((dx || dy) && isok(fx+dx,fy+dy) &&
                        !IS_STWALL(levl[fx+dx][fy+dy].typ)) {
                    d2 = dist2(fx+dx,fy+dy,tx,ty);
                    if (d2 < d1) {
                        d1 = d2;
                        nfx = fx+dx;
                        nfy = fy+dy;
                    }
                }

        /* Break if the md couldn't find a new position. */
        if (nfx == fx && nfy == fy) break;

        fx = nfx;			/* this is our new position */
        fy = nfy;

        /* Break if the md reaches its destination. */
        if (fx == tx && fy == ty) break;

        if ((mon = m_at(fx,fy)) != 0)	/* save monster at this position */
            verbalize("%s", md_exclamations());
        else if (fx == u.ux && fy == u.uy)
            verbalize("Excuse me.");

        place_monster(md,fx,fy);	/* put md down */
        newsym(fx,fy);			/* see it */
        flush_screen(0);		/* make sure md shows up */
        delay_output();			/* wait a little bit */

        /* Remove md from the dungeon.  Restore original mon, if necessary. */
        if (mon) {
            if ((mon->mx != fx) || (mon->my != fy))
                place_worm_seg(mon, fx, fy);
            else
                place_monster(mon, fx, fy);
        } else
            remove_monster(fx, fy);
        newsym(fx,fy);
    }

    /*
     * Check for a monster at our stopping position (this is possible, but
     * very unlikely).  If one exists, then have the md leave in disgust.
     */
    if ((mon = m_at(fx, fy)) != 0) {
        place_monster(md, fx, fy);	/* display md with text below */
        newsym(fx, fy);
        verbalize("This place's too crowded.  I'm outta here.");

        if ((mon->mx != fx) || (mon->my != fy))	/* put mon back */
            place_worm_seg(mon, fx, fy);
        else
            place_monster(mon, fx, fy);

        newsym(fx, fy);
        return FALSE;
    }

    place_monster(md, fx, fy);	/* place at final spot */
    newsym(fx, fy);
    flush_screen(0);
    delay_output();			/* wait a little bit */

    return TRUE;
}
예제 #14
0
파일: rigid.c 프로젝트: ohirose/cpd
int rigid(double       **  W,        /*  D+1 x  D          | Linear map            */
          double       **  T,        /*  M   x  D          | Moved points          */
          double       **  P,        /*  M+1 x  N+1        | Matching probablity   */
          double       *** C,        /*  4 x max(M,N) x D  | Working memory        */
          double       *   S,        /*  nlp x M x D       | Working wemory (3D)   */
          const double **  X,        /*  N   x  D          | Point set 1 (Data)    */
          const double **  Y,        /*  M   x  D          | Point set 2 (Data)    */
          const int        size[3],  /*  M,  N, D          | D must be 2 or 3      */
          const double     prms[2],  /*  parameters: nloop, omg                    */
          const int        verb      /*  flag: verbose                             */
       ){
  int i,j,m,n,d,M,N,D,lp,ws,wi[WSIZE]; int info; char jobz='A';
  int nlp=(int)prms[0]; double omg=prms[1],reg=1e-9;
  double conv,s,noise,sgm2=0,pres1=1e10,pres2=1e20,val,c1,c2;
  double mX[3],mY[3],A[9],B[9],a[3],U[9],L[3],Vt[9],wd[WSIZE];
  double **R,**PXc,**Xc,**Yc;

  M=size[0];N=size[1];D=size[2]; assert(D<=3);
  R=W;PXc=C[0];Xc=C[1];Yc=C[2];ws=WSIZE;

  /* initialize */
  for(d=0;d<D;d++)for(i=0;i<D;i++) R[d][i]=d==i?1:0;
  for(m=0;m<M;m++)for(d=0;d<D;d++) T[m][d]=Y[m][d];
  for(m=0;m<M;m++)for(n=0;n<N;n++) sgm2+=dist2(X[n],Y[m],D);sgm2/=M*N*D;

  /* main computation */
  for(lp=0;lp<nlp;lp++){noise=(pow(2.0*M_PI*sgm2,0.5*D)*M*omg)/(N*(1-omg));
    if(S)for(m=0;m<M;m++)for(d=0;d<D;d++) S[m+d*M+lp*M*D]=T[m][d];

    /* compute matching probability */
    for(n=0;n<=N;n++) P[M][n]=0;
    for(m=0;m<=M;m++) P[m][N]=0;
    for(m=0;m< M;m++)for(n=0;n<N;n++) P[m][n]=exp(-dist2(X[n],T[m],D)/(2.0*sgm2))+reg;
    for(m=0;m< M;m++)for(n=0;n<N;n++) P[M][n]+=P[m][n];
    for(m=0;m<=M;m++)for(n=0;n<N;n++) P[m][n]/=P[M][n]+noise;
    for(m=0;m< M;m++)for(n=0;n<N;n++) P[m][N]+=P[m][n];
    for(m=0;m< M;m++) P[M][N]+=P[m][N];

    /* centerize X and Y */
    for(d=0;d<D;d++){mX[d]=0;for(n=0;n<N;n++) mX[d]+=X[n][d]*P[M][n];mX[d]/=P[M][N];}
    for(d=0;d<D;d++){mY[d]=0;for(m=0;m<M;m++) mY[d]+=Y[m][d]*P[m][N];mY[d]/=P[M][N];}
    for(n=0;n<N;n++)for(d=0;d<D;d++) Xc[n][d]=X[n][d]-mX[d];
    for(m=0;m<M;m++)for(d=0;d<D;d++) Yc[m][d]=Y[m][d]-mY[d];

    /* A=Xc'*P'*Yc */
    for(m=0;m<M;m++)for(d=0;d<D;d++){PXc[m][d]=0;for(n=0;n<N;n++) PXc[m][d]+=P  [m][n]*Xc[n][d];}
    for(d=0;d<D;d++)for(i=0;i<D;i++){A[d+i*D ]=0;for(m=0;m<M;m++) A[d+i*D] +=PXc[m][d]*Yc[m][i];}
    for(d=0;d<D;d++)for(i=0;i<D;i++) B[d+i*D]=A[d+i*D];

    /* compute svd of A and rotation matrix R */
    dgesdd_(&jobz,&D,&D,B,&D,L,U,&D,Vt,&D,wd,&ws,wi,&info);
    val=det(U,D)*det(Vt,D); if(val<0)for(d=0;d<D;d++)U[d+D*(D-1)]*=-1;
    for(i=0;i<D;i++)for(j=0;j<D;j++){R[i][j]=0;for(d=0;d<D;d++) R[i][j]+=U[i+d*D]*Vt[d+j*D];}

    /* compute scaling s and intercept a */
    c1=c2=0;
    for(d=0;d<D;d++)for(i=0;i<D;i++)c1+=A[d+i*D]*R[d][i];
    for(d=0;d<D;d++)for(m=0;m<M;m++)c2+=SQ(Yc[m][d])*P[m][N]; s=c1/c2;

    /* compute transformation T */
    for(d=0;d<D;d++){val=0;for(i=0;i<D;i++)val+=R[d][i]*mY[i];a[d]=mX[d]-s*val;}
    for(m=0;m<M;m++)for(d=0;d<D;d++){val=0;for(i=0;i<D;i++)val+=R[d][i]*Y[m][i];T[m][d]=s*val+a[d];}

    /* compute sgm2 (corresponds to residual) */
    pres2=pres1;pres1=sgm2;sgm2=-s*c1;
    for(n=0;n<N;n++)for(d=0;d<D;d++) sgm2+=SQ(Xc[n][d])*P[M][n]; sgm2/=P[M][N]*D;

    /* check convergence */
    conv=log(pres2)-log(sgm2 );
    if(verb) printOptIndex('r',lp,P[M][N],sqrt(sgm2),noise,conv);
    if(fabs(conv)<1e-8)break;
  }

  return lp;
}
예제 #15
0
	/**test two vectors for equality with given treshold*/
	bool eqals( const vec2& v, float eps )const{
	    return dist2( v ) <= sqr( eps );
	};
예제 #16
0
 P inv() const{
     return P(x/dist2(), y/dist2());
 }
예제 #17
0
파일: gbutil.c 프로젝트: alwanderer/gromacs
void orient(int natom, rvec *x, rvec *v, rvec angle, matrix box)
{
    real  longest, rij, rzi;
    int   i, j, m, max_i = 0, max_j = 0;
    rvec  origin;
    int   temp;
    real  alfa = 0, beta = 0, gamma = 0;
    t_pbc pbc;

    set_pbc(&pbc, -1, box);

    /*first i am going to look for the longest atom-atom distance*/
    longest = dist2(&pbc, x[0], x[1]);
    i       = 0;
    j       = 1;
    for (i = 0; (i < natom); i++)
    {
        for (j = 0; (j < natom); j++)
        {
            rij = dist2(&pbc, x[i], x[j]);
            if (rij > longest)
            {
                max_i   = i;
                max_j   = j;
                longest = rij;
            }
        }
    }
    /* first check if x[max_i]<x[max_j] else swap*/
    if (x[max_i][2] > x[max_j][2])
    {
        temp  = max_i;
        max_i = max_j;
        max_j = temp;
    }

    /*set the origin to x[i]*/
    for (m = 0; (m < DIM); m++)
    {
        origin[m] = x[max_i][m];
    }
    for (i = 0; (i < natom); i++)
    {
        for (m = 0; (m < DIM); m++)
        {
            x[i][m] -= origin[m];
        }
    }

    /* calculate the rotation angles alfa(x_axis) and beta(y_axis)
     * the rotation angles must be calculated clockwise looking
     * along the rotation axis to the origin*
     * alfa (x-axis)
     */
    alfa = atan(x[max_j][ZZ]/x[max_j][YY])-M_PI_2;
    beta = M_PI_2-atan(x[max_j][ZZ]/x[max_j][XX]);
    rotate_conf(natom, x, v, alfa, beta, gamma);

    /* now search the longest distance for rotation along the z_axis */
    longest = distance_to_z(x[0]);
    max_i   = 0;
    for (i = 1; (i < natom); i++)
    {
        rzi = distance_to_z(x[i]);
        if (rzi > longest)
        {
            longest = rzi;
            max_i   = i;
        }
    }
    gamma = atan(x[max_i][YY]/x[max_i][XX])-M_PI_2;
    rotate_conf(natom, x, v, 0, 0, gamma);
    angle[0] = alfa;
    angle[1] = beta;
    angle[2] = gamma;
} /*orient()*/
예제 #18
0
inline T	dist( const tgeVector3T<T>& aV1, const tgeVector3T<T>& aV2 )
{
	return std::sqrt( dist2( aV1, aV2 ) );
}
예제 #19
0
        void meanShiftSegmentation(const oclMat &src, Mat &dst, int sp, int sr, int minsize, TermCriteria criteria)
        {
            CV_Assert(src.type() == CV_8UC4);
            const int nrows = src.rows;
            const int ncols = src.cols;
            const int hr = sr;
            const int hsp = sp;

            // Perform mean shift procedure and obtain region and spatial maps
            oclMat h_rmap, h_spmap;
            meanShiftProc(src, h_rmap, h_spmap, sp, sr, criteria);
            Mat rmap = h_rmap;
            Mat spmap = h_spmap;

            Graph<SegmLinkVal> g(nrows * ncols, 4 * (nrows - 1) * (ncols - 1)
                                 + (nrows - 1) + (ncols - 1));

            // Make region adjacent graph from image
            Vec4b r1;
            Vec4b r2[4];
            Vec2s sp1;
            Vec2s sp2[4];
            int dr[4];
            int dsp[4];
            for (int y = 0; y < nrows - 1; ++y)
            {
                Vec4b *ry = rmap.ptr<Vec4b>(y);
                Vec4b *ryp = rmap.ptr<Vec4b>(y + 1);
                Vec2s *spy = spmap.ptr<Vec2s>(y);
                Vec2s *spyp = spmap.ptr<Vec2s>(y + 1);
                for (int x = 0; x < ncols - 1; ++x)
                {
                    r1 = ry[x];
                    sp1 = spy[x];

                    r2[0] = ry[x + 1];
                    r2[1] = ryp[x];
                    r2[2] = ryp[x + 1];
                    r2[3] = ryp[x];

                    sp2[0] = spy[x + 1];
                    sp2[1] = spyp[x];
                    sp2[2] = spyp[x + 1];
                    sp2[3] = spyp[x];

                    dr[0] = dist2(r1, r2[0]);
                    dr[1] = dist2(r1, r2[1]);
                    dr[2] = dist2(r1, r2[2]);
                    dsp[0] = dist2(sp1, sp2[0]);
                    dsp[1] = dist2(sp1, sp2[1]);
                    dsp[2] = dist2(sp1, sp2[2]);

                    r1 = ry[x + 1];
                    sp1 = spy[x + 1];

                    dr[3] = dist2(r1, r2[3]);
                    dsp[3] = dist2(sp1, sp2[3]);

                    g.addEdge(pix(y, x, ncols), pix(y, x + 1, ncols), SegmLinkVal(dr[0], dsp[0]));
                    g.addEdge(pix(y, x, ncols), pix(y + 1, x, ncols), SegmLinkVal(dr[1], dsp[1]));
                    g.addEdge(pix(y, x, ncols), pix(y + 1, x + 1, ncols), SegmLinkVal(dr[2], dsp[2]));
                    g.addEdge(pix(y, x + 1, ncols), pix(y + 1, x, ncols), SegmLinkVal(dr[3], dsp[3]));
                }
            }
            for (int y = 0; y < nrows - 1; ++y)
            {
                r1 = rmap.at<Vec4b>(y, ncols - 1);
                r2[0] = rmap.at<Vec4b>(y + 1, ncols - 1);
                sp1 = spmap.at<Vec2s>(y, ncols - 1);
                sp2[0] = spmap.at<Vec2s>(y + 1, ncols - 1);
                dr[0] = dist2(r1, r2[0]);
                dsp[0] = dist2(sp1, sp2[0]);
                g.addEdge(pix(y, ncols - 1, ncols), pix(y + 1, ncols - 1, ncols), SegmLinkVal(dr[0], dsp[0]));
            }
            for (int x = 0; x < ncols - 1; ++x)
            {
                r1 = rmap.at<Vec4b>(nrows - 1, x);
                r2[0] = rmap.at<Vec4b>(nrows - 1, x + 1);
                sp1 = spmap.at<Vec2s>(nrows - 1, x);
                sp2[0] = spmap.at<Vec2s>(nrows - 1, x + 1);
                dr[0] = dist2(r1, r2[0]);
                dsp[0] = dist2(sp1, sp2[0]);
                g.addEdge(pix(nrows - 1, x, ncols), pix(nrows - 1, x + 1, ncols), SegmLinkVal(dr[0], dsp[0]));
            }

            DjSets comps(g.numv);

            // Find adjacent components
            for (int v = 0; v < g.numv; ++v)
            {
                for (int e_it = g.start[v]; e_it != -1; e_it = g.edges[e_it].next)
                {
                    int c1 = comps.find(v);
                    int c2 = comps.find(g.edges[e_it].to);
                    if (c1 != c2 && g.edges[e_it].val.dr < hr && g.edges[e_it].val.dsp < hsp)
                        comps.merge(c1, c2);
                }
            }

            vector<SegmLink> edges;
            edges.reserve(g.numv);

            // Prepare edges connecting differnet components
            for (int v = 0; v < g.numv; ++v)
            {
                int c1 = comps.find(v);
                for (int e_it = g.start[v]; e_it != -1; e_it = g.edges[e_it].next)
                {
                    int c2 = comps.find(g.edges[e_it].to);
                    if (c1 != c2)
                        edges.push_back(SegmLink(c1, c2, g.edges[e_it].val));
                }
            }

            // Sort all graph's edges connecting differnet components (in asceding order)
            std::sort(edges.begin(), edges.end());

            // Exclude small components (starting from the nearest couple)
            for (size_t i = 0; i < edges.size(); ++i)
            {
                int c1 = comps.find(edges[i].from);
                int c2 = comps.find(edges[i].to);
                if (c1 != c2 && (comps.size[c1] < minsize || comps.size[c2] < minsize))
                    comps.merge(c1, c2);
            }

            // Compute sum of the pixel's colors which are in the same segment
            Mat h_src = src;
            vector<Vec4i> sumcols(nrows * ncols, Vec4i(0, 0, 0, 0));
            for (int y = 0; y < nrows; ++y)
            {
                Vec4b *h_srcy = h_src.ptr<Vec4b>(y);
                for (int x = 0; x < ncols; ++x)
                {
                    int parent = comps.find(pix(y, x, ncols));
                    Vec4b col = h_srcy[x];
                    Vec4i &sumcol = sumcols[parent];
                    sumcol[0] += col[0];
                    sumcol[1] += col[1];
                    sumcol[2] += col[2];
                }
            }

            // Create final image, color of each segment is the average color of its pixels
            dst.create(src.size(), src.type());

            for (int y = 0; y < nrows; ++y)
            {
                Vec4b *dsty = dst.ptr<Vec4b>(y);
                for (int x = 0; x < ncols; ++x)
                {
                    int parent = comps.find(pix(y, x, ncols));
                    const Vec4i &sumcol = sumcols[parent];
                    Vec4b &dstcol = dsty[x];
                    dstcol[0] = static_cast<uchar>(sumcol[0] / comps.size[parent]);
                    dstcol[1] = static_cast<uchar>(sumcol[1] / comps.size[parent]);
                    dstcol[2] = static_cast<uchar>(sumcol[2] / comps.size[parent]);
                }
            }
        }
예제 #20
0
int _tmain(int argc, _TCHAR* argv[])
{
	//cpp11 random, need to include <random>
	std::random_device rd;
	std::mt19937 mt(rd());

	//DNA population containers
	std::vector<std::shared_ptr<DNA>> vPopulation;
	std::vector<std::shared_ptr<DNA>> vPool;

	//Initial Data.
	//
	//Get initial Population.
	int iGeneLength = 0, iPopulationSize;
	std::cout << "Type Population Size (int): ";
	std::string sPopulationSize;
	std::getline(std::cin, sPopulationSize);
	iPopulationSize = std::stoi(sPopulationSize);
	/*
	//Get the value typed as string
	std::string sPopulationSize;
	std::getline(std::cin, sPopulationSize);
	//Convert to int
	iPopulationSize = stoi(sPopulationSize);
	*/
	std::cout << "Selected Population Size (int): " << iPopulationSize << std::endl;
	//
	//Get the target string.
	std::cout << "Type a sentence for the target: ";
	std::string sTarget;
	std::getline(std::cin, sTarget);
	std::cout << std::endl;
	//
	//Initialize loop trackers.
	iGeneLength = sTarget.size();
	bool bReachedGoal = false;
	unsigned long lGeneration = 1;

	//Initiate population
	for (int i = 0; i < iPopulationSize; ++i){
		std::shared_ptr<DNA> temp(new DNA(iGeneLength));
		vPopulation.push_back(temp);
	}

	//Begin the Circle Of Life™
	while (!bReachedGoal){

		//Print Info
		std::cout << "Generation: " << lGeneration << std::endl;

		//Determine fitness and print highest and gene
		for (auto it = vPopulation.begin(); it != vPopulation.end(); ++it){
			(*it)->SetFitness(sTarget);
		}
		double highest = 0;
		int index = 0;
		for (auto it = vPopulation.begin(); it != vPopulation.end(); ++it){
			if ((*it)->GetFitness() > highest){
				highest = (*it)->GetFitness();
				index = it - vPopulation.begin();
			}
		}

		//Output results, and check if target has been met.
		std::cout << "Highest Fitness: " << highest * 100.0 << "% with gene sequence: " 
			<< vPopulation.at(index)->GetGeneString() << std::endl;
		if (highest == 1) {
			bReachedGoal = true;
			std::cout << "\nFinished. Press enter to exit the program." << std::endl;
			std::cin.ignore();
		}

		//Fill out mating pool
		vPool.clear();
		for (auto it = vPopulation.begin(); it != vPopulation.end(); ++it){
			int n = (*it)->GetFitness() * 100;
			for (int i = 0; i < n; ++i){

				/*Create a new shared_ptr i number of times based on
				the fitness. */
				vPool.push_back((*it));
			}
		}

		//Clear old population (not realistic but deal)
		vPopulation.clear();

		//Mate pairs to recreate population
		std::uniform_int_distribution<int> dist2(0, vPool.size()-1);

		//Refill population to same size as it was with new DNA
		for (int i = 0; i < iPopulationSize; ++i){

			//Init iterators
			int x = 0;
			int y = 0;

			//Set random value based on size of pool container
			x = dist2(mt);
			y = dist2(mt);

			//Make sure not to use the same parent twice
			while (vPool[x] == vPool[y]){
				y = dist2(mt);
			}

			//Create new DNA based on parents
			std::shared_ptr<DNA> temp(new DNA(vPool.at(x), vPool.at(y)));

			//Add mutation
			temp->Mutate();

			//Place new DNA in population container
			vPopulation.push_back(temp);
		}

		//Increase generation
		++lGeneration;
	}
	return 0;
}
예제 #21
0
// compute distance from c to segment between a and b
double DistancePointSegment(PT a, PT b, PT c) {
	return sqrt(dist2(c, ProjectPointSegment(a, b, c)));
}
예제 #22
0
int ProjectShell::findNodes(int red, int blue, double * iP, int nP)
{
  // first of all, check against red and blue vertices
  //
  if (dbg)
    {
      std::cout<< "red, blue, nP, P " << red << " " << blue << " " << nP <<"\n";
      for (int n=0; n<nP; n++)
	std::cout << " \t" << iP[2*n] << "\t" << iP[2*n+1] << "\n";

    }
  int * foundIds = new int [nP];
  for (int i=0; i<nP; i++)
    {
      double * pp = &iP[2*i];// iP+2*i
      int found = 0; 
      // first, are they on vertices from red or blue?
      PSTriangle2D & redTri = m_redMesh[red];
      int j=0;
      for (j=0; j<3&& !found; j++)
	{
          int node = redTri.v[j];
          double d2 = dist2( pp, m_xy+(3*node) );	
          if (dbg && i==0)
	    std::cout<< "  red node " << j << " " << node << " " << m_xy[3*node] << " " << m_xy[3*node+1] << " d2:" << d2 <<  " \n";
	  if (d2<epsilon)
	    {
              foundIds[i] = node; // no new node
	      found = 1;
	    }
	}
      PSTriangle2D & blueTri = m_blueMesh[blue];
      for (j=0; j<3 && !found; j++)
        {
          int node = blueTri.v[j];
          double d2 = dist2( pp, m_xy+(3*node) );	
          if (dbg && i==0) 
	    std::cout<< "  blu node " << j << " " << node << " " << m_xy[3*node] << " " << m_xy[3*node+1] << " d2:" << d2 <<  " \n";
	  if (d2<epsilon)
	    {
              foundIds[i] = node; // no new node
	      found = 1;
            }
        }
      if (!found)
        {
	  // find the edge it belongs, first
	  //
	  for (j=0; j<3; j++)
	    {
	      int edge = redTri.e[j];
	      int ae = abs(edge);
	      int v1 = ps_edges[ae-1].v[0];
	      int v2 = ps_edges[ae-1].v[1];
	      double area = area2D (&m_xy[3*v1], &m_xy[3*v2], pp);
	      if (dbg)
		std::cout << "   edge " << j << ": " << edge << " " << v1 << " " << v2 << "  area : " << area << "\n"; 
	      if ( fabs(area) < epsilon*epsilon  )
		{
		  // found the edge; now find if there is a point in the list here
		  std::list<int> & expts = ps_edges[ae-1].extraNodes;
		  // if the points pp is between extra points, then just give that id
		  // if not, create a new point, (check the id)
		  //
		  std::list<int>::iterator it;
		  for ( it = expts.begin(); it!=expts.end() && !found; it++)
		    {
		      int pnt = *it;
		      double d2 = dist2(pp, &m_xy[3*pnt]);
		      if (d2<epsilon)
			{
			  found = 1;
			  foundIds[i] = pnt;
			}
		    }
		  if (!found)
		    {
		      // create a new point in 2d (at the intersection)
		      foundIds [i] = m_num2dPoints;
		      expts.push_back(m_num2dPoints);
		      if (m_2dcapacity < m_num2dPoints+1)
			{
			  // exit, underestimate number of intersection points
			  std::cout << " underestimate capacity for 2d array\n";
			  // double the capacity of m_xy array
			     
			  double * new_xy = new double [6* m_2dcapacity];
			  int jj=0;
			  for (jj=0; jj<3*m_2dcapacity; jj++)
			    {
			      new_xy[jj] = m_xy[jj];
				
			    }
			  for (jj=3*m_2dcapacity-1; jj<6*m_2dcapacity; jj+=3)
			    new_xy[jj] = 0.;
			  // make 0 the z coordinate
			  m_2dcapacity *= 2;
			  delete [] m_xy;
			  m_xy = new_xy;
			}
		      m_xy[3*m_num2dPoints] = pp[0];
		      m_xy[3*m_num2dPoints+1] = pp[1];
		      m_num2dPoints++;
		      if (dbg)
			{
			  std::cout<< " new 2d " << m_num2dPoints - 1 << " : " << pp[0] << " " << pp[1] <<  "on edge " << ae << "\n";
			}
		      found = 1;
		    }
		} 
	    }
        }
      if (!found)
        {
	  std::cout << " a point pp is not on a red triangle " << *pp << " " << pp[1] << " red triangle " << red << " \n";
	  exit (1);
        }
    }
  // now we can build the triangles, from P array, with foundIds
  if (nP>=3)
    {
      // what are the triangles 
      FinalTriangle ftr;
      ftr.redTriangle = red;
      ftr.blueTriangle = blue;
      ftr.v[0] = foundIds[0];
      for (int i=1; i<=nP-2; i++)
	{
	  // triangle 0, i, i+1
	  ftr.v[1]=foundIds[i];
	  ftr.v[2] = foundIds[i+1];
	  m_finalMesh.push_back(ftr);
	  if (dbg)
	    {
	      std::cout << " triangle " << ftr.v[0] << " " << ftr.v[1] << " " << ftr.v[2] << "\n"; 
	    }
	}
    }
  delete [] foundIds;
  foundIds = NULL;
  return 0;
}
예제 #23
0
/* dist:
 * Returns the distance between points a and b.
 */
static COORD dist(Ppoint_t a, Ppoint_t b)
{
    return sqrt(dist2(a, b));
}
예제 #24
0
int ProjectShell::projectIn2D()
{
  // considering the direction, classify each projected triangle as red or blue
  // the red ones are positive (inbound), blue ones are negative
  // first decide the other 2 vectors
  double vv[3]={0.,-1., 0.};
  cross(m_dirX, m_direction, vv);
  
  double d1 = dist(m_dirX);
  if (d1<1.e-5)// consider another direction
    {
      vv[1]=0.; vv[2]=-1.;
      cross(m_dirX, m_direction, vv);
      d1 = dist(m_dirX);
      if (d1<1.e-5)
	{
	  std::cerr << "cannot find a suitable direction; abort\n";
	  return 1;
	}
    }
  int k=0;
  for (k=0; k<3; k++)
    m_dirX[k]/=d1;
  cross(m_dirY, m_direction, m_dirX);
  // dirY must be already normalized, but why worry
  d1 = dist(m_dirY);
  if (d1==0.)
    {
      std::cerr<< " get out of here, it is hopeless\n";
      return 1;
    }
  for (k=0; k<3; k++)
    m_dirY[k]/=d1;
  
  // now do the projection in 2d
  //   we have 3 vectors, m_dirX, m_dirY, m_direction
  // for every point, A, the projection in xy plane will be just
  // xA = A . u; yA = A . v  (dirX and dirY)

  // also, it is important to compute the normal 
  // some triangles will be reverted and some may be projecting to a line

  // coordinates of the projected nodes on 2D
  // what could be a good estimate for total number of points in 2D? 
  //  we start with 2d capacity 3* m_numNodes 
  m_2dcapacity = m_numNodes*3;
  m_num2dPoints = m_numNodes; // directly project 3d nodes in 2d, keep the ids
  m_xy = new double [3*m_2dcapacity];
  // this array will be parallel with the original mesh
  // new nodes will appear after intersection computation
  // triangles are characterized by their orientation: negative, positive or 0

  // the neighbors will be preserved
  // some edges will be collapsed, and also some triangles
  // in those cases, what will be the neighbors?
  // flag them with a high number ()

  //m_finalNodes.resize(3*m_numNodes); // the actual size is n_numNodes first
  for ( k=0; k<m_numNodes; k++)
    {
      // double xx= dot( ps_nodes[k].xyz, m_dirX);
      // double yy= dot( ps_nodes[k].xyz, m_dirY);
      // _finalNodes[k].x =  dot( ps_nodes[k].xyz, m_dirX);
      // _finalNodes[k].y =  dot( ps_nodes[k].xyz, m_dirY);
      m_xy[3*k] = dot( ps_nodes[k].xyz, m_dirX);
      m_xy[3*k+1] = dot( ps_nodes[k].xyz, m_dirY);
      //  m_finalNodes[k].x =  m_xy[2*k];
      //  m_finalNodes[k].y =  m_xy[2*k+1];
    }
  for (k=0; k<m_2dcapacity; k++)
    m_xy[3*k+2] = 0;// the z ccordinate is always 0 !!
  // if any edges are collapsed, the corresponding triangles should be collapsed too
  // we will collapse the triangles first, then the edges
  // when a triangle is collapsed on an edge, it should break in 2 other triangles
  // we should really form another triangle array, in 2D 

  // first, loop over edges and see which are eliminated;
  
  double *edgeLen2D=new double [m_numEdges];
  for ( k=0; k<m_numEdges; k++)
    {
      
      int v0 = ps_edges[k].v[0];
      int v1 = ps_edges[k].v[1];
      edgeLen2D[k] = dist2(&m_xy[3*v0], &m_xy[3*v1]); 
    }
  double *triArea = new double [m_numTriangles];
  for ( k=0; k<m_numTriangles; k++)
    {
      const int * pV =&( ps_triangles[k].v[0]);
      triArea[k] = area2D( &m_xy[ 3*pV[0]],  &m_xy[ 3*pV[1]],  &m_xy[ 3*pV[2]]);
    }
  // if an edge is length 0, we must collapse the nodes, and 2 triangles
  // first construct a new 2d mesh
  // we will have to classify all triangles, edges
  // for each triangle we need to know its original triangle

  // first mark all triangles; then count positive, negative and zero area (some tolerance is needed)
  int numNeg = 0, numPos = 0, numZero = 0;
  // those that are negative will be reverted in the new array
  int * newTriId = new int [m_numTriangles];
  for (k=0; k<m_numTriangles ; k++)
    {
      if (triArea[k] > 0)
	{
	  //numPos++;
          newTriId[k] = numPos++;
	}
      else if (triArea[k] <0)
	{
	  //numNeg ++;
	  newTriId[k] = numNeg++;
	}
      else
	{
	  numZero ++;
	  newTriId[k] = -1;// receive no Id, as it will not be carried along
	}
    }
  // there are 2 groups: red (positive), blue (negative)
  // revert the negative ones, and decide the neighbors

  // red triangles are positive, blue are negative
  // the negative ones need to be reverted; revert the nodes, first, then edges
  // do we really need the edges? or just the neighboring triangles?
  // if a neighbor is the other sign or zero, will become boundary marker (large number, m_numTriangles+1) 
  // 
  // we need to find first 2 triangles (red and blue) that are intersecting
  // they will be the seeds
  m_redMesh = new PSTriangle2D [numPos] ;
  m_blueMesh = new  PSTriangle2D [numNeg];
  m_numPos = numPos;
  m_numNeg = numNeg;
  // do another loop, to really build the 2D mesh we start with
  // we may have potential starting triangles for the marching along, if the sign of one of the neighbors is 
  // different
  for (k=0; k<m_numTriangles ; k++)
    {
      PS3DTriangle & orgTria = ps_triangles[k];
      if (triArea[k] > 0)
	{
	  //numPos++;
          PSTriangle2D & redTria= m_redMesh[newTriId[k]];
	  redTria.oldId = k ; // index 
	  redTria.area = triArea[k];
          for (int j=0; j<3; j++)
	    {
	       
              // copy the edge information too
	      redTria.e[j] = orgTria.e[j]; 
              // qualify the neighbors
	      //
	      int t = orgTria.t[j];
	      redTria.v[j] = orgTria.v[j];
	      if (triArea[t]>0)
		{
		  redTria.t[j] = newTriId[t];// will be the index in red mesh 
		}
	      else
		{
		  redTria.t[j] = numPos; // marker for boundary
		}
	    }
	  
	}
      else if (triArea[k] <0)
	{
	  //numNeg ++;
	  PSTriangle2D & blueTria= m_blueMesh[newTriId[k]];
	  blueTria.oldId = k;
	  blueTria.area =triArea[k]; // this is for debugging I think
          for (int j=0; j<3; j++)
	    {
	      // copy the edge information too
	      blueTria.e[j] = orgTria.e[j];
	      // qualify the neighbors
	      // 
	      int t = orgTria.t[j];
	      blueTria.v[j] = orgTria.v[j];
	      if (triArea[t]<0)
		{
		  blueTria.t[j] = newTriId[t];// will be the index in red mesh 
		}
	      else
		{
		  blueTria.t[j] = numNeg; // marker for boundary
		}
	    }
     
	}
      else
	{
	  // numZero ++;
	  // newTriId[k] = -1;// receive no Id, as it will not be carried along
	  // nothing to do for null triangles
	}
    }
  // revert the blue triangles, so they become positive oriented too
  for (k=0; k<numNeg; k++)
    {
      // 
      PSTriangle2D & blueTri = m_blueMesh[k];
      int tmp = blueTri.v[1];
      blueTri.v[1] = blueTri.v[2];
      blueTri.v[2] = tmp;
      // this is really stupid: 
      // we should have switched triangle 1 and 3, not 2 and 3
      // hard to catch
      tmp = blueTri.t[0];
      blueTri.t[0] = blueTri.t[2];
      blueTri.t[2] = tmp;
      // reverse the edges too
      tmp = blueTri.e[0];
      blueTri.e[0] = -blueTri.e[2];
      blueTri.e[1] = -blueTri.e[1];
      blueTri.e[2] = -tmp; 
    }
  // at this point, we have red triangles and blue triangles, and 2d nodes array
  // we will start creating new triangles, step by step, pointing to the nodes in _finalNodes
  m_numCurrentNodes = m_numNodes;
  if (dbg)
    {
      std::ofstream fout("dbg.m"); 
      fout << "P=[ \n";
      for (int i=0; i<m_numNodes; i++)
	{
	  fout << m_xy[3*i] << " " << m_xy[3*i+1] << "\n";
	}
      fout << "];\n ";
      fout << "Ta=[ \n";
      for (int k=0; k<m_numPos; k++)
	{
	  PSTriangle2D & redTri = m_redMesh[k];
	  for (int j=0; j<3; j++)
	    fout << redTri.v[j]+1 << " " ;
	  for (int jj=0; jj<3; jj++)
	    {
	      fout << redTri.t[jj]+1 << " " ;
	    }
	  fout << "\n";
	}
      fout << "]; \n";
      fout << "Tb=[ \n";
      for (int kk=0; kk<m_numNeg; kk++)
	{
	  PSTriangle2D & blueTri = m_blueMesh[kk];
	  for (int j=0; j<3; j++)
	    fout << blueTri.v[j]+1 << " " ;
	  for (int jj=0; jj<3; jj++)
	    {
	      fout << blueTri.t[jj]+1 << " " ;
	    }
	  fout << "\n";
	}
      fout << "]; \n";
      fout.close();
      
    }
  delete [] edgeLen2D;
  delete [] triArea;
  delete [] newTriId;
  return 0; 
}
예제 #25
0
//#define DEBUG_MODE
int SingleSLAM::poseUpdate3D(bool largeErr) {
	propagateFeatureStates();
//get the feature points corresponding to the map points
	std::vector<Track2DNode*> nodes;
	int num = getStaticMappedTrackNodes(nodes);
	if (num < 1) {
        double* cR = m_camPos.current()->R;
        double* cT = m_camPos.current()->t;
        CamPoseItem* camPos = m_camPos.add(currentFrame(), camId,cR, cT);
        updateCamParamForFeatPts(K, camPos);

		warn(
				"[camera id:%d]intra-camera pose update failed! less than five static map points (%d)",
				camId, num);
        //leaveBACriticalSection();
        //CoSLAM::ptr->pause();
        //enterBACriticalSection();
		return -1;
	}

//choose the feature points for pose estimation
	std::vector<FeaturePoint*> featPts;
	chooseStaticFeatPts(featPts);
	std::vector<FeaturePoint*> mappedFeatPts;

	mappedFeatPts.reserve(nRowBlk * nColBlk * 2);

	for (size_t i = 0; i < featPts.size(); i++) {
		if (featPts[i]->mpt)
			mappedFeatPts.push_back(featPts[i]);
	}

//get the 2D-3D corresponding points
	int n3D2Ds = mappedFeatPts.size();
	Mat_d ms(n3D2Ds, 2), Ms(n3D2Ds, 3), covs(n3D2Ds, 9);
	for (int i = 0; i < n3D2Ds; i++) {
		FeaturePoint* fp = mappedFeatPts[i];
		ms.data[2 * i] = fp->x;
		ms.data[2 * i + 1] = fp->y;

		Ms.data[3 * i] = fp->mpt->x;
		Ms.data[3 * i + 1] = fp->mpt->y;
		Ms.data[3 * i + 2] = fp->mpt->z;

		memcpy(covs.data, fp->mpt->cov, sizeof(double) * 9);
	}

	Mat_d R(3, 3), t(3, 1);
	double* cR = m_camPos.current()->R;
	double* cT = m_camPos.current()->t;

	IntraCamPoseOption opt;

	//test
	Mat_d old_errs(n3D2Ds, 1);
	//get reprojection error beform pose update
	for (int i = 0; i < n3D2Ds; i++) {
		FeaturePoint* fp = mappedFeatPts[i];
		double m[2];
		project(K.data, cR, cT, fp->mpt->M, m);
		old_errs[i] = dist2(m, fp->m);
	}
	//end of test

	intraCamEstimate(K.data, cR, cT, Ms.rows, 0, Ms.data, ms.data,
			Param::maxErr, R.data, t.data, &opt);

	Mat_d new_errs(n3D2Ds, 1);
	for (int i = 0; i < n3D2Ds; i++) {
		FeaturePoint* fp = mappedFeatPts[i];
		double m[2];
		project(K.data, R, t, fp->mpt->M, m);
		new_errs[i] = dist2(m, fp->m);
	}

//find outliers
	int numOut = 0;
	double errThres = largeErr ? 6.0 : 2.0;
	double rm[2], var[4], ivar[4];
	for (int i = 0; i < num; i++) {
		double* pM = nodes[i]->pt->mpt->M;
		double* pCov = nodes[i]->pt->mpt->cov;
		project(K, R, t, pM, rm);
		getProjectionCovMat(K, R, t, pM, pCov, var, Const::PIXEL_ERR_VAR);
		mat22Inv(var, ivar);
		double err = mahaDist2(rm, nodes[i]->pt->m, ivar);
		if (err < errThres) { //inlier
			nodes[i]->pt->reprojErr = err;
			seqTriangulate(K, R, t, nodes[i]->pt->m, pM, pCov,
					Const::PIXEL_ERR_VAR);
			project(K, R, t, pM, rm);
			getProjectionCovMat(K, R, t, pM, pCov, var, Const::PIXEL_ERR_VAR);
			mat22Inv(var, ivar);
			err = mahaDist2(rm, nodes[i]->pt->m, ivar);
			//			if (err >= 1) {
			//				nodes[i]->pt->mpt->setUncertain();
			//				//test
			//				printf("poseUpdate:1\n");
			//			}
		} else {
			//outliers
			numOut++;
			double repErr = dist2(rm, nodes[i]->pt->m);
			nodes[i]->pt->reprojErr = repErr;
			nodes[i]->pt->mpt->setUncertain();
		}
	}
	CamPoseItem* camPos = m_camPos.add(currentFrame(), camId, R.data, t.data);
	updateCamParamForFeatPts(K, camPos);
//	if (currentFrame() > 9)
//		MyApp::bStop = true;
#ifdef DEBUG_MODE
//if the number of outliers are too much (may due to some distant points)
	if (currentFrame() >= 76) {
		//test
		printf("f:%d,cam:%d : n3d2d:%d, num:%d, numOut:%d\n", currentFrame(),
				camId, n3D2Ds, num, numOut);
		char dirPath[1024];
		sprintf(dirPath, "/home/tsou/slam_posefailed/%s", MyApp::timeStr);
		mkdir(dirPath, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);

		savePGM(m_img, "/home/tsou/slam_posefailed/%s/%d_img_%d.pgm",
				MyApp::timeStr, currentFrame(), camId);
		writeMat(Ms, "/home/tsou/slam_posefailed/%s/%d_pts3d_%d.txt",
				MyApp::timeStr, currentFrame(), camId);
		writeMat(ms, "/home/tsou/slam_posefailed/%s/%d_pts2d_%d.txt",
				MyApp::timeStr, currentFrame(), camId);
		writeMat(covs, "/home/tsou/slam_posefailed/%s/%d_cov3d_%d.txt",
				MyApp::timeStr, currentFrame(), camId);
		writeMat(3, 3, K, "/home/tsou/slam_posefailed/%s/%d_K_%d.txt",
				MyApp::timeStr, currentFrame(), camId);
		writeMat(old_errs, "/home/tsou/slam_posefailed/%s/%d_old_errs_%d.txt",
				MyApp::timeStr, currentFrame(), camId);
		writeMat(new_errs, "/home/tsou/slam_posefailed/%s/%d_new_errs_%d.txt",
				MyApp::timeStr, currentFrame(), camId);

		writeMat(3, 3, cR, "/home/tsou/slam_posefailed/%s/%d_oldR_%d.txt",
				MyApp::timeStr, currentFrame(), camId);
		writeMat(3, 1, cT, "/home/tsou/slam_posefailed/%s/%d_oldT_%d.txt",
				MyApp::timeStr, currentFrame(), camId);
		writeMat(R, "/home/tsou/slam_posefailed/%s/%d_R_%d.txt", MyApp::timeStr,
				currentFrame(), camId);
		writeMat(t, "/home/tsou/slam_posefailed/%s/%dT_%d.txt", MyApp::timeStr,
				currentFrame(), camId);

		//test
		logInfo("program paused for debug at camera %d!\n", currentFrame(), camId);
		leaveBACriticalSection();
		CoSLAM::ptr->pause();
		enterBACriticalSection();
	}
#endif
	return num;
}
예제 #26
0
int SortAndRemoveDoubles(double * P, int & nP)
{
  if (nP<2)
    return 0; // nothing to do
  // center of gravity for the points
  double c[2] = {0., 0.};
  int k=0;
  for (k=0; k<nP; k++)
    {
      c[0]+=P[2*k];
      c[1]+=P[2*k+1];
    }
  c[0]/=nP;
  c[1]/=nP;
  double angle[12]; // could be at most 12 points; much less usually
  for (k=0; k<nP; k++)
    {
      double x = P[2*k] - c[0], y = P[2*k+1] - c[1] ;
      if ( x!= 0. || y!=0.)
        angle[k] = atan2(y, x);
      else
	{
	  angle[k] = 0;
	  // this means that the points are on a line, or all coincident // degenerate case
	}
    } 
  // sort according to angle; also eliminate close points
  int sorted = 1;
  do
    {
      sorted = 1;
      for(k=0; k<nP-1; k++)
	{
	  if (angle[k]>angle[k+1])
	    {
	      sorted = 0;
	      swap ( angle+k, angle+k+1);
	      swap ( P+(2*k), P+(2*k+2));
	      swap ( P+(2*k+1), P+(2*k+3));
	    }
	}
    }
  while (!sorted);
  // eliminate doubles

  int i=0, j=1; // the next one; j may advance faster than i
  // check the unit 
  // double epsilon = 1.e-5; // these are cm; 2 points are the same if the distance is less than 1.e-5 cm
  while (j<nP)
    {
      double d2 = dist2 ( &P[2*i], &P[2*j]);
      if (d2 > epsilon)
	{
	  i++;  
	  P[2*i] = P[2*j];
	  P[2*i+1] = P[2*j+1]; 
	}
      j++;
    }
  // test also the last point with the first one (index 0)
  
  double d2 = dist2(P, &P[2*i]); // check the first and last points (ordered from -pi to +pi)
  if (d2 > epsilon)
    {
      nP = i+1;
    }
  else
    nP = i; // effectively delete the last point (that would have been the same with first)
  if (nP==0)
    nP=1; // we should be left with at least one point we already tested if nP is 0 originally
  return 0;
}
예제 #27
0
void _grid2_thread(int *num_threads, int *cur_thread, Array<complex<T> > &data, Array<T> &coords, Array<T> &weight, Array<complex<T> > &out, Array<T> &kernel_table, T dx, T dy)
{
    int imin, imax, jmin, jmax, i, j;
    int width = out.dimensions(0); // assume isotropic dims
    int width_div2 = width / 2;
    uint64_t arms = 1;
    uint64_t points_in_arm = 1;
    uint64_t arms_times_coils = 1;
    uint64_t coils = 1;
    uint64_t coil, arm, point;
    uint64_t width_times_coil = width;
    T x, y, ix, jy;
    T kernelRadius = DEFAULT_RADIUS_FOV_PRODUCT / width;
    T kernelRadius_sqr = kernelRadius * kernelRadius;
    T width_inv = 1.0 / width;
    
    T dist_multiplier = (kernel_table.dimensions(0) - 1)/kernelRadius_sqr;
    
    out = complex<T>(0.0);
    
    if (coords.ndim() == 3)
    {
        points_in_arm = coords.dimensions(1);
        arms = coords.dimensions(2);
    }
    
    if (out.ndim() == 3)
    {
        coils = out.dimensions(2);
    }
    arms_times_coils = arms * coils;
    width_times_coil = width * coils;
    
    /* split threads up by even chunks of data_out memory */
    uint64_t numSections = coils / *num_threads;
    uint64_t coil_start = *cur_thread * numSections;
    uint64_t coil_end = (*cur_thread+1) * numSections;
    
    /* loop over output data points */
    for (coil=coil_start; coil<coil_end; coil++)
    {
        for (arm=0; arm<arms; arm++)
        {
            for (point=0; point<points_in_arm; point++)
            {
                complex<T> d = data(point,arm,coil) * weight(point,arm);
                
                /* get the coordinates of the datapoint to grid
                 *  these vary between -.5 -- +.5               */
                x = coords(0,point,arm);
                y = coords(1,point,arm);
                
                /* add shift phase */
                d *= exp( complex<T>(0, -2.*M_PI*(x*dx+y*dy)) );
                
                /* set the boundaries of final dataset for gridding this point */
                ix = x * width + width_div2;
                set_minmax(ix, &imin, &imax, width, (T) DEFAULT_RADIUS_FOV_PRODUCT);
                jy = y * width + width_div2;
                set_minmax(jy, &jmin, &jmax, width, (T) DEFAULT_RADIUS_FOV_PRODUCT);
                
                /* grid this point onto the neighboring cartesian points */
                for (j=jmin; j<=jmax; ++j)
                {
                    jy = (j - width_div2) * width_inv;
                    for (i=imin; i<=imax; ++i)
                    {
                        ix = (i - width_div2) * width_inv;
                        T dist_sqr = dist2(ix - x, jy - y);
                        if (dist_sqr < kernelRadius_sqr)
                        {
                            T ker = get1(kernel_table, (int) rint(dist_sqr * dist_multiplier));
                            out(i,j,coil) += ker * d;
                        }
                    }// x
                }// y
            }//point in arms
        } //arms
    } //coil
}
예제 #28
0
/* return TRUE if the caller needs to place the ball and chain down again
 *
 *  Should not be called while swallowed.  Should be called before movement,
 *  because we might want to move the ball or chain to the hero's old position.
 *
 * It is called if we are moving.  It is also called if we are teleporting
 * *if* the ball doesn't move and we thus must drag the chain.  It is not
 * called for ordinary teleportation.
 *
 * allow_drag is only used in the ugly special case where teleporting must
 * drag the chain, while an identical-looking movement must drag both the ball
 * and chain.
 */
boolean
drag_ball(xchar x, xchar y, int *bc_control, xchar * ballx, xchar * bally,
          xchar * chainx, xchar * chainy, boolean * cause_delay,
          boolean allow_drag)
{
    struct trap *t = NULL;
    boolean already_in_rock;

    *ballx = uball->ox;
    *bally = uball->oy;
    *chainx = uchain->ox;
    *chainy = uchain->oy;
    *bc_control = 0;
    *cause_delay = FALSE;

    if (dist2(x, y, uchain->ox, uchain->oy) <= 2) {     /* nothing moved */
        move_bc(1, *bc_control, *ballx, *bally, *chainx, *chainy);
        return TRUE;
    }

    /* only need to move the chain? */
    if (carried(uball) || distmin(x, y, uball->ox, uball->oy) <= 2) {
        xchar oldchainx = uchain->ox, oldchainy = uchain->oy;

        *bc_control = BC_CHAIN;
        move_bc(1, *bc_control, *ballx, *bally, *chainx, *chainy);
        if (carried(uball)) {
            /* move chain only if necessary */
            if (distmin(x, y, uchain->ox, uchain->oy) > 1) {
                *chainx = u.ux;
                *chainy = u.uy;
            }
            return TRUE;
        }
#define CHAIN_IN_MIDDLE(chx, chy) \
    (distmin(x, y, chx, chy) <= 1 && \
     distmin(chx, chy, uball->ox, uball->oy) <= 1)
#define IS_CHAIN_ROCK(x,y) \
    (IS_ROCK(level->locations[x][y].typ) || \
     (IS_DOOR(level->locations[x][y].typ) && \
      (level->locations[x][y].doormask & (D_CLOSED|D_LOCKED))))
/* Don't ever move the chain into solid rock.  If we have to, then instead
 * undo the move_bc() and jump to the drag ball code.  Note that this also
 * means the "cannot carry and drag" message will not appear, since unless we
 * moved at least two squares there is no possibility of the chain position
 * being in solid rock.
 */
#define SKIP_TO_DRAG { *chainx = oldchainx; *chainy = oldchainy; \
    move_bc(0, *bc_control, *ballx, *bally, *chainx, *chainy); \
    goto drag; }
        if (IS_CHAIN_ROCK(u.ux, u.uy) || IS_CHAIN_ROCK(*chainx, *chainy)
            || IS_CHAIN_ROCK(uball->ox, uball->oy))
            already_in_rock = TRUE;
        else
            already_in_rock = FALSE;

        switch (dist2(x, y, uball->ox, uball->oy)) {
            /* two spaces diagonal from ball, move chain inbetween */
        case 8:
            *chainx = (uball->ox + x) / 2;
            *chainy = (uball->oy + y) / 2;
            if (IS_CHAIN_ROCK(*chainx, *chainy) && !already_in_rock)
                SKIP_TO_DRAG;
            break;

            /* player is distance 2/1 from ball; move chain to one of the two
               spaces between @ __ 0 */
        case 5:{
                xchar tempx, tempy, tempx2, tempy2;

                /* find position closest to current position of chain */
                /* no effect if current position is already OK */
                if (abs(x - uball->ox) == 1) {
                    tempx = x;
                    tempx2 = uball->ox;
                    tempy = tempy2 = (uball->oy + y) / 2;
                } else {
                    tempx = tempx2 = (uball->ox + x) / 2;
                    tempy = y;
                    tempy2 = uball->oy;
                }
                if (IS_CHAIN_ROCK(tempx, tempy) &&
                    !IS_CHAIN_ROCK(tempx2, tempy2) && !already_in_rock) {
                    if (allow_drag) {
                        /* Avoid pathological case *if* not teleporting: 0 0_
                           _X move northeast -----> X@ @ */
                        if (dist2(u.ux, u.uy, uball->ox, uball->oy) == 5 &&
                            dist2(x, y, tempx, tempy) == 1)
                            SKIP_TO_DRAG;
                        /* Avoid pathological case *if* not teleporting: 0 0 _X 
                           move east -----> X_ @ @ */
                        if (dist2(u.ux, u.uy, uball->ox, uball->oy) == 4 &&
                            dist2(x, y, tempx, tempy) == 2)
                            SKIP_TO_DRAG;
                    }
                    *chainx = tempx2;
                    *chainy = tempy2;
                } else if (!IS_CHAIN_ROCK(tempx, tempy) &&
                           IS_CHAIN_ROCK(tempx2, tempy2) && !already_in_rock) {
                    if (allow_drag) {
                        if (dist2(u.ux, u.uy, uball->ox, uball->oy) == 5 &&
                            dist2(x, y, tempx2, tempy2) == 1)
                            SKIP_TO_DRAG;
                        if (dist2(u.ux, u.uy, uball->ox, uball->oy) == 4 &&
                            dist2(x, y, tempx2, tempy2) == 2)
                            SKIP_TO_DRAG;
                    }
                    *chainx = tempx;
                    *chainy = tempy;
                } else if (IS_CHAIN_ROCK(tempx, tempy) &&
                           IS_CHAIN_ROCK(tempx2, tempy2) && !already_in_rock) {
                    SKIP_TO_DRAG;
                } else if (dist2(tempx, tempy, uchain->ox, uchain->oy) <
                           dist2(tempx2, tempy2, uchain->ox, uchain->oy) ||
                           ((dist2(tempx, tempy, uchain->ox, uchain->oy) ==
                             dist2(tempx2, tempy2, uchain->ox, uchain->oy)) &&
                            rn2(2))) {
                    *chainx = tempx;
                    *chainy = tempy;
                } else {
                    *chainx = tempx2;
                    *chainy = tempy2;
                }
                break;
            }

            /* ball is two spaces horizontal or vertical from player; move */
            /* chain inbetween *unless* current chain position is OK */
        case 4:
            if (CHAIN_IN_MIDDLE(uchain->ox, uchain->oy))
                break;
            *chainx = (x + uball->ox) / 2;
            *chainy = (y + uball->oy) / 2;
            if (IS_CHAIN_ROCK(*chainx, *chainy) && !already_in_rock)
                SKIP_TO_DRAG;
            break;

            /* ball is one space diagonal from player.  Check for the following 
               special case: @ _ moving southwest becomes @_ 0 0 (This will
               also catch teleporting that happens to resemble this case, but
               oh well.) Otherwise fall through. */
        case 2:
            if (dist2(x, y, uball->ox, uball->oy) == 2 &&
                dist2(x, y, uchain->ox, uchain->oy) == 4) {
                if (uchain->oy == y)
                    *chainx = uball->ox;
                else
                    *chainy = uball->oy;
                if (IS_CHAIN_ROCK(*chainx, *chainy) && !already_in_rock)
                    SKIP_TO_DRAG;
                break;
            }
            /* fall through */
        case 1:
        case 0:
            /* do nothing if possible */
            if (CHAIN_IN_MIDDLE(uchain->ox, uchain->oy))
                break;
            /* otherwise try to drag chain to player's old position */
            if (CHAIN_IN_MIDDLE(u.ux, u.uy)) {
                *chainx = u.ux;
                *chainy = u.uy;
                break;
            }
            /* otherwise use player's new position (they must have teleported,
               for this to happen) */
            *chainx = x;
            *chainy = y;
            break;

        default:
            impossible("bad chain movement");
            break;
        }
#undef SKIP_TO_DRAG
#undef IS_CHAIN_ROCK
#undef CHAIN_IN_MIDDLE
        return TRUE;
    }

drag:

    if (near_capacity() > SLT_ENCUMBER && dist2(x, y, u.ux, u.uy) <= 2) {
        pline("You cannot %sdrag the heavy iron ball.",
              invent ? "carry all that and also " : "");
        action_completed();
        return FALSE;
    }

    if ((is_pool(level, uchain->ox, uchain->oy) &&
         /* water not mere continuation of previous water */
         (level->locations[uchain->ox][uchain->oy].typ == POOL ||
          !is_pool(level, uball->ox, uball->oy) ||
          level->locations[uball->ox][uball->oy].typ == POOL))
        || ((t = t_at(level, uchain->ox, uchain->oy)) &&
            (t->ttyp == PIT || t->ttyp == SPIKED_PIT || t->ttyp == HOLE ||
             t->ttyp == TRAPDOOR))) {

        if (Levitation) {
            pline("You feel a tug from the iron ball.");
            if (t)
                t->tseen = 1;
        } else {
            struct monst *victim;

            pline("You are jerked back by the iron ball!");
            if ((victim = m_at(level, uchain->ox, uchain->oy)) != 0) {
                int tmp;

                tmp = -2 + Luck + find_mac(victim);
                tmp += omon_adj(victim, uball, TRUE);
                if (tmp >= rnd(20))
                    hmon(victim, uball, 1);
                else
                    miss(xname(uball), victim);

            }   /* now check again in case mon died */
            if (!m_at(level, uchain->ox, uchain->oy)) {
                u.ux = uchain->ox;
                u.uy = uchain->oy;
                newsym(u.ux0, u.uy0);
            }
            action_interrupted();

            *bc_control = BC_BALL;
            move_bc(1, *bc_control, *ballx, *bally, *chainx, *chainy);
            *ballx = uchain->ox;
            *bally = uchain->oy;
            move_bc(0, *bc_control, *ballx, *bally, *chainx, *chainy);
            spoteffects(TRUE);
            return FALSE;
        }
    }

    *bc_control = BC_BALL | BC_CHAIN;

    move_bc(1, *bc_control, *ballx, *bally, *chainx, *chainy);
    if (dist2(x, y, u.ux, u.uy) > 2) {
        /* Awful case: we're still in range of the ball, so we thought we could 
           only move the chain, but it turned out that the target square for
           the chain was rock, so we had to drag it instead. But we can't drag
           it either, because we teleported and are more than one square from
           our old position.  Revert to the teleport behavior. */
        *ballx = *chainx = x;
        *bally = *chainy = y;
    } else {
        *ballx = uchain->ox;
        *bally = uchain->oy;
        *chainx = u.ux;
        *chainy = u.uy;
    }
    *cause_delay = TRUE;
    return TRUE;
}
예제 #29
0
void  ne_sw_combine
(
  long    left,
  long    mid,
  long    right,
  Point*  pt,
  long*   sorted,
  long*   aux,
  long    oct,
  nn_array*  nn
)
{
  long   i, j, k, y2; 
  long   i1;
  long   i2; 
  long   best_i2;     /* index of current best nearest-neighbor */
  long   best_dist;   /* distance to best nearest-neighbor      */
  long   d;

#ifdef DEBUG
  assert( right > mid );
  assert( mid > left );
#endif

  /*
    update north-east nearest neighbors accross the mid-line
  */

  i1 = left;
  i2 = mid;   y2 = pt[ sorted[i2] ].y;

  while( (i1 < mid) && (pt[ sorted[i1] ].y >= y2) )
  {
    i1++;
  }
  
  if( i1 < mid )
  {
    best_i2   = i2;
    best_dist = dist2( pt + sorted[i1], pt + sorted[best_i2] );
    i2++;

    while( (i1 < mid) && (i2 < right) )
    {
      if( pt[ sorted[i1] ].y < pt[ sorted[i2] ].y )
      {
        d = dist2( pt + sorted[i1], pt + sorted[i2] );
        if( d < best_dist ) 
        {
          best_i2   = i2;
          best_dist = d;
        }
        i2++;
      }
      else 
      {
        if( (nn[ sorted[i1] ][oct] == -1) || 
            ( best_dist < dist2( pt + sorted[i1], pt + nn[ sorted[i1] ][oct]) ) 
           )
        {
          nn[ sorted[i1] ][oct] = sorted[best_i2];
        }
        i1++;
        if( i1 < mid )
        {
          best_dist = dist2( pt + sorted[i1], pt + sorted[best_i2] );
        }
      }    
    }

    while( i1 < mid )
    {
      if( (nn[ sorted[i1] ][oct] == -1) || 
          ( dist2( pt + sorted[i1], pt + sorted[best_i2] ) < 
            dist2( pt + sorted[i1], pt + nn[ sorted[i1] ][oct]) ) 
        )
      {
        nn[ sorted[i1] ][oct] = sorted[best_i2];
      }
      i1++;
    }
  }
  /*
    repeat for south-west nearest neighbors
  */

  oct = (oct + 4) % 8;

  i1 = right - 1;
  i2 = mid - 1;   y2 = pt[ sorted[i2] ].y;
     
  while( (i1 >= mid) && (pt[ sorted[i1] ].y <= y2) )
  {
    i1--;
  }

  if( i1 >= mid )
  {
    best_i2   = i2;
    best_dist = dist2( pt + sorted[i1], pt + sorted[best_i2] );
    i2--;

    while( (i1 >= mid) && (i2 >= left) )
    {
      if( pt[ sorted[i1] ].y > pt[ sorted[i2] ].y )
      {
        d = dist2( pt + sorted[i1], pt + sorted[i2] );
        if( d < best_dist ) 
        {
          best_i2   = i2;   
          best_dist = d;
        }
        i2--;
      }
      else 
      {
        if( (nn[ sorted[i1] ][oct] == -1) || 
            ( best_dist < dist2( pt + sorted[i1], pt + nn[ sorted[i1] ][oct]) ) 
           )
        {
          nn[ sorted[i1] ][oct] = sorted[best_i2];
        }
        i1--;
        if( i1 >= mid )
        {
          best_dist = dist2( pt + sorted[i1], pt + sorted[best_i2] );
        }
      }    
    }

    while( i1 >= mid )
    {
      if( (nn[ sorted[i1] ][oct] == -1) || 
          ( dist2( pt + sorted[i1], pt + sorted[best_i2] ) < 
            dist2( pt + sorted[i1], pt + nn[ sorted[i1] ][oct]) ) 
        )
      {
        nn[ sorted[i1] ][oct] = sorted[best_i2];
      }
      i1--;
    }
  }

  /*
    merge sorted[left..mid-1] with sorted[mid..right-1] by y-coordinate
  */

  i = left;  /* first unprocessed element in left  list  */
  j = mid;   /* first unprocessed element in right list  */
  k = left;  /* first free available slot in output list */

  while( (i < mid) && (j < right) )
  {
    if( pt[ sorted[i] ].y >= pt[ sorted[j] ].y )
    {
      aux[k++] = sorted[i++]; 
    }
    else 
    {
      aux[k++] = sorted[j++]; 
    }
  }

  /*
    copy leftovers 
  */
  while( i < mid   ) {  aux[k++] = sorted[i++]; }
  while( j < right ) {  aux[k++] = sorted[j++]; }

  /*
    now copy sorted points from 'aux' to 'sorted' 
  */

  for( i = left;  i < right;  i++ )  { sorted[i] = aux[i]; }

#if 0
  memcpy( (void*)(sorted+left),             /* destination */
          (void*)(aux+left),             /* source      */
          (size_t)(right-left)*sizeof(long) /* number of bytes */ 
        );
#endif

}
void TrigonometricPathVessel::finish( const std::vector<double>& buffer ) {
  // Store the data calculated during mpi loop
  StoreDataVessel::finish( buffer );
  // Get current value of all arguments
  for(unsigned i=0; i<cargs.size(); ++i) cargs[i]=mymap->getArgument(i);

  // Determine closest and second closest point to current position
  double lambda=mymap->getLambda();
  std::vector<double> dist( getNumberOfComponents() ), dist2( getNumberOfComponents() );;
  retrieveSequentialValue( 0, false, dist );
  retrieveSequentialValue( 1, false, dist2 );
  iclose1=getStoreIndex(0); iclose2=getStoreIndex(1);
  double mindist1=dist[0], mindist2=dist2[0];
  if( lambda>0.0 ) {
    mindist1=-std::log( dist[0] ) / lambda;
    mindist2=-std::log( dist2[0] ) / lambda;
  }
  if( mindist2<mindist1 ) {
    double tmp=mindist1; mindist1=mindist2; mindist2=tmp;
    iclose1=getStoreIndex(1); iclose2=getStoreIndex(0);
  }
  for(unsigned i=2; i<getNumberOfStoredValues(); ++i) {
    retrieveSequentialValue( i, false, dist );
    double ndist=dist[0];
    if( lambda>0.0 ) ndist=-std::log( dist[0] ) / lambda;
    if( ndist<mindist1 ) {
      mindist2=mindist1; iclose2=iclose1;
      mindist1=ndist; iclose1=getStoreIndex(i);
    } else if( ndist<mindist2 ) {
      mindist2=ndist; iclose2=getStoreIndex(i);
    }
  }
  // And find third closest point
  int isign = iclose1 - iclose2;
  if( isign>1 ) isign=1; else if( isign<-1 ) isign=-1;
  int iclose3 = iclose1 + isign; double v2v2;
  // We now have to compute vectors connecting the three closest points to the
  // new point
  double v1v1 = (mymap->getReferenceConfiguration( iclose1 ))->calculate( mymap->getPositions(), mymap->getPbc(), mymap->getArguments(), mypack1, true );
  double v3v3 = (mymap->getReferenceConfiguration( iclose2 ))->calculate( mymap->getPositions(), mymap->getPbc(), mymap->getArguments(), mypack3, true );
  if( iclose3<0 || iclose3>=mymap->getFullNumberOfTasks() ) {
    ReferenceConfiguration* conf2=mymap->getReferenceConfiguration( iclose1 );
    v2v2=(mymap->getReferenceConfiguration( iclose2 ))->calc( conf2->getReferencePositions(), mymap->getPbc(), mymap->getArguments(),
         conf2->getReferenceArguments(), mypack2, true );
    (mymap->getReferenceConfiguration( iclose2 ))->extractDisplacementVector( conf2->getReferencePositions(), mymap->getArguments(),
        conf2->getReferenceArguments(), false, projdir );
  } else {
    ReferenceConfiguration* conf2=mymap->getReferenceConfiguration( iclose3 );
    v2v2=(mymap->getReferenceConfiguration( iclose1 ))->calc( conf2->getReferencePositions(), mymap->getPbc(), mymap->getArguments(),
         conf2->getReferenceArguments(), mypack2, true );
    (mymap->getReferenceConfiguration( iclose1 ))->extractDisplacementVector( conf2->getReferencePositions(), mymap->getArguments(),
        conf2->getReferenceArguments(), false, projdir );
  }

  // Stash derivatives of v1v1
  for(unsigned i=0; i<mymap->getNumberOfArguments(); ++i) mypack1_stashd_args[i]=mypack1.getArgumentDerivative(i);
  if( mymap->getNumberOfAtoms()>0 ) {
    ReferenceAtoms* at = dynamic_cast<ReferenceAtoms*>( mymap->getReferenceConfiguration( iclose1 ) );
    const std::vector<double> & displace( at->getDisplace() );
    for(unsigned i=0; i<mymap->getNumberOfAtoms(); ++i) {
      mypack1_stashd_atoms[i]=mypack1.getAtomDerivative(i); mypack1.getAtomsDisplacementVector()[i] /= displace[i];
    }
  }
  // Calculate the dot product of v1 with v2
  double v1v2 = (mymap->getReferenceConfiguration(iclose1))->projectDisplacementOnVector( projdir, mymap->getArguments(), cargs, mypack1 );

  // This computes s value
  double spacing = mymap->getPropertyValue( iclose1, (mymap->property.begin())->first ) - mymap->getPropertyValue( iclose2, (mymap->property.begin())->first );
  double root = sqrt( v1v2*v1v2 - v2v2 * ( v1v1 - v3v3) );
  dx = 0.5 * ( (root + v1v2) / v2v2 - 1.);
  double path_s = mymap->getPropertyValue(iclose1, (mymap->property.begin())->first ) + spacing * dx; sp->set( path_s );
  double fact = 0.25*spacing / v2v2;
  // Derivative of s wrt arguments
  for(unsigned i=0; i<mymap->getNumberOfArguments(); ++i) {
    sp->setDerivative( i, fact*( mypack2.getArgumentDerivative(i) + (v2v2 * (-mypack1_stashd_args[i] + mypack3.getArgumentDerivative(i))
                                 + v1v2*mypack2.getArgumentDerivative(i) )/root ) );
  }
  // Derivative of s wrt atoms
  unsigned narg=mymap->getNumberOfArguments(); Tensor vir; vir.zero(); fact = 0.5*spacing / v2v2;
  if( mymap->getNumberOfAtoms()>0 ) {
    for(unsigned i=0; i<mymap->getNumberOfAtoms(); ++i) {
      Vector ader = fact*(( v1v2*mypack1.getAtomDerivative(i) + 0.5*v2v2*(-mypack1_stashd_atoms[i] + mypack3.getAtomDerivative(i) ) )/root + mypack1.getAtomDerivative(i) );
      for(unsigned k=0; k<3; ++k) sp->setDerivative( narg+3*i+k, ader[k] );
      vir-=Tensor( mymap->getPosition(i), ader );
    }
    // Set the virial
    unsigned nbase=narg+3*mymap->getNumberOfAtoms();
    for(unsigned i=0; i<3; ++i) for(unsigned j=0; j<3; ++j) sp->setDerivative( nbase+3*i+j, vir(i,j) );
  }
  // Now compute z value
  ReferenceConfiguration* conf2=mymap->getReferenceConfiguration( iclose1 );
  double v4v4=(mymap->getReferenceConfiguration( iclose2 ))->calc( conf2->getReferencePositions(), mymap->getPbc(), mymap->getArguments(),
              conf2->getReferenceArguments(), mypack2, true );
  // Extract vector connecting frames
  (mymap->getReferenceConfiguration( iclose2 ))->extractDisplacementVector( conf2->getReferencePositions(), mymap->getArguments(),
      conf2->getReferenceArguments(), false, projdir );
  // Calculate projection of vector on line connnecting frames
  double proj = (mymap->getReferenceConfiguration(iclose1))->projectDisplacementOnVector( projdir, mymap->getArguments(), cargs, mypack1 );
  double path_z = v1v1 + dx*dx*v4v4 - 2*dx*proj;
  // Derivatives for z path
  path_z = sqrt(path_z); zp->set( path_z ); vir.zero();
  for(unsigned i=0; i<mymap->getNumberOfArguments(); ++i) zp->setDerivative( i, (mypack1_stashd_args[i] - 2*dx*mypack1.getArgumentDerivative(i))/(2.0*path_z) );
  // Derivative wrt atoms
  if( mymap->getNumberOfAtoms()>0 ) {
    for(unsigned i=0; i<mymap->getNumberOfAtoms(); ++i) {
      Vector dxder; for(unsigned k=0; k<3; ++k) dxder[k] = ( 2*v4v4*dx - 2*proj )*spacing*sp->getDerivative( narg + 3*i+k );
      Vector ader = ( mypack1_stashd_atoms[i] - 2.*dx*mypack1.getAtomDerivative(i) + dxder )/ (2.0*path_z);
      for(unsigned k=0; k<3; ++k) zp->setDerivative( narg+3*i+k, ader[k] );
      vir-=Tensor( mymap->getPosition(i), ader );
    }
    // Set the virial
    unsigned nbase=narg+3*mymap->getNumberOfAtoms();
    for(unsigned i=0; i<3; ++i) for(unsigned j=0; j<3; ++j) zp->setDerivative( nbase+3*i+j, vir(i,j) );
  }
}