/* 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();
}
// 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;
}
/*****************************************************************************
*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 */
// 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);
}
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;
}
// 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;
}
// 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;
}
// 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);
}
double dist() const { return sqrt((double)dist2()); }
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;
}
}
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;
}
/*
* 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 = >able[la * ainc + lb * binc];
// fill table with x
fillgtable(>able[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;
}
}
/*
* 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;
}
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;
}
/**test two vectors for equality with given treshold*/
bool eqals( const vec2& v, float eps )const{
return dist2( v ) <= sqr( eps );
};
P inv() const{
return P(x/dist2(), y/dist2());
}
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()*/
inline T dist( const tgeVector3T<T>& aV1, const tgeVector3T<T>& aV2 )
{
return std::sqrt( dist2( aV1, aV2 ) );
}
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]);
}
}
}
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;
}
// 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)));
}
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;
}
/* dist:
* Returns the distance between points a and b.
*/
static COORD dist(Ppoint_t a, Ppoint_t b)
{
return sqrt(dist2(a, b));
}
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;
}
//#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;
}
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;
}
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
}
/* 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;
}
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) );
}
}