void mul(int a) { for (int i = 0; i < nlen; i++) { number[i] *= a; } norm(); }
Camera::Camera(GameHandle cameraHandle, Ogre::SceneNode* camNode, Ogre::RenderTarget* renderTarget, s32 width, s32 height) : mCameraNode(camNode), mRenderTarget(renderTarget), mHandle(cameraHandle), mNodeCreated(false), mRenderTargetCreated(false) { Ogre::SceneManager* sceneMgr; if (mCameraNode == NULL) // Create SceneNode { sceneMgr = Ogre::Root::getSingleton().getSceneManager(BFG_SCENEMANAGER); if (sceneMgr->hasSceneNode(stringify(mHandle))) { mCameraNode = sceneMgr->getSceneNode(stringify(mHandle)); } else { mCameraNode = sceneMgr->getRootSceneNode()->createChildSceneNode(stringify(mHandle)); mNodeCreated = true; } } else { sceneMgr = mCameraNode->getCreator(); } mCameraNode->resetOrientation(); mCameraNode->setPosition(toOgre(v3::ZERO)); v3 target = toBFG(mCameraNode->getOrientation().zAxis()); norm(target); Ogre::Camera* cam; cam = sceneMgr->createCamera(stringify(mHandle)); cam->setFOVy(Ogre::Degree(60.0f)); cam->setNearClipDistance(0.1f); cam->setFarClipDistance(250000.0f); cam->lookAt(toOgre(target) * 10); mCameraNode->attachObject(cam); infolog << "Camera: " << stringify(mHandle) << " created."; if (mRenderTarget == NULL) { // Create renderToTexture RenderTarget if (width == 0 || height == 0) { throw std::logic_error("Too few information to create a render target."); } cam->setAspectRatio((f32)width / (f32)height); Ogre::TexturePtr texture = Ogre::TextureManager::getSingleton().createManual ( stringify(mHandle), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, width, height, 0, Ogre::PF_R8G8B8, Ogre::TU_RENDERTARGET ); mRenderTarget = texture->getBuffer()->getRenderTarget(); prepareRenderTarget(); mRenderTarget->addViewport(cam); mRenderTarget->getViewport(0)->setClearEveryFrame(true); mRenderTarget->getViewport(0)->setBackgroundColour(Ogre::ColourValue::Black); mRenderTarget->getViewport(0)->setOverlaysEnabled(false); Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().getByName(stringify(mHandle)); if (mat.isNull()) { mat = Ogre::MaterialManager::getSingleton().create( stringify(mHandle), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); } Ogre::Technique* tech = mat->getTechnique(0); if (!tech) { tech = mat->createTechnique(); } Ogre::Pass* pass = tech->getPass(0); if (!pass) { pass = tech->createPass(); } pass->setLightingEnabled(false); Ogre::TextureUnitState* txState = NULL; if (pass->getNumTextureUnitStates() > 0) { txState = pass->getTextureUnitState(0); txState->setTextureName(stringify(mHandle)); } else { pass->createTextureUnitState(stringify(mHandle)); } mRenderTarget->setAutoUpdated(true); mRenderTargetCreated = true; infolog << "Rendertarget: " << stringify(mHandle) << " created."; } else { prepareRenderTarget(); f32 width = mRenderTarget->getWidth(); f32 height = mRenderTarget->getHeight(); cam->setAspectRatio(width / height); mRenderTarget->addViewport(cam); } Main::eventLoop()->connect ( ID::VE_UPDATE_POSITION, this, &Camera::viewEventHandler, mHandle ); Main::eventLoop()->connect ( ID::VE_UPDATE_ORIENTATION, this, &Camera::viewEventHandler, mHandle ); }
Vector Vector :: unit( void ) const { return (*this) / norm(); }
void Angle::analyzeFrame(int frnr, const t_trxframe &fr, t_pbc *pbc, TrajectoryAnalysisModuleData *pdata) { AnalysisDataHandle *dh = pdata->dataHandle("angle"); std::vector<Selection *> sel1 = pdata->parallelSelections(_sel1); std::vector<Selection *> sel2 = pdata->parallelSelections(_sel2); checkSelections(sel1, sel2); rvec v1, v2; rvec c1, c2; switch (_g2type[0]) { case 'z': clear_rvec(v2); v2[ZZ] = 1.0; clear_rvec(c2); break; case 's': copy_rvec(_sel2[0]->x(0), c2); break; } dh->startFrame(frnr, fr.time); int incr1 = _bSplit1 ? 1 : _natoms1; int incr2 = _bSplit2 ? 1 : _natoms2; int ngrps = _bMulti ? _sel1.size() : 1; for (int g = 0; g < ngrps; ++g) { real ave = 0.0; int n = 0; int i, j; for (i = j = 0; i < sel1[g]->posCount(); i += incr1) { rvec x[4]; real angle; copy_pos(sel1, _bSplit1, _natoms1, g, i, x); switch (_g1type[0]) { case 'a': if (pbc) { pbc_dx(pbc, x[0], x[1], v1); pbc_dx(pbc, x[2], x[1], v2); } else { rvec_sub(x[0], x[1], v1); rvec_sub(x[2], x[1], v2); } angle = gmx_angle(v1, v2); break; case 'd': { rvec dx[3]; if (pbc) { pbc_dx(pbc, x[0], x[1], dx[0]); pbc_dx(pbc, x[2], x[1], dx[1]); pbc_dx(pbc, x[2], x[3], dx[2]); } else { rvec_sub(x[0], x[1], dx[0]); rvec_sub(x[2], x[1], dx[1]); rvec_sub(x[2], x[3], dx[2]); } cprod(dx[0], dx[1], v1); cprod(dx[1], dx[2], v2); angle = gmx_angle(v1, v2); real ipr = iprod(dx[0], v2); if (ipr < 0) { angle = -angle; } break; } case 'v': case 'p': calc_vec(_natoms1, x, pbc, v1, c1); switch (_g2type[0]) { case 'v': case 'p': copy_pos(sel2, _bSplit2, _natoms2, 0, j, x); calc_vec(_natoms2, x, pbc, v2, c2); j += incr2; break; case 't': // FIXME: This is not parallelizable. if (frnr == 0) { copy_rvec(v1, _vt0[n]); } copy_rvec(_vt0[n], v2); break; case 'z': c1[XX] = c1[YY] = 0.0; break; case 's': if (pbc) { pbc_dx(pbc, c1, c2, v2); } else { rvec_sub(c1, c2, v2); } break; default: GMX_THROW(InternalError("invalid -g2 value")); } angle = gmx_angle(v1, v2); break; default: GMX_THROW(InternalError("invalid -g1 value")); } angle *= RAD2DEG; real dist = 0.0; if (_bDumpDist) { if (pbc) { rvec dx; pbc_dx(pbc, c2, c1, dx); dist = norm(dx); } else { dist = sqrt(distance2(c1, c2)); } } if (_bAll) { dh->addPoint(n + 1, angle); } ave += angle; ++n; } if (n > 0) { ave /= n; } dh->addPoint(g, ave); } dh->finishFrame(); }
vec proj(const vec& base, const vec& v) { if (isZero(base)) return base; else return ((eucInnerProd(base, v)/pow(norm(base),2)) * base); }
Quaternion& Quaternion::normalize() { float n = norm(); return (*this)/=n; }
void normalize() { double vecNorm = norm(); x/=vecNorm; y/=vecNorm;};
void do_coupling(FILE *log,int nfile,t_filenm fnm[], t_coupl_rec *tcr,real t,int step,real ener[], t_forcerec *fr,t_inputrec *ir,bool bMaster, t_mdatoms *md,t_idef *idef,real mu_aver,int nmols, t_commrec *cr,matrix box,tensor virial, tensor pres,rvec mu_tot, rvec x[],rvec f[],bool bDoIt) { #define enm2Debye 48.0321 #define d2e(x) (x)/enm2Debye #define enm2kjmol(x) (x)*0.0143952 /* = 2.0*4.0*M_PI*EPSILON0 */ static real *f6,*f12,*fa,*fb,*fc,*fq; static bool bFirst = TRUE; int i,j,ati,atj,atnr2,type,ftype; real deviation[eoObsNR],prdev[eoObsNR],epot0,dist,rmsf; real ff6,ff12,ffa,ffb,ffc,ffq,factor,dt,mu_ind; real Epol,Eintern,Virial,muabs,xiH=-1,xiS=-1,xi6,xi12; rvec fmol[2]; bool bTest,bPrint; t_coupl_LJ *tclj; t_coupl_BU *tcbu; t_coupl_Q *tcq; t_coupl_iparams *tip; atnr2 = idef->atnr * idef->atnr; if (bFirst) { if (PAR(cr)) fprintf(log,"GCT: this is parallel\n"); else fprintf(log,"GCT: this is not parallel\n"); fflush(log); snew(f6, atnr2); snew(f12,atnr2); snew(fa, atnr2); snew(fb, atnr2); snew(fc, atnr2); snew(fq, idef->atnr); if (tcr->bVirial) { int nrdf = 0; real TTT = 0; real Vol = det(box); for(i=0; (i<ir->opts.ngtc); i++) { nrdf += ir->opts.nrdf[i]; TTT += ir->opts.nrdf[i]*ir->opts.ref_t[i]; } TTT /= nrdf; /* Calculate reference virial from reference temperature and pressure */ tcr->ref_value[eoVir] = 0.5*BOLTZ*nrdf*TTT - (3.0/2.0)* Vol*tcr->ref_value[eoPres]; fprintf(log,"GCT: TTT = %g, nrdf = %d, vir0 = %g, Vol = %g\n", TTT,nrdf,tcr->ref_value[eoVir],Vol); fflush(log); } bFirst = FALSE; } bPrint = MASTER(cr) && do_per_step(step,ir->nstlog); dt = ir->delta_t; /* Initiate coupling to the reference pressure and temperature to start * coupling slowly. */ if (step == 0) { for(i=0; (i<eoObsNR); i++) tcr->av_value[i] = tcr->ref_value[i]; if ((tcr->ref_value[eoDipole]) != 0.0) { mu_ind = mu_aver - d2e(tcr->ref_value[eoDipole]); /* in e nm */ Epol = mu_ind*mu_ind/(enm2kjmol(tcr->ref_value[eoPolarizability])); tcr->av_value[eoEpot] -= Epol; fprintf(log,"GCT: mu_aver = %g(D), mu_ind = %g(D), Epol = %g (kJ/mol)\n", mu_aver*enm2Debye,mu_ind*enm2Debye,Epol); } } /* We want to optimize the LJ params, usually to the Vaporization energy * therefore we only count intermolecular degrees of freedom. * Note that this is now optional. switch UseEinter to yes in your gct file * if you want this. */ dist = calc_dist(log,x); muabs = norm(mu_tot); Eintern = Ecouple(tcr,ener)/nmols; Virial = virial[XX][XX]+virial[YY][YY]+virial[ZZ][ZZ]; /*calc_force(md->nr,f,fmol);*/ clear_rvec(fmol[0]); /* Use a memory of tcr->nmemory steps, so we actually couple to the * average observable over the last tcr->nmemory steps. This may help * in avoiding local minima in parameter space. */ set_act_value(tcr,eoPres, ener[F_PRES],step); set_act_value(tcr,eoEpot, Eintern, step); set_act_value(tcr,eoVir, Virial, step); set_act_value(tcr,eoDist, dist, step); set_act_value(tcr,eoMu, muabs, step); set_act_value(tcr,eoFx, fmol[0][XX], step); set_act_value(tcr,eoFy, fmol[0][YY], step); set_act_value(tcr,eoFz, fmol[0][ZZ], step); set_act_value(tcr,eoPx, pres[XX][XX],step); set_act_value(tcr,eoPy, pres[YY][YY],step); set_act_value(tcr,eoPz, pres[ZZ][ZZ],step); epot0 = tcr->ref_value[eoEpot]; /* If dipole != 0.0 assume we want to use polarization corrected coupling */ if ((tcr->ref_value[eoDipole]) != 0.0) { mu_ind = mu_aver - d2e(tcr->ref_value[eoDipole]); /* in e nm */ Epol = mu_ind*mu_ind/(enm2kjmol(tcr->ref_value[eoPolarizability])); epot0 -= Epol; if (debug) { fprintf(debug,"mu_ind: %g (%g D) mu_aver: %g (%g D)\n", mu_ind,mu_ind*enm2Debye,mu_aver,mu_aver*enm2Debye); fprintf(debug,"Eref %g Epol %g Erunav %g Eact %g\n", tcr->ref_value[eoEpot],Epol,tcr->av_value[eoEpot], tcr->act_value[eoEpot]); } } if (bPrint) { pr_ff(tcr,t,idef,cr,nfile,fnm); } /* Calculate the deviation of average value from the target value */ for(i=0; (i<eoObsNR); i++) { deviation[i] = calc_deviation(tcr->av_value[i],tcr->act_value[i], tcr->ref_value[i]); prdev[i] = tcr->ref_value[i] - tcr->act_value[i]; } deviation[eoEpot] = calc_deviation(tcr->av_value[eoEpot],tcr->act_value[eoEpot], epot0); prdev[eoEpot] = epot0 - tcr->act_value[eoEpot]; if (bPrint) pr_dev(tcr,t,prdev,cr,nfile,fnm); /* First set all factors to 1 */ for(i=0; (i<atnr2); i++) { f6[i] = f12[i] = fa[i] = fb[i] = fc[i] = 1.0; } for(i=0; (i<idef->atnr); i++) fq[i] = 1.0; /* Now compute the actual coupling compononents */ if (!fr->bBHAM) { if (bDoIt) { for(i=0; (i<tcr->nLJ); i++) { tclj=&(tcr->tcLJ[i]); ati=tclj->at_i; atj=tclj->at_j; ff6 = ff12 = 1.0; if (tclj->eObs == eoForce) { gmx_fatal(FARGS,"Hack code for this to work again "); if (debug) fprintf(debug,"Have computed derivatives: xiH = %g, xiS = %g\n",xiH,xiS); if (ati == 1) { /* Hydrogen */ ff12 += xiH; } else if (ati == 2) { /* Shell */ ff12 += xiS; } else gmx_fatal(FARGS,"No H, no Shell, edit code at %s, line %d\n", __FILE__,__LINE__); if (ff6 > 0) set_factor_matrix(idef->atnr,f6, sqrt(ff6), ati,atj); if (ff12 > 0) set_factor_matrix(idef->atnr,f12,sqrt(ff12),ati,atj); } else { if (debug) fprintf(debug,"tcr->tcLJ[%d].xi_6 = %g, xi_12 = %g deviation = %g\n",i, tclj->xi_6,tclj->xi_12,deviation[tclj->eObs]); factor=deviation[tclj->eObs]; upd_f_value(log,idef->atnr,tclj->xi_6, dt,factor,f6, ati,atj); upd_f_value(log,idef->atnr,tclj->xi_12,dt,factor,f12,ati,atj); } } } if (PAR(cr)) { gprod(cr,atnr2,f6); gprod(cr,atnr2,f12); #ifdef DEBUGGCT dump_fm(log,idef->atnr,f6,"f6"); dump_fm(log,idef->atnr,f12,"f12"); #endif } upd_nbfplj(log,fr->nbfp,idef->atnr,f6,f12,tcr->combrule); /* Copy for printing */ for(i=0; (i<tcr->nLJ); i++) { tclj=&(tcr->tcLJ[i]); ati = tclj->at_i; atj = tclj->at_j; if (atj == -1) atj = ati; tclj->c6 = C6(fr->nbfp,fr->ntype,ati,atj); tclj->c12 = C12(fr->nbfp,fr->ntype,ati,atj); } } else { if (bDoIt) { for(i=0; (i<tcr->nBU); i++) { tcbu = &(tcr->tcBU[i]); factor = deviation[tcbu->eObs]; ati = tcbu->at_i; atj = tcbu->at_j; upd_f_value(log,idef->atnr,tcbu->xi_a,dt,factor,fa,ati,atj); upd_f_value(log,idef->atnr,tcbu->xi_b,dt,factor,fb,ati,atj); upd_f_value(log,idef->atnr,tcbu->xi_c,dt,factor,fc,ati,atj); } } if (PAR(cr)) { gprod(cr,atnr2,fa); gprod(cr,atnr2,fb); gprod(cr,atnr2,fc); } upd_nbfpbu(log,fr->nbfp,idef->atnr,fa,fb,fc); /* Copy for printing */ for(i=0; (i<tcr->nBU); i++) { tcbu=&(tcr->tcBU[i]); ati = tcbu->at_i; atj = tcbu->at_j; if (atj == -1) atj = ati; tcbu->a = BHAMA(fr->nbfp,fr->ntype,ati,atj); tcbu->b = BHAMB(fr->nbfp,fr->ntype,ati,atj); tcbu->c = BHAMC(fr->nbfp,fr->ntype,ati,atj); if (debug) fprintf(debug,"buck (type=%d) = %e, %e, %e\n", tcbu->at_i,tcbu->a,tcbu->b,tcbu->c); } } if (bDoIt) { for(i=0; (i<tcr->nQ); i++) { tcq=&(tcr->tcQ[i]); if (tcq->xi_Q) ffq = 1.0 + (dt/tcq->xi_Q) * deviation[tcq->eObs]; else ffq = 1.0; fq[tcq->at_i] *= ffq; } } if (PAR(cr)) gprod(cr,idef->atnr,fq); for(j=0; (j<md->nr); j++) { md->chargeA[j] *= fq[md->typeA[j]]; } for(i=0; (i<tcr->nQ); i++) { tcq=&(tcr->tcQ[i]); for(j=0; (j<md->nr); j++) { if (md->typeA[j] == tcq->at_i) { tcq->Q = md->chargeA[j]; break; } } if (j == md->nr) gmx_fatal(FARGS,"Coupling type %d not found",tcq->at_i); } for(i=0; (i<tcr->nIP); i++) { tip = &(tcr->tIP[i]); type = tip->type; ftype = idef->functype[type]; factor = dt*deviation[tip->eObs]; switch(ftype) { case F_BONDS: if (tip->xi.harmonic.krA) idef->iparams[type].harmonic.krA *= (1+factor/tip->xi.harmonic.krA); if (tip->xi.harmonic.rA) idef->iparams[type].harmonic.rA *= (1+factor/tip->xi.harmonic.rA); break; default: break; } tip->iprint=idef->iparams[type]; } }
void SimpleGLMeshMaterial::draw( TriangleMesh *mesh ) { if (mesh != NULL) { DynamicArrayOfStructures *pts = mesh->getVertices(); if (!pts->providesAttribute("position")) return; AAT POS = pts->getAAT("position"); AAT COL; bool hasCol = pts->providesAttribute("color"); if (hasCol) COL = pts->getAAT("color"); AAT NRM; bool hasNormal = pts->providesAttribute("normal"); if (hasNormal) NRM = pts->getAAT("normal"); DynamicArrayOfStructures *idx = mesh->getTriangles(); if (!idx->providesAttribute("index")) return; AAT IDX = idx->getAAT("index"); glPushAttrib(GL_ENABLE_BIT); if( showWireFrameOnly && showWireFrameOnlyDepthTest ) { glEnable(GL_DEPTH_TEST); if( cullBackFace ) glEnable(GL_CULL_FACE); else glDisable(GL_CULL_FACE); glEnable( GL_POLYGON_OFFSET_FILL ); glPolygonOffset( 1.2f, 4.0f ); const card32 numTri = idx->getNumEntries(); glBegin(GL_TRIANGLES); glColor3f(1,1,1); for (card32 i=0; i<numTri; i++) { Vector3i tind = idx->get<int32, 3>(i, IDX); Vector3f pos[3]; pos[0] = pts->get<float32, 3>(tind[0], POS); pos[1] = pts->get<float32, 3>(tind[1], POS); pos[2] = pts->get<float32, 3>(tind[2], POS); glVertex3fv(pos[0].data()); glVertex3fv(pos[1].data()); glVertex3fv(pos[2].data()); } glEnd(); } else if( !showWireFrameOnly ) { if( normalsDirectionColors ) { glEnable(GL_DEPTH_TEST); if( cullBackFace ) glEnable(GL_CULL_FACE); else glDisable(GL_CULL_FACE); const card32 numTri = idx->getNumEntries(); glBegin(GL_TRIANGLES); for (card32 i=0; i<numTri; i++) { Vector3i tind = idx->get<int32, 3>(i, IDX); Vector3f pos[3]; Vector3f color[3]; Vector3f normal[3]; pos[0] = pts->get<float32, 3>(tind[0], POS); pos[1] = pts->get<float32, 3>(tind[1], POS); pos[2] = pts->get<float32, 3>(tind[2], POS); if (!hasNormal || !useVertexNormals) { Vector3f tnormal = (pos[2]-pos[0]).crossProduct(pos[1]-pos[0]); float32 n = norm(tnormal); if (n > 1e-7) tnormal /= n; else continue; normal[0] = tnormal; normal[1] = tnormal; normal[2] = tnormal; } if (hasNormal && useVertexNormals) { normal[0] = pts->get<float32, 3>(tind[0], NRM); normal[1] = pts->get<float32, 3>(tind[1], NRM); normal[2] = pts->get<float32, 3>(tind[2], NRM); } for( card32 j=0; j<3; j++ ) { color[j] = (normal[j] + makeVector3f(1,1,1))* 0.5f; } glColor3fv(color[0].data()); glVertex3fv(pos[0].data()); glColor3fv(color[1].data()); glVertex3fv(pos[1].data()); glColor3fv(color[2].data()); glVertex3fv(pos[2].data()); } glEnd(); } else { glEnable(GL_DEPTH_TEST); glEnable(GL_LIGHTING); glEnable(GL_NORMALIZE); glEnable(GL_COLOR_MATERIAL); glDisable(GL_CULL_FACE); glColorMaterial(GL_FRONT_AND_BACK, GL_DIFFUSE); glEnable(GL_LIGHT0); glColor4f(1,1,1,1); float lightPos[] = {100.0f,1000.0f,100.0f,1.0f}; glLightfv( GL_LIGHT0, GL_POSITION, lightPos ); const card32 numTri = idx->getNumEntries(); glBegin(GL_TRIANGLES); for (card32 i=0; i<numTri; i++) { Vector3i tind = idx->get<int32, 3>(i, IDX); Vector3f pos[3]; Vector3f color[3]; Vector3f normal[3]; pos[0] = pts->get<float32, 3>(tind[0], POS); pos[1] = pts->get<float32, 3>(tind[1], POS); pos[2] = pts->get<float32, 3>(tind[2], POS); if (!hasNormal || !useVertexNormals) { Vector3f tnormal = (pos[2]-pos[0]).crossProduct(pos[1]-pos[0]); float32 n = norm(tnormal); if (n > 1e-20) tnormal /= n; normal[0] = tnormal; normal[1] = tnormal; normal[2] = tnormal; } if (hasCol) { color[0] = pts->get<float32, 3>(tind[0], COL); color[1] = pts->get<float32, 3>(tind[1], COL); color[2] = pts->get<float32, 3>(tind[2], COL); } if (hasNormal && useVertexNormals) { normal[0] = pts->get<float32, 3>(tind[0], NRM); normal[1] = pts->get<float32, 3>(tind[1], NRM); normal[2] = pts->get<float32, 3>(tind[2], NRM); } if (hasCol) glColor3fv(color[0].data()); glNormal3fv(normal[0].data()); glVertex3fv(pos[0].data()); if (hasCol) glColor3fv(color[1].data()); glNormal3fv(normal[1].data()); glVertex3fv(pos[1].data()); if (hasCol) glColor3fv(color[2].data()); glNormal3fv(normal[2].data()); glVertex3fv(pos[2].data()); } glEnd(); } } glPopAttrib(); glPushAttrib(GL_ENABLE_BIT); if (wireframe || showWireFrameOnly) { if( showWireFrameOnlyDepthTest && showWireFrameOnly) glEnable(GL_DEPTH_TEST); else glDisable(GL_DEPTH_TEST); glDisable(GL_LIGHTING); glDisable(GL_COLOR_MATERIAL); if( !blackWireFrame ) glColor4f(1,1,1,0.2f); else glColor4f(0,0,0,0.2f); glBlendFunc(GL_SRC_ALPHA, GL_ONE); glDepthMask(false); glEnable(GL_LINE_SMOOTH); glDisable( GL_CULL_FACE ); const card32 numTri = idx->getNumEntries(); glBegin(GL_LINES); for (card32 i=0; i<numTri; i++) { Vector3i tind = idx->get<int32, 3>(i, IDX); Vector3f pos[3]; pos[0] = pts->get<float32, 3>(tind[0], POS); pos[1] = pts->get<float32, 3>(tind[1], POS); pos[2] = pts->get<float32, 3>(tind[2], POS); glVertex3fv(pos[0].data()); glVertex3fv(pos[1].data()); glVertex3fv(pos[1].data()); glVertex3fv(pos[2].data()); glVertex3fv(pos[2].data()); glVertex3fv(pos[0].data()); } glEnd(); glDepthMask(true); } glPopAttrib(); glPushAttrib(GL_ENABLE_BIT); if( drawNormalsVectors && hasNormal) { glBegin( GL_LINES ); glColor3f( 1,0,0); for( card32 i=0;i<pts->getNumEntries();i++ ) { Vector3f pos = pts->get<float32,3>(i, POS ); Vector3f normal = pts->get<float32,3>(i, NRM ); Vector3f p2 = pos + normal * normalLength; glVertex3fv( &pos[0] ); glVertex3fv( &p2[0] ); } glEnd(); } glPopAttrib(); } }
/* simgal main ---------------------------------------------------------------*/ int main(int argc, char **argv) { FILE *fp; rnxopt_t rnxopt={{0}}; obs_t obs={0}; nav_t nav={0}; gtime_t ts={0},te={0}; double es[]={2000,1,1,0,0,0},ee[]={2000,1,1,0,0,0},tint=30.0; double pos[3]={0},rr[3]; char *infile[16]={0},*outfile=""; int i,j,n=0,opt=0; for (i=1;i<argc;i++) { if (!strcmp(argv[i],"-o")&&i+1<argc) outfile=argv[++i]; else if (!strcmp(argv[i],"-ts")&&i+1<argc) { sscanf(argv[++i],"%lf/%lf/%lf %lf:%lf:%lf",es,es+1,es+2,es+3,es+4,es+5); ts=epoch2time(es); } else if (!strcmp(argv[i],"-te")&&i+1<argc) { sscanf(argv[++i],"%lf/%lf/%lf %lf:%lf:%lf",ee,ee+1,ee+2,ee+3,ee+4,ee+5); te=epoch2time(ee); } else if (!strcmp(argv[i],"-ti")&&i+1<argc) tint=atof(argv[++i]); else if (!strcmp(argv[i],"-r")&&i+3<argc) { for (j=0;j<3;j++) pos[j]=atof(argv[++i]); /* lat,lon,hgt */ } else infile[n++]=argv[i]; } if (n<=0) { fprintf(stderr,"no input file\n"); return -1; } if (!*outfile) { fprintf(stderr,"no output file\n"); return -1; } if (norm(pos,3)<=0.0) { fprintf(stderr,"no receiver pos\n"); return -1; } pos[0]*=D2R; pos[1]*=D2R; pos2ecef(pos,rr); /* read simulated/real rinex nav files */ readrnx(infile,n,&obs,&nav); if (nav.n<=0) { fprintf(stderr,"no nav data\n"); return -1; } /* generate simulated observation data */ if (!simobs(ts,te,tint,rr,&nav,&obs,opt)) return -1; /* output rinex obs file */ if (!(fp=fopen(outfile,"w"))) { fprintf(stderr,"error : outfile open %s\n",outfile); return -1; } fprintf(stderr,"saving...: %s\n",outfile); strcpy(rnxopt.prog,PROGNAME); strcpy(rnxopt.comment[0],"SIMULATED OBS DATA"); rnxopt.tstart=ts; rnxopt.tstart=te; rnxopt.navsys=SYS_ALL; rnxopt.obstype=OBSTYPE_PR|OBSTYPE_CP|OBSTYPE_SNR; rnxopt.freqtype=FREQTYPE_L1|FREQTYPE_L2|FREQTYPE_L5|FREQTYPE_L7; for (i=0;i<3;i++) rnxopt.apppos[i]=rr[i]; outrnxobsh(fp,&rnxopt); for (i=0;i<obs.n;i=j) { for (j=i;j<obs.n;j++) { if (timediff(obs.data[j].time,obs.data[i].time)>0.001) break; } outrnxobsb(fp,&rnxopt,obs.data+i,j-i,0); } fclose(fp); return 0; }
/* generate simulated observation data ---------------------------------------*/ static int simobs(gtime_t ts, gtime_t te, double tint, const double *rr, nav_t *nav, obs_t *obs, int opt) { gtime_t time; obsd_t data[MAXSAT]={{{0}}}; double pos[3],rs[3*MAXSAT],dts[MAXSAT],r,e[3],azel[2]; double ecp[MAXSAT][NFREQ]={{0}},epr[MAXSAT][NFREQ]={{0}}; double snr[MAXSAT][NFREQ]={{0}},ers[MAXSAT][3]={{0}}; double iono,trop,fact,cp,pr,dtr=0.0,rref[3],bl; int i,j,k,n,ns,amb[MAXSAT][NFREQ]={{0}},sys,prn; char s[64]; double pref[]={36.106114294,140.087190410,70.3010}; /* ref station */ rtklib_trace(3,"simobs:nnav=%d ngnav=%d\n",nav->n,nav->ng); for (i=0;i<2;i++) pref[i]*=D2R; pos2ecef(pref,rref); for (i=0;i<3;i++) rref[i]-=rr[i]; bl=norm(rref,3)/1E4; /* baseline (10km) */ srand(0); /* ephemeris error */ for (i=0;i<MAXSAT;i++) { data[i].sat=i+1; data[i].P[0]=2E7; for (j=0;j<3;j++) ers[i][j]=randn(0.0,erreph); } srand(tickget()); ecef2pos(rr,pos); n=(int)(timediff(te,ts)/tint+1.0); for (i=0;i<n;i++) { time=timeadd(ts,tint*i); time2str(time,s,0); for (j=0;j<MAXSAT;j++) data[j].time=time; for (j=0;j<3;j++) { /* iteration for pseudorange */ satpos(time,data,MAXSAT,nav,rs,dts); for (k=0;k<MAXSAT;k++) { if ((r=geodist(rs+k*3,rr,e))<=0.0) continue; data[k].P[0]=r+CLIGHT*(dtr-dts[k]); } } satpos(time,data,MAXSAT,nav,rs,dts); for (j=ns=0;j<MAXSAT;j++) { /* add ephemeris error */ for (k=0;k<3;k++) rs[k+j*3]+=ers[j][k]; if ((r=geodist(rs+j*3,rr,e))<=0.0) continue; satazel(pos,e,azel); if (azel[1]<minel*D2R) continue; iono=ionmodel(time,nav->ion,pos,azel); trop=tropmodel(pos,azel,0.3); /* add ionospheric error */ iono+=errion*bl*ionmapf(pos,azel); snrmodel(azel,snr[j]); errmodel(azel,snr[j],ecp[j],epr[j]); sys=satsys(data[j].sat,&prn); for (k=0;k<NFREQ;k++) { data[j].L[k]=data[j].P[k]=0.0; data[j].SNR[k]=0; data[j].LLI[k]=0; if (sys==SYS_GPS) { if (k>=3) continue; /* no L5a/L5b in gps */ if (k>=2&&!gpsblock[prn-1]) continue; /* no L5 in block II */ } else if (sys==SYS_GLO) { if (k>=3) continue; } else if (sys==SYS_GAL) { if (k==1) continue; /* no L2 in galileo */ } else continue; /* generate observation data */ fact=lam[k]*lam[k]/lam[0]/lam[0]; cp=r+CLIGHT*(dtr-dts[j])-fact*iono+trop+ecp[j][k]; pr=r+CLIGHT*(dtr-dts[j])+fact*iono+trop+epr[j][k]; if (amb[j][k]==0) amb[j][k]=(int)(-cp/lam[k]); data[j].L[k]=cp/lam[k]+amb[j][k]; data[j].P[k]=pr; data[j].SNR[k]=(unsigned char)snr[j][k]; data[j].LLI[k]=data[j].SNR[k]<slipthres?1:0; } if (obs->nmax<=obs->n) { if (obs->nmax==0) obs->nmax=65532; else obs->nmax+=65532; if (!(obs->data=(obsd_t *)realloc(obs->data,sizeof(obsd_t)*obs->nmax))) { fprintf(stderr,"malloc error\n"); return 0; } } obs->data[obs->n++]=data[j]; ns++; } fprintf(stderr,"time=%s nsat=%2d\r",s,ns); } fprintf(stderr,"\n"); return 1; }
// return horizontal distance between two positions in cm float get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination) { return norm(destination.x-origin.x,destination.y-origin.y); }
void ISNorm::norm (ssi_sample_t &sample) { for (ssi_size_t i = 0; i < sample.num; i++) { norm (*sample.streams[i], i); } }
bool ISNorm::SaveParams(const ssi_char_t *path, Params ¶ms, File::TYPE type) { ssi_char_t string[SSI_MAX_CHAR]; FilePath fp(path); ssi_char_t *path_xml; ssi_char_t *path_data; if (ssi_strcmp(fp.getExtension(), "norm", false)) { path_xml = ssi_strcpy(path); } else { path_xml = ssi_strcat(path, ".norm"); } path_data = ssi_strcat(path_xml, "~"); TiXmlElement norm("norm"); TiXmlElement norm_method("method"); norm_method.InsertEndChild(TiXmlText(METHOD_NAMES[params.method])); norm.InsertEndChild(norm_method); if (params.method == METHOD::SCALE) { TiXmlElement norm_limits("limits"); ssi_sprint(string, "%f %f", params.limits[0], params.limits[1]); norm_limits.InsertEndChild(TiXmlText(string)); norm.InsertEndChild(norm_limits); } TiXmlElement norm_dim("dim"); ssi_sprint(string, "%u", params.n_features); norm_dim.InsertEndChild(TiXmlText(string)); norm.InsertEndChild(norm_dim); TiXmlElement norm_type("type"); norm_type.InsertEndChild(TiXmlText(File::TYPE_NAMES[type])); norm.InsertEndChild(norm_type); TiXmlDocument doc; TiXmlDeclaration head("1.0", "", ""); doc.InsertEndChild(head); doc.InsertEndChild(norm); doc.SaveFile(path_xml); if (params.method != METHOD::NONE) { FILE *fp = fopen(path_data, type == File::BINARY ? "wb" : "w"); if (fp) { switch (params.method) { case METHOD::SCALE: if (type == File::BINARY) { fwrite(params.mins, params.n_features, sizeof(ssi_real_t), fp); fwrite(params.maxs, params.n_features, sizeof(ssi_real_t), fp); } else { ssi_real_t *mins = params.mins; ssi_real_t *maxs = params.maxs; for (ssi_size_t i = 0; i < params.n_features; i++) { ssi_fprint(fp, "%f %f\n", *mins++, *maxs++); } } break; case METHOD::ZSCORE: if (type == File::BINARY) { fwrite(params.mean, params.n_features, sizeof(ssi_real_t), fp); fwrite(params.stdv, params.n_features, sizeof(ssi_real_t), fp); } else { ssi_real_t *mean = params.mean; ssi_real_t *stdv = params.stdv; for (ssi_size_t i = 0; i < params.n_features; i++) { ssi_fprint(fp, "%f %f\n", *mean++, *stdv++); } } break; } } else { ssi_wrn("could not open file '%s'", path_data); return false; } fclose(fp); } delete[] path_data; delete[] path_xml; return true; }
void findCircleParameter::findPoints(Point2i center, int radius, std::vector<cv::Point> &points, camMode projMode) { std::vector<cv::Point3f> spherePoints(points.size()); std::vector<cv::Point3f>::iterator itSphere = spherePoints.begin(); cout << points << endl; std::vector<cv::Point>::iterator it = points.begin(); while (it != points.end()) { int u = it->x; int v = it->y; //Convert to cartiesian cooradinate in unity circle int x_cart = (u - center.x); int y_cart = -(v - center.y); //convert to polar axes double theta = cvFastArctan(y_cart, x_cart)*PI / 180; double p = sqrt(pow(x_cart, 2) + pow(y_cart, 2)); double foval = 0.0; double Theta_sphere; switch (projMode) { case STEREOGRAPHIC: foval = radius / (2 * tan(FOV / 4)); Theta_sphere = 2 * atan(p / (2 * foval)); break; case EQUIDISTANCE: foval = radius / (FOV / 2); Theta_sphere = p / foval; break; case EQUISOLID: foval = radius / (2 * sin(FOV / 4)); Theta_sphere = 2 * asin(p / (2 * foval)); break; case ORTHOGONAL: foval = radius / sin(FOV / 2); Theta_sphere = asin(p / foval); break; default: cout << "The camera mode hasn't been choose!" << endl; } //convert to sphere surface parameter cooradinate double Phi_sphere = theta; //convert to sphere surface 3D cooradinate itSphere->x = sin(Theta_sphere)*cos(Phi_sphere); itSphere->y = sin(Theta_sphere)*sin(Phi_sphere); itSphere->z = cos(Theta_sphere); double temp = itSphere->x*itSphere->x + itSphere->y*itSphere->y + itSphere->z*itSphere->z; cout << "[x, y, z] = " << *itSphere << endl << "norm = " << sqrt(temp) << endl; ++it; ++itSphere; } /////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////// double angle = acos(spherePoints[0].dot(spherePoints[1])); //double angle = PI; cout << "spherePoints[0]=" << spherePoints[0] << endl; cout << "spherePoints[1]=" << spherePoints[1] << endl; cv::Point3f e3 = spherePoints[0].cross(spherePoints[1]); double norm_e3 = norm(e3); e3.x /= norm_e3; e3.y /= norm_e3; e3.z /= norm_e3; if (e3.dot(Point3f(0, 0, 1)) < 0) { e3 = spherePoints[1].cross(spherePoints[0]); double norm_e3 = norm(e3); e3.x /= norm_e3; e3.y /= norm_e3; e3.z /= norm_e3; //swap shpereSpoint[0] and spherePoints[1] spherePoints[0] = spherePoints[0] + spherePoints[1]; spherePoints[1] = spherePoints[0] - spherePoints[1]; spherePoints[0] = spherePoints[0] - spherePoints[1]; } cv::Point3f e1 = spherePoints[0]; cv::Point3f e2 = e3.cross(e1); cout << "e1.e2=" << e1.dot(e2) << endl << e2.dot(e3) << endl << e3.dot(e1) << endl; cout << "norm(e1)=" << norm(e1) << endl << norm(e2) << endl << norm(e3) << endl; std::vector<Point3f> tmpK; tmpK.push_back(e1); tmpK.push_back(e2); tmpK.push_back(e3); cout << e1 << endl << e2 << endl << e3 << endl; cout << "tmpK=" << tmpK << endl; Mat K = Mat(tmpK).reshape(1).t(); //从标准空间坐标到报像头空间坐标的变换矩阵 cout << "K=" << K << endl; Mat T = K.inv(CV_SVD);//从报像头空间坐标到标准空间坐标平面变换矩阵 points.clear(); const int count = 20; double step = angle / count; double start = 0.0; int l = 0; while (l++ <= count) { Point3f stdPt(cos(start), sin(start), 0); Mat matPt(stdPt); cout << matPt << endl << K << endl; Mat ptSphere(K*matPt); cout << ptSphere << endl; Mat_<double> ptSphere_double; ptSphere.convertTo(ptSphere_double, CV_64F); double x = ptSphere_double.at<double>(0, 0); double y = ptSphere_double.at<double>(1, 0); double z = ptSphere_double.at<double>(2, 0); cout << x << ", " << y << ", " << z << endl; //Convert from sphere cooradinate to the parameter sphere cooradinate double Theta_sphere = acos(z); double Phi_sphere = cvFastArctan(y, x)*PI / 180;//return value in Angle //////////////////////////////////////////////////////////////////////////////// double foval = 0.0; double p; switch (projMode) { case STEREOGRAPHIC: foval = radius / (2 * tan(FOV / 4)); p = 2 * foval*tan(Theta_sphere / 2); break; case EQUIDISTANCE: foval = radius / (FOV / 2); p = foval*Theta_sphere; break; case EQUISOLID: foval = radius / (2 * sin(FOV / 4)); p = 2 * foval*sin(Theta_sphere / 2); break; case ORTHOGONAL: foval = radius / sin(FOV / 2); p = foval*sin(Theta_sphere); break; default: cout << "The camera mode hasn't been choose!" << endl; } //Convert from parameter sphere cooradinate to fish-eye polar cooradinate //p = sin(Theta_sphere); double theta = Phi_sphere; //Convert from fish-eye polar cooradinate to cartesian cooradinate double x_cart = p*cos(theta); double y_cart = p*sin(theta); //double R = radius / sin(camerFieldAngle / 2); //Convert from cartesian cooradinate to image cooradinate double u = x_cart + center.x; double v = -y_cart + center.y; Point pt = Point(u, v); points.push_back(pt); ////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////// start += step; } ///////////////////////////////////////////////////////////////////// cout << points << endl; }
C3DPlotCanvas::C3DPlotCanvas(Project* project_s, C3DPlotFrame* t_frame, HighlightState* highlight_state_s, const std::vector<GeoDaVarInfo>& v_info, const std::vector<int>& col_ids, wxWindow *parent, wxWindowID id, const wxPoint& pos, const wxSize& size, long style) : wxGLCanvas(parent, id, pos, size, style), ball(0), project(project_s), table_int(project_s->GetTableInt()), num_obs(project_s->GetTableInt()->GetNumberRows()), num_vars(v_info.size()), num_time_vals(1), highlight_state(highlight_state_s), var_info(v_info), data(v_info.size()), scaled_d(v_info.size()), c3d_plot_frame(t_frame) { selectable_fill_color = GdaConst::three_d_plot_default_point_colour; highlight_color = GdaConst::three_d_plot_default_highlight_colour; canvas_background_color=GdaConst::three_d_plot_default_background_colour; data_stats.resize(var_info.size()); var_min.resize(var_info.size()); var_max.resize(var_info.size()); std::vector<double> temp_vec(num_obs); for (int v=0; v<num_vars; v++) { table_int->GetColData(col_ids[v], data[v]); table_int->GetColData(col_ids[v], scaled_d[v]); int data_times = data[v].shape()[0]; data_stats[v].resize(data_times); for (int t=0; t<data_times; t++) { for (int i=0; i<num_obs; i++) { temp_vec[i] = data[v][t][i]; } data_stats[v][t].CalculateFromSample(temp_vec); } } c3d_plot_frame->ClearAllGroupDependencies(); for (int i=0, sz=var_info.size(); i<sz; ++i) { c3d_plot_frame->AddGroupDependancy(var_info[i].name); } VarInfoAttributeChange(); UpdateScaledData(); xs = 0.1; xp = 0.0; ys = 0.1; yp = 0.0; zs = 0.1; zp = 0.0; m_bLButton = false; m_bRButton = false; m_brush = false; bb_min[0] = -1; bb_min[1] = -1; bb_min[2] = -1; bb_max[0] = 1; bb_max[1] = 1; bb_max[2] = 1; Vec3f b_min(bb_min); Vec3f b_max(bb_max); Vec3f ctr((bb_max[0]+bb_min[0])/2.0f, (bb_max[1]+bb_min[1])/2.0f, (bb_max[2]+bb_min[2])/2.0f); float radius = norm(b_max - ctr); ball = new Arcball(); ball->bounding_sphere(ctr, radius); m_x = false; m_y = false; m_z = false; m_d = true; b_select = false; isInit = false; highlight_state->registerObserver(this); }
void vertex::normalize(void) { float norm1 = 1./norm(); x *= norm1; y *= norm1; z *= norm1; }
Vector<Type>& Vector<Type>::normalize() { double n = norm(); if(n == 0) errorV("norm = 0"); return (*this) /= n; }
Quaternion Quaternion::inverse() const { return conjugate()/norm(); }
TEUCHOS_UNIT_TEST( Rythmos_BackwardEulerStepper, restart ) { RCP<const MomentoBase<double> > stepper_momento; // place to store solution at time = 1.0 RCP<const VectorBase<double> > x_norestart; RCP<const VectorBase<double> > xdot_norestart; // Create Backward Euler stepper // Step to t = 0.5, pull out the momento, then step the rest of the way to t = 1.0. { RCP<SinCosModel> model = sinCosModel(true); Thyra::ModelEvaluatorBase::InArgs<double> model_ic = model->getNominalValues(); RCP<Thyra::NonlinearSolverBase<double> > neSolver = timeStepNonlinearSolver<double>(); RCP<BackwardEulerStepper<double> > stepper = backwardEulerStepper<double>(model,neSolver); stepper->setInitialCondition(model_ic); double dt = 0.1; // Step to t=0.5 for (int i=0 ; i<5 ; ++i) { double dt_taken = stepper->takeStep(dt,STEP_TYPE_FIXED); TEST_ASSERT( dt_taken == dt ); } // Pull out the momento stepper_momento = stepper->getMomento(); // Step to t=1.0 for (int i=0 ; i<5 ; ++i) { double dt_taken = stepper->takeStep(dt,STEP_TYPE_FIXED); TEST_ASSERT( dt_taken == dt ); } { // Pull out the final solution Array<double> time_vec; time_vec.push_back(1.0); Array<RCP<const VectorBase<double> > > x_vec; Array<RCP<const VectorBase<double> > > xdot_vec; Array<double> accuracy_vec; stepper->getPoints(time_vec,&x_vec,&xdot_vec,&accuracy_vec); x_norestart = x_vec[0]; xdot_norestart = xdot_vec[0]; } } // Create a new Backward Euler Stepper { RCP<BackwardEulerStepper<double> > stepper = backwardEulerStepper<double>(); RCP<SinCosModel> model = sinCosModel(true); RCP<Thyra::NonlinearSolverBase<double> > neSolver = timeStepNonlinearSolver<double>(); // Put the momento back into the stepper stepper->setMomento(stepper_momento.ptr(),model,neSolver); double dt = 0.1; // Step to t=1.0 for (int i=0 ; i<5 ; ++i) { double dt_taken = stepper->takeStep(dt,STEP_TYPE_FIXED); TEST_ASSERT( dt_taken == dt ); } { // Pull out the final solution after restart Array<double> time_vec; time_vec.push_back(1.0); Array<RCP<const VectorBase<double> > > x_vec; Array<RCP<const VectorBase<double> > > xdot_vec; Array<double> accuracy_vec; stepper->getPoints(time_vec,&x_vec,&xdot_vec,&accuracy_vec); RCP<const VectorBase<double> > x_restart = x_vec[0]; RCP<const VectorBase<double> > xdot_restart = xdot_vec[0]; // Verify that x_restart == x_norestart RCP<VectorBase<double> > x_diff = createMember(x_restart->space()); V_VmV( x_diff.ptr(), *x_norestart, *x_restart ); double x_normDiff = norm(*x_diff); double tol = 1.0e-10; TEST_COMPARE( x_normDiff, <, tol ); // Verify that xdot_restart == xdot_norestart RCP<VectorBase<double> > xdot_diff = createMember(x_restart->space()); V_VmV( xdot_diff.ptr(), *xdot_norestart, *xdot_restart ); double xdot_normDiff = norm(*xdot_diff); TEST_COMPARE( xdot_normDiff, <, tol ); } } }
double norm(const sequence<T> &X, double p=2 ) { return norm( X.buffer(), p ); }
float shortest_angle_xy( const moon9::vec3 &a, const moon9::vec3 &b ) { float angle_rad = acos( dot( norm(a), norm(b) ) ); return rad2deg( angle_rad ); }
bool isZero(const vec& v) { return (norm(v) == 0); }
TYPE_MF_BB MedianFlow::calcRect(const TYPE_MF_BB &rect, const vector<TYPE_MF_PT> &pts, const vector<TYPE_MF_PT> &FPts, const vector<int> &rejected, int &status) { const int size = int(pts.size()); vector<TYPE_MF_COORD> dxs, dys; for(int i = 0; i < size; i++) { if(rejected[i]) continue; dxs.push_back(FPts[i].x - pts[i].x); dys.push_back(FPts[i].y - pts[i].y); } if(dxs.size() <= 1) { status = MF_TRACK_F_PTS; return BB_ERROR; } sort(dxs.begin(), dxs.end()); sort(dys.begin(), dys.end()); TYPE_MF_COORD dx = dxs[dxs.size() / 2]; TYPE_MF_COORD dy = dys[dys.size() / 2]; TYPE_MF_PT delta(dx, dy); vector<float> ratios; vector<float> absDist; for(int i = 0; i < size; i++) { if(rejected[i]) continue; for(int j = i + 1; j < size; j++) { if(rejected[j]) continue; float dist0 = norm(Mat(pts[i]), Mat(pts[j])); float dist1 = norm(Mat(FPts[i]), Mat(FPts[j])); float ratio = dist1 / dist0; ratios.push_back(ratio); } } sort(ratios.begin(), ratios.end()); float ratio = ratios[ratios.size() / 2]; TYPE_MF_BB ret(delta + rect.tl(), delta + rect.br()); TYPE_MF_PT center((float)(ret.tl().x + ret.br().x) / 2, (float)(ret.tl().y + ret.br().y) / 2); TYPE_MF_PT tl(center.x - ret.width / 2 * ratio, center.y - ret.height / 2 * ratio); TYPE_MF_PT br(center.x + ret.width / 2 * ratio, center.y + ret.height / 2 * ratio); ret = TYPE_MF_BB(tl, br); // debug //cout << ret << endl; // for(int i = 0; i < size; i++) { //if(rejected[i]) continue; float dist = norm(Mat(pts[i]), Mat(FPts[i])); absDist.push_back(dist); } sort(absDist.begin(), absDist.end()); float medianAbsDist = absDist[(int)absDist.size() / 2]; for(auto &i : absDist) //caution : must add '&' { i = abs(i - medianAbsDist); } sort(absDist.begin(), absDist.end()); cerr << "FB :" << absDist[(int)absDist.size() / 2] << endl; if(absDist[(int)absDist.size() / 2] > MF_ERROR_DIST) //if(medianAbsDist > 20) { status = MF_TRACK_F_CONFUSION; return BB_ERROR; } if(!isBoxUsable(ret)) { status = MF_TRACK_F_BOX; return BB_ERROR; } status = MF_TRACK_SUCCESS; return ret; }
void normalize(vec& v) { scalar mag = norm(v); for (vec::iterator it=v.begin(); it != v.end(); ++it) (*it) = (*it)/mag; }
Quaternion Quaternion::operator!() const { DefType n=-1.0/norm(); return Quaternion(X*n, Y*n, Z*n, -W*n); // ~(*this)/norm(); }
void Vector :: normalize( void ) { (*this) /= norm(); }
void calc_potential(const char *fn, atom_id **index, int gnx[], double ***slPotential, double ***slCharge, double ***slField, int *nslices, t_topology *top, int ePBC, int axis, int nr_grps, double *slWidth, double fudge_z, gmx_bool bSpherical, gmx_bool bCorrect, const output_env_t oenv) { rvec *x0; /* coordinates without pbc */ matrix box; /* box (3x3) */ int natoms; /* nr. atoms in trj */ t_trxstatus *status; int i, n, /* loop indices */ teller = 0, ax1 = 0, ax2 = 0, nr_frames = 0, /* number of frames */ slice; /* current slice */ double slVolume; /* volume of slice for spherical averaging */ double qsum, nn; real t; double z; rvec xcm; gmx_rmpbc_t gpbc = NULL; switch (axis) { case 0: ax1 = 1; ax2 = 2; break; case 1: ax1 = 0; ax2 = 2; break; case 2: ax1 = 0; ax2 = 1; break; default: gmx_fatal(FARGS, "Invalid axes. Terminating\n"); } if ((natoms = read_first_x(oenv, &status, fn, &t, &x0, box)) == 0) { gmx_fatal(FARGS, "Could not read coordinates from statusfile\n"); } if (!*nslices) { *nslices = static_cast<int>(box[axis][axis] * 10.0); /* default value */ } fprintf(stderr, "\nDividing the box in %d slices\n", *nslices); snew(*slField, nr_grps); snew(*slCharge, nr_grps); snew(*slPotential, nr_grps); for (i = 0; i < nr_grps; i++) { snew((*slField)[i], *nslices); snew((*slCharge)[i], *nslices); snew((*slPotential)[i], *nslices); } gpbc = gmx_rmpbc_init(&top->idef, ePBC, natoms); /*********** Start processing trajectory ***********/ do { *slWidth = box[axis][axis]/(*nslices); teller++; gmx_rmpbc(gpbc, natoms, box, x0); /* calculate position of center of mass based on group 1 */ calc_xcm(x0, gnx[0], index[0], top->atoms.atom, xcm, FALSE); svmul(-1, xcm, xcm); for (n = 0; n < nr_grps; n++) { /* Check whether we actually have all positions of the requested index * group in the trajectory file */ if (gnx[n] > natoms) { gmx_fatal(FARGS, "You selected a group with %d atoms, but only %d atoms\n" "were found in the trajectory.\n", gnx[n], natoms); } for (i = 0; i < gnx[n]; i++) /* loop over all atoms in index file */ { if (bSpherical) { rvec_add(x0[index[n][i]], xcm, x0[index[n][i]]); /* only distance from origin counts, not sign */ slice = static_cast<int>(norm(x0[index[n][i]])/(*slWidth)); /* this is a nice check for spherical groups but not for all water in a cubic box since a lot will fall outside the sphere if (slice > (*nslices)) { fprintf(stderr,"Warning: slice = %d\n",slice); } */ (*slCharge)[n][slice] += top->atoms.atom[index[n][i]].q; } else { z = x0[index[n][i]][axis]; z = z + fudge_z; if (z < 0) { z += box[axis][axis]; } if (z > box[axis][axis]) { z -= box[axis][axis]; } /* determine which slice atom is in */ slice = static_cast<int>((z / (*slWidth))); (*slCharge)[n][slice] += top->atoms.atom[index[n][i]].q; } } } nr_frames++; } while (read_next_x(oenv, status, &t, x0, box)); gmx_rmpbc_done(gpbc); /*********** done with status file **********/ close_trj(status); /* slCharge now contains the total charge per slice, summed over all frames. Now divide by nr_frames and integrate twice */ if (bSpherical) { fprintf(stderr, "\n\nRead %d frames from trajectory. Calculating potential" "in spherical coordinates\n", nr_frames); } else { fprintf(stderr, "\n\nRead %d frames from trajectory. Calculating potential\n", nr_frames); } for (n = 0; n < nr_grps; n++) { for (i = 0; i < *nslices; i++) { if (bSpherical) { /* charge per volume is now the summed charge, divided by the nr of frames and by the volume of the slice it's in, 4pi r^2 dr */ slVolume = 4*M_PI * sqr(i) * sqr(*slWidth) * *slWidth; if (slVolume == 0) { (*slCharge)[n][i] = 0; } else { (*slCharge)[n][i] = (*slCharge)[n][i] / (nr_frames * slVolume); } } else { /* get charge per volume */ (*slCharge)[n][i] = (*slCharge)[n][i] * (*nslices) / (nr_frames * box[axis][axis] * box[ax1][ax1] * box[ax2][ax2]); } } /* Now we have charge densities */ } if (bCorrect && !bSpherical) { for (n = 0; n < nr_grps; n++) { nn = 0; qsum = 0; for (i = 0; i < *nslices; i++) { if (std::abs((*slCharge)[n][i]) >= GMX_DOUBLE_MIN) { nn++; qsum += (*slCharge)[n][i]; } } qsum /= nn; for (i = 0; i < *nslices; i++) { if (std::abs((*slCharge)[n][i]) >= GMX_DOUBLE_MIN) { (*slCharge)[n][i] -= qsum; } } } } for (n = 0; n < nr_grps; n++) { /* integrate twice to get field and potential */ p_integrate((*slField)[n], (*slCharge)[n], *nslices, *slWidth); } if (bCorrect && !bSpherical) { for (n = 0; n < nr_grps; n++) { nn = 0; qsum = 0; for (i = 0; i < *nslices; i++) { if (std::abs((*slCharge)[n][i]) >= GMX_DOUBLE_MIN) { nn++; qsum += (*slField)[n][i]; } } qsum /= nn; for (i = 0; i < *nslices; i++) { if (std::abs((*slCharge)[n][i]) >= GMX_DOUBLE_MIN) { (*slField)[n][i] -= qsum; } } } } for (n = 0; n < nr_grps; n++) { p_integrate((*slPotential)[n], (*slField)[n], *nslices, *slWidth); } /* Now correct for eps0 and in spherical case for r*/ for (n = 0; n < nr_grps; n++) { for (i = 0; i < *nslices; i++) { if (bSpherical) { (*slPotential)[n][i] = ELC * (*slPotential)[n][i] * -1.0E9 / (EPS0 * i * (*slWidth)); (*slField)[n][i] = ELC * (*slField)[n][i] * 1E18 / (EPS0 * i * (*slWidth)); } else { (*slPotential)[n][i] = ELC * (*slPotential)[n][i] * -1.0E9 / EPS0; (*slField)[n][i] = ELC * (*slField)[n][i] * 1E18 / EPS0; } } } sfree(x0); /* free memory used by coordinate array */ }
//w=PI/2 Mat corrector::latitudeCorrection3(Mat imgOrg, Point2i center, int radius, distMapMode distMap, double theta_left, double phi_up, double camerFieldAngle, camMode camProjMode) { if (!(camerFieldAngle > 0 && camerFieldAngle <= PI)) { cout << "The parameter \"camerFieldAngle\" must be in the interval (0,PI]." << endl; return Mat(); } double rateOfWindow = 0.9; //int width = imgOrg.size().width*rateOfWindow; //int height = width; int width = max(imgOrg.cols, imgOrg.rows); int height = width; //int height = imgOrg.rows; Size imgSize(width, height); int center_x = imgSize.width / 2; int center_y = imgSize.height / 2; Mat retImg(imgSize, CV_8UC3, Scalar(0, 0, 0)); double dx = camerFieldAngle / imgSize.width; double dy = camerFieldAngle / imgSize.height; //coordinate for latitude map double latitude; double longitude; //unity sphere coordinate double x, y, z, r; //parameter cooradinate of sphere coordinate double Theta_sphere; double Phi_sphere; //polar cooradinate for fish-eye Image double p; double theta; //cartesian coordinate double x_cart, y_cart; //Image cooradinate of imgOrg double u, v; Point pt, pt1, pt2, pt3, pt4; //Image cooradinate of imgRet int u_latitude, v_latitude; Rect imgArea(0, 0, imgOrg.cols, imgOrg.rows); //offset of imgRet Origin double longitude_offset, latitude_offset; longitude_offset = (PI - camerFieldAngle) / 2; latitude_offset = (PI - camerFieldAngle) / 2; double foval = 0.0;//焦距 cv::Mat_<Vec3b> _retImg = retImg; cv::Mat_<Vec3b> _imgOrg = imgOrg; //according to the camera type to do the calibration for (int j = 0; j < imgSize.height; j++) { for (int i = 0; i < imgSize.width; i++) { Point3f tmpPt(i - center_x, center_y - j, 600);//最后一个参数用来修改成像面的焦距 double normPt = norm(tmpPt); switch (distMap) { case PERSPECTIVE: tmpPt.x /= normPt; tmpPt.y /= normPt; tmpPt.z /= normPt; x = tmpPt.x; y = tmpPt.y; z = tmpPt.z; break; case LATITUDE_LONGTITUDE: //latitude = latitude_offset + j*dy; latitude = getPhi((double)j*4.0 / imgSize.height); longitude = getPhi((double)i * 4 / imgSize.width); //latitude = latitude_offset + j*dy; //longitude = longitude_offset + i*dx; //Convert from latitude cooradinate to the sphere cooradinate x = -sin(latitude)*cos(longitude); y = cos(latitude); z = sin(latitude)*sin(longitude); break; default: break; } if (distMap == PERSPECTIVE) { //double theta = PI/4; //double phi = -PI/2; cv::Mat curPt(cv::Point3f(x, y, z)); std::vector<cv::Point3f> pts; //向东旋转地球 //pts.push_back(cv::Point3f(cos(theta), 0, -sin(theta))); //pts.push_back(cv::Point3f(0, 1, 0)); //pts.push_back(cv::Point3f(sin(theta), 0, cos(theta))); //向南旋转地球 //pts.push_back(cv::Point3f(1, 0, 0)); //pts.push_back(cv::Point3f(0, cos(phi), sin(phi))); //pts.push_back(cv::Point3f(0, -sin(phi), cos(phi))); //两个方向旋转 pts.push_back(cv::Point3f(cos(theta_left), 0, sin(theta_left))); pts.push_back(cv::Point3f(sin(phi_up)*sin(theta_left), cos(phi_up), -sin(phi_up)*cos(theta_left))); pts.push_back(cv::Point3f(-cos(phi_up)*sin(theta_left), sin(phi_up), cos(phi_up)*cos(theta_left))); cv::Mat revert = cv::Mat(pts).reshape(1).t(); cv::Mat changed(revert*curPt); cv::Mat_<double> changed_double; changed.convertTo(changed_double, CV_64F); x = changed_double.at<double>(0, 0); y = changed_double.at<double>(1, 0); z = changed_double.at<double>(2, 0); //std::cout << curPt << std::endl // <<revert<<std::endl; } //Convert from unit sphere cooradinate to the parameter sphere cooradinate Theta_sphere = acos(z); Phi_sphere = cvFastArctan(y, x);//return value in Angle Phi_sphere = Phi_sphere*PI / 180;//Convert from Angle to Radian switch (camProjMode) { case STEREOGRAPHIC: foval = radius / (2 * tan(camerFieldAngle / 4)); p = 2 * foval*tan(Theta_sphere / 2); break; case EQUIDISTANCE: foval = radius / (camerFieldAngle / 2); p = foval*Theta_sphere; break; case EQUISOLID: foval = radius / (2 * sin(camerFieldAngle / 4)); p = 2 * foval*sin(Theta_sphere / 2); break; case ORTHOGONAL: foval = radius / sin(camerFieldAngle / 2); p = foval*sin(Theta_sphere); break; default: cout << "The camera mode hasn't been choose!" << endl; } //Convert from parameter sphere cooradinate to fish-eye polar cooradinate //p = sin(Theta_sphere); theta = Phi_sphere; //Convert from fish-eye polar cooradinate to cartesian cooradinate x_cart = p*cos(theta); y_cart = p*sin(theta); //double R = radius / sin(camerFieldAngle / 2); //Convert from cartesian cooradinate to image cooradinate u = x_cart + center.x; v = -y_cart + center.y; pt = Point(u, v); if (!pt.inside(imgArea)) { continue; } _retImg.at<Vec3b>(j, i) = _imgOrg.at<Vec3b>(pt); } } //imshow("org", _imgOrg); //imshow("ret", _retImg); //cv::waitKey(); #ifdef _DEBUG_ cv::namedWindow("Corrected Image", CV_WINDOW_AUTOSIZE); imshow("Corrected Image", retImg); cv::waitKey(); #endif //imwrite("ret.jpg", retImg); return retImg; }
/* ******************************************************************************** */ int CalcConc(double *x, int **A, double *G, double *x0, int numSS, int numTotal, int MaxIters, double tol, double deltaBar, double eta, double kT, int MaxNoStep, int MaxTrial, double PerturbScale, int quiet, int WriteLogFile, char *logFile, double MolesWaterPerLiter, unsigned long seed) { /* Computes the equilbrium mole fractions of species in dilute solution using a trust region algorithm on the dual problem. Discussion of the method is in Dirks, et al., Thermodynamic analysis of interacting nucleic acid strands, SIAM Review, (2006), in press. The trust region algorithm for solving the dual problem is that in Nocedal and Wright, Numerical Optimization, 1999, page 68, with the dogleg method on page 71. Returns 1 if converged and 0 otherwise. */ int i,j; // Counters, i is over single-species and j is over all complexes int iters; // Number of iterations double *AbsTol; // The absolute tolerance on all values of gradient double rho; // Ratio of actual to predicted reduction in trust region method double delta; // Radius of trust region double *Grad; // The gradient of -g(lambda) double *lambda; // Lagrange multipliers (dual variables),x[j] = Q[j]*exp(lambda[j]) // for j \in \Psi^0 double *p; // The step we take toward minimization double **Hes; // The Hessian double FreeEnergy; // The free energy of the solution unsigned long rand_seed = 0; // Random number seed int nNoStep; // Number of iterations without taking a step int nTrial; // Number of times we've perturbed lambda int **AT; // Transpose of A int RunStats[6]; // Statistics on results from getSearchDir (see comments below) FILE *fplog; // Log file // Initialize iters just so compiler doesn't give a warning when optimization is on iters = 0; // Allocate memory AT = (int **) malloc(numTotal * sizeof(int *)); for (j = 0; j < numTotal; j++) { AT[j] = (int *) malloc(numSS * sizeof(int)); } Hes = (double **) malloc(numSS * sizeof(double *)); for (i = 0; i < numSS; i++) { Hes[i] = (double *) malloc(numSS * sizeof(double)); } AbsTol = (double *) malloc(numSS * sizeof(double)); Grad = (double *) malloc(numSS * sizeof(double)); lambda = (double *) malloc(numSS * sizeof(double)); p = (double *) malloc(numSS * sizeof(double)); // The absolute tolerance is a percentage of the entries in x0 for (i = 0; i < numSS; i++) { AbsTol[i] = tol * x0[i]; } // Compute AT (transpose of A), useful to have around. IntTranspose(AT,A,numSS,numTotal); nTrial = 0; for (i = 0; i < numSS; i++) { Grad[i] = AbsTol[i] + 1.0; // Initialize just to get started. } while (CheckTol(Grad,AbsTol,numSS) == 0 && nTrial < MaxTrial) { if (nTrial == 1) { // Seed the random number generator if necessary rand_seed = GetRandSeed(seed); init_genrand(rand_seed); } // Set initial guess getInitialGuess(x0,lambda,G,AT,A,numSS,numTotal,PerturbScale,rand_seed); // Calculate the counts of the species based on lambda if (getx(x,lambda,G,AT,numSS,numTotal) == 0) { // Should be fine; checked prev. if (quiet == 0) { printf("Overflow error in calcution of mole fractions.\n\n"); printf("Exiting....\n"); } exit(ERR_OVERFLOW); } // Calculate the gradient getGrad(Grad,x0,x,A,numSS,numTotal); // Initialize delta to be just less than deltaBar delta = 0.99 * deltaBar; // Initializations iters = 0; nNoStep = 0; RunStats[0] = 0; // Number of pure Newton steps (didn't hit trust region boundary) RunStats[1] = 0; // Number of pure Cauchy steps (hit trust region boundary) RunStats[2] = 0; // Number of dogleg steps (part Newton and part Cauchy) RunStats[3] = 0; // Number of steps with Cholesky failure forcing Cauchy step RunStats[4] = 0; // Number of steps with irrelevant Cholesky failures RunStats[5] = 0; // Number of failed dogleg calculations // Run trust region with these initial conditions while (iters < MaxIters && CheckTol(Grad,AbsTol,numSS) == 0 && nNoStep < MaxNoStep) { // Compute the Hessian (symmetric, positive, positive definite) getHes(Hes,x,A,numSS,numTotal); // Solve for the search direction (RunStats[getSearchDir(p,Grad,Hes,delta,numSS) - 1])++; // Calculate rho, ratio of actual to predicted reduction rho = getRho(lambda,p,Grad,x,Hes,x0,G,AT,numSS,numTotal); // Adjust delta and make step based on rho if (rho < 0.25) { delta /= 4.0; } else if (rho > 0.75 && fabs(norm(p,numSS) - delta) < NUM_PRECISION) { delta = min2(2.0*delta,deltaBar); } if (rho > eta) { for (i = 0; i < numSS; i++) { lambda[i] += p[i]; } nNoStep = 0; } else { nNoStep++; } // Calculate the mole fractions of the complexes based on lambda if (getx(x,lambda,G,AT,numSS,numTotal) == 0) {// Should be fine;checked prev. if (quiet == 0) { printf("Overflow error in calcution of mole fractions.\n\n"); printf("Exiting....\n"); } exit(ERR_OVERFLOW); } // Calculate the gradient getGrad(Grad,x0,x,A,numSS,numTotal); // Advance the iterations count iters++; } // Advance the number of perturbations we've tried nTrial++; } // Compute the free energy FreeEnergy = 0; // First the reference free energy for (i = 0; i < numSS; i++) { FreeEnergy += x0[i]*(1.0 - log(x0[i])); } // Now the free energy for (j = 0; j < numTotal; j++) { if (x[j] > 0) { FreeEnergy += x[j]*(log(x[j]) + G[j] - 1.0); } } // Convert to kcal/liter of solution FreeEnergy *= kT*MolesWaterPerLiter; /* **************** WRITE OUT RESULTS ********************************* */ if ( nTrial == MaxTrial && quiet == 0) { printf("\n\n TRUST REGION METHOD DID NOT CONVERGE DUE TO PRECISION ISSUES\n\n"); } // Report errors in conservation of mass to screen if (quiet == 0) { // Print out values of the gradient, which is the error in cons. of mass printf("Error in conservation of mass:\n"); for (i = 0; i < numSS; i++) { printf(" %8.6e Molar\n",Grad[i]*MolesWaterPerLiter); } printf("\n"); // Print out the free energy of the solution printf("Free energy = %8.6e kcal/litre of solution\n",FreeEnergy); } // Write out details of calculation to outfile if (WriteLogFile) { if ((fplog = fopen(logFile,"a")) == NULL) { if (quiet == 0) { printf("Error opening %s.\n\nExiting....\n",logFile); } exit(ERR_LOG); } if (nTrial == MaxTrial) { fprintf(fplog,"TRUST REGION DID NOT CONVERGE DUE TO PRECISION ISSUES\n\n"); } fprintf(fplog," --Trust region results:\n"); fprintf(fplog," No. of initial conditions tried: %d\n",nTrial); fprintf(fplog," Results from succesful trial:\n"); fprintf(fplog," No. of iterations: %d\n",iters); fprintf(fplog," No. of Newton steps: %d\n",RunStats[0]); fprintf(fplog," No. of Cauchy steps: %d\n",RunStats[1]); fprintf(fplog," No. of dogleg steps: %d\n",RunStats[2]); fprintf(fplog," No. of Cholesky failures resulting in Cauchy steps: %d\n" ,RunStats[3]); fprintf(fplog," No. of inconsequential Cholesky failures: %d\n", RunStats[4]); fprintf(fplog," No. of dogleg failures: %d\n",RunStats[5]); fprintf(fplog," Error in conservation of mass (units of molarity):\n"); fprintf(fplog," Error\tTolerance\n"); for (i = 0; i < numSS; i++) { fprintf(fplog, " %.14e\t%.14e\n",Grad[i]*MolesWaterPerLiter, AbsTol[i]*MolesWaterPerLiter); } fprintf(fplog," --Free energy of solution = %.14e kcal/litre of solution\n", FreeEnergy); fclose(fplog); } /* **************** END OF WRITING OUT RESULTS **************************** */ // Free memory for (j = 0; j < numTotal; j++) { free(AT[j]); } for (i = 0; i < numSS; i++) { free(Hes[i]); } free(AbsTol); free(AT); free(Hes); free(Grad); free(p); free(lambda); // Return convergence if (nTrial == MaxTrial) { return 0; } else { return 1; } }