ElRotation3D cOneRotPowelOptimize::Param2Rot(const double * aP) { if (DEBUG_POWEL) { for (int aK=0 ; aK<5 ; aK++) std::cout << aP[aK] << " "; std::cout << "\n"; } if (mContr==ePoseFigee) return ElRotation3D(mU,mMatr,true); aP+= mI0; double aF = 0.05; Pt3dr aTr = mU*cos(sqrt(ElSquare(aP[0]*aF)+ElSquare(aP[1]*aF))) + mV*sin(aP[0]*aF) + mW*sin(aP[1]*aF); aTr = vunit(aTr) ; double aN = mNorm; if (mContr==ePoseLibre) aN *= 1+aF*aP[5]; return ElRotation3D ( vunit(aTr) * aN, mMatr * ElMatrix<double>::Rotation(aP[2]*aF,aP[3]*aF,aP[4]*aF), true ); }
void TraitementSpec_CdD (std::vector<cCibleCalib> & aVCC,tFINT aFCol) { std::vector<int> aVS1; std::vector<Pt2dr> aVSP; std::vector<Pt2dr> aVTgt; for (int aK=0 ; aK<int(aVCC.size()) ; aK++) { int aCol = aFCol(aK); ELISE_ASSERT(aCol>=0 || (aCol==0),"TraitementSpec_CdD"); while ( int(aVS1.size()) <= aCol) { aVS1.push_back(0); aVSP.push_back(Pt2dr(0,0)); } aVS1[aCol]++; Pt3dr aP3 =aVCC[aK].Position(); aVSP[aCol] = aVSP[aCol]+Pt2dr(aP3.x,aP3.y); } for (int aK=0 ;aK<int(aVSP.size()) ; aK++) { aVSP[aK] = aVSP[aK] / aVS1[aK]; aVTgt.push_back(Pt2dr(0,0)); } for (int aK=0 ; aK<int(aVCC.size()) ; aK++) { int aC0 = aFCol(aK); Pt2dr aTgt(0,0); if (aC0==0) { Pt2dr aD01 = vunit(aVSP[1]-aVSP[0]); Pt2dr aD12 = vunit(aVSP[2]-aVSP[1]); aTgt = vunit(aD01 + (aD01-aD12)/2.0); } else if (aC0 == int(aVSP.size())-1) { Pt2dr aDP1P0 = vunit(aVSP[aC0]-aVSP[aC0-1]); Pt2dr aDP2P1 = vunit(aVSP[aC0-1]-aVSP[aC0-2]); aTgt = vunit(aDP1P0 + (aDP1P0-aDP2P1)/2.0); } else { Pt2dr aDP0 = vunit(aVSP[aC0]-aVSP[aC0-1]); Pt2dr aD0N = vunit(aVSP[aC0+1]-aVSP[aC0]); aTgt = vunit((aDP0+aD0N)/2.0); } aVTgt[aC0] = aTgt; Pt2dr aN = aTgt * Pt2dr(0,1); aVCC[aK].Normale() = Pt3dr(aN.x,aN.y,0); } for (int aK=0 ;aK<int(aVSP.size()) ; aK++) std::cout << aK << " :: " << aVTgt[aK] << "\n"; }
// Calcul les vecteur qui sont |_ dans un plan contenant la base void cMEPCoCentrik::ComputePlanBase(const ElMatrix<REAL> & aMat) { Pt3di aRouge(255,0,0); Pt3di aVert(0,255,0); Pt3di aGrayFonc(64,64,64); Pt3di aGrayClair(180,180,180); double aMulP = 1.1; std::vector<Pt3dr> aVpt; for (int aKP =0 ; aKP< int(mVP1.size()) ; aKP++) { Pt3dr aQ1 = vunit(PZ1(mVP1[aKP])); Pt3dr aQ2 = aMat * vunit(PZ1(mVP2[aKP])); Pt3dr anOB = aQ1^aQ2; if (euclid(anOB) != 0) { anOB = vunit(anOB); if (mDoPly) { mColPly.push_back(aRouge); mPtsPly.push_back(anOB); mColPly.push_back(aGrayFonc); mPtsPly.push_back(aQ1*aMulP); mColPly.push_back(aGrayClair); mPtsPly.push_back(aQ2*aMulP); } mVPlanBase.push_back(anOB); } } if (mDoPly) { for (int aK=0 ; aK<10000 ; aK++) { int aKA = NRrandom3(mVPlanBase.size()); int aKB = NRrandom3(mVPlanBase.size()); Pt3dr aBase = mVPlanBase[aKA] ^ mVPlanBase[aKB]; if (euclid (aBase)>1e-6) { mColPly.push_back(aVert); mPtsPly.push_back(vunit(aBase)); } } } }
Pt3dr cTriangle::getNormale(bool normalize) const { vector <Pt3dr> vPts; getVertexes(vPts); if (normalize) { Pt3dr p1 = vunit(vPts[1]-vPts[0]); Pt3dr p2 = vunit(vPts[2]-vPts[0]); return vunit(p1^p2); } else return (vPts[1]-vPts[0])^(vPts[2]-vPts[0]); }
void cMEPCoCentrik::OneItereRotPur(ElMatrix<REAL> & aMat,double & anErrStd) { L2SysSurResol aSysLin3(3); aSysLin3.GSSR_Reset(false); std::vector<double> aVRes; double aSomP=0; double aSomErr=0; for (ElPackHomologue::const_iterator itP=mPack.begin() ; itP!=mPack.end() ; itP++) { Pt3dr aQ1 = vunit(PZ1(itP->P1())); Pt3dr aQ2 = aMat * vunit(PZ1(itP->P2())); double aVQ2[3],aVQ1[3]; aQ2.to_tab(aVQ2); aQ1.to_tab(aVQ1); double anEcart = euclid(aQ1-aQ2); aVRes.push_back(anEcart); double aPds = itP->Pds() / (1 + ElSquare(anEcart / (2*anErrStd))); aSomP += aPds; aSomErr += aPds * square_euclid(aQ1-aQ2);; ElMatrix<REAL> aMQ2 = MatProVect(aQ2); for (int aY=0 ; aY< 3 ; aY++) { double aCoeff[3]; for (int aX=0 ; aX< 3 ; aX++) aCoeff[aX] = aMQ2(aX,aY); aSysLin3.GSSR_AddNewEquation(aPds,aCoeff,aVQ2[aY]-aVQ1[aY],0); } } double anErrRobut = MoyKPPVal(aVRes,NbForEcart(aVRes.size())); double anErrQuad = sqrt(aSomErr/aSomP); if (0) { std::cout << "ERR QUAD " << anErrQuad * mFoc << " Robust " << anErrRobut * mFoc << "\n"; } anErrStd = anErrQuad; Im1D_REAL8 aSol = aSysLin3.GSSR_Solve (0); double * aData = aSol.data(); ElMatrix<double> aMPV = MatProVect(Pt3dr(aData[0],aData[1],aData[2])); // std::cout << " aData " << aData[0] << " " << aData[1] << " " << aData[2] << "\n"; // aMat = NearestRotation(aMat * (ElMatrix<double>(3,true) +aMPV)); aMat = NearestRotation((ElMatrix<double>(3,true) +aMPV) * aMat); }
Pt2dr prolong ( TIm2D<INT1,INT> & im, const Seg2d & seg, REAL step, REAL Rvlim, bool BorneInf, bool etirement ) { Pt2dr dec = vunit(seg.p0()-seg.p1()) * step; if (! etirement) dec = -dec; Pt2dr pt = seg.p0(); INT Ivlim = (INT)(Rvlim * (1<<(2*NBB))); Pt2di p0 (1,1); Pt2di p1 (im.sz()-Pt2di(2,2)); for (;;) { ElPFixed<NBB> Fpt (pt); if (! Fpt.inside(p0,p1)) return pt; INT v = TImGet<INT1,INT,NBB>::getb2(im,Fpt); if ( BorneInf ? (v>Ivlim) : (v<Ivlim) ) pt += dec; else return pt; } }
ElRotation3D RotationCart2RTL(Pt3dr aP, double aZ,Pt3dr aDirX ) { Pt3dr aDirZ = vunit(aP); aP = aP - aDirZ * aZ; // Pt3dr aDirX = vunit(Pt3dr(0,0,1)^aDirZ); Pt3dr aDirY = vunit(aDirZ^aDirX); aDirX = vunit(aDirY^aDirZ); ElRotation3D aRTL2Cart(aP,MatFromCol(aDirX,aDirY,aDirZ),true); // std::cout << "TES-RTL :: " << aRTL2Cart.inv().ImAff(aP *1.001) << "\n"; // std::cout << "TES-RTL :: " << aRTL2Cart.inv().ImAff(aP) << "\n"; return aRTL2Cart.inv(); }
void get_ray_dir(t_env *e, double x, double y) { e->ray.dir = vunit(vsub(vsub( vadd(e->camera.l, vmult(e->camera.u, x * e->camera.stepx)), vmult(e->camera.v, y * e->camera.stepy)), e->camera.loc)); e->ray.loc = e->camera.loc; e->ray.o_in = NULL; e->ray.ior = 1; }
Pt2dr cApply_CreateEpip_main::DirEpipIm2(cBasicGeomCap3D * aG1,cBasicGeomCap3D * aG2,ElPackHomologue & aPack,bool AddToP1) { Pt2dr aSz = Pt2dr(aG1->SzBasicCapt3D()); Pt2dr aSomTens2(0,0); double aIProf = aG1->GetVeryRoughInterProf(); double aEps = 5e-4; double aLenghtSquare = ElMin(mLengthMin,sqrt((aSz.x*aSz.y) / (mNbXY*mNbXY))); int aNbX = ElMax(1+3*mDegre,round_up(aSz.x /aLenghtSquare)); int aNbY = ElMax(1+3*mDegre,round_up(aSz.y /aLenghtSquare)); std::cout << "NBBBB " << aNbX << " " << aNbY << "\n"; for (int aKX=0 ; aKX<= aNbX ; aKX++) { double aPdsX = ElMax(aEps,ElMin(1-aEps,aKX /double(aNbX))); for (int aKY=0 ; aKY<= aNbY ; aKY++) { double aPdsY = ElMax(aEps,ElMin(1-aEps,aKY/double(aNbY))); Pt2dr aPIm1 = aSz.mcbyc(Pt2dr(aPdsX,aPdsY)); if (aG1->CaptHasData(aPIm1)) { Pt3dr aPT1; Pt3dr aC1; aG1->GetCenterAndPTerOnBundle(aC1,aPT1,aPIm1); std::vector<Pt2dr> aVPIm2; for (int aKZ = -mNbZ ; aKZ <= mNbZ ; aKZ++) { Pt3dr aPT2 = aC1 + (aPT1-aC1) * (1+(aIProf*aKZ) / mNbZ); if (aG1->PIsVisibleInImage(aPT2) && aG2->PIsVisibleInImage(aPT2)) { aVPIm2.push_back(aG2->Ter2Capteur(aPT2)); ElCplePtsHomologues aCple(aPIm1,aVPIm2.back(),1.0); if (! AddToP1) aCple.SelfSwap(); aPack.Cple_Add(aCple); } } if (aVPIm2.size() >=2) { Pt2dr aDir2 = vunit(aVPIm2.back()-aVPIm2[0]); aSomTens2 = aSomTens2 + aDir2 * aDir2; // On double l'angle pour en faire un tenseur } } } } Pt2dr aRT = Pt2dr::polar(aSomTens2,0.0); return Pt2dr::FromPolar(1.0,aRT.y/2.0); }
void setup_camera_plane(t_env *e) { t_vector n; t_vector c; double w; double h; h = 18.0 * ARBITRARY_NUMBER / 35.0; w = h * (double)e->x / (double)e->y; n = vunit(vsub(e->camera.loc, e->camera.dir)); e->camera.u = vunit(vcross(e->camera.up, n)); e->camera.v = vunit(vcross(n, e->camera.u)); c = vsub(e->camera.loc, vmult(n, ARBITRARY_NUMBER)); e->camera.l = vadd(vsub(c, vmult(e->camera.u, w / 2.0)), vmult(e->camera.v, h / 2.0)); e->camera.stepx = w / (double)e->x; e->camera.stepy = h / (double)e->y; }
int main(int argc, char* argv[]) { puts("args: path/to/obj path/to/bmp"); FILE* const fobj = oload(argc == 3 ? argv[1] : "model/salesman.obj"); SDL_Surface* const fdif = sload(argc == 3 ? argv[2] : "model/salesman.bmp"); const Obj obj = oparse(fobj); const Triangles tv = tvgen(obj); // Triangle Vertices. const Triangles tt = ttgen(obj); // Triangle Textures. const Triangles tn = tngen(obj); // Triangle Normals. const Sdl sdl = ssetup(800, 600); float* const zbuff = (float*) malloc(sizeof(float) * sdl.xres * sdl.yres); for(Input input = iinit(); !input.done; input = ipump(input)) { uint32_t* const pixel = slock(sdl); reset(zbuff, pixel, sdl.xres * sdl.yres); const Vertex center = { 0.0f, 0.0f, 0.0f }; const Vertex upward = { 0.0f, 1.0f, 0.0f }; const Vertex eye = { sinf(input.xt), sinf(input.yt), cosf(input.xt) }; const Vertex z = vunit(vsub(eye, center)); const Vertex x = vunit(vcross(upward, z)); const Vertex y = vcross(z, x); for(int i = 0; i < tv.count; i++) { const Triangle nrm = tviewnrm(tn.triangle[i], x, y, z); const Triangle tex = tt.triangle[i]; const Triangle tri = tviewtri(tv.triangle[i], x, y, z, eye); const Triangle per = tperspective(tri); const Triangle vew = tviewport(per, sdl); const Target target = { vew, nrm, tex, fdif }; tdraw(sdl.yres, pixel, zbuff, target); } sunlock(sdl); schurn(sdl); spresent(sdl); } // Let the OS free hoisted memory for a quick exit. return 0; }
ElMatrix<REAL> GlobMepRelCocentrique(double & anEcartMin,const ElPackHomologue & aPack, int aNbRansac,int aNbMaxPts) { aNbMaxPts = ElMin(aNbMaxPts,aPack.size()); std::vector<Pt3dr> aVDir1; std::vector<Pt3dr> aVDir2; cRandNParmiQ aRand(aNbMaxPts,aPack.size()); for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++) { if (aRand.GetNext()) { aVDir1.push_back(vunit(PZ1(itH->P1()))); aVDir2.push_back(vunit(PZ1(itH->P2()))); } } ElMatrix<REAL> aRes(3,3); anEcartMin = 1e60; while (aNbRansac) { int aKA = NRrandom3(aVDir1.size()); int aKB = NRrandom3(aVDir2.size()); if (aKA!=aKB) { aNbRansac--; ElMatrix<REAL> aMat = ComplemRotation(aVDir1[aKA],aVDir1[aKB],aVDir2[aKA],aVDir2[aKB]); double anEc = SomEcartDist(aMat,aVDir1,aVDir2); if (anEc<anEcartMin) { anEcartMin = anEc; aRes = aMat; } } } return aRes; }
void El_Window::draw_arrow ( Pt2dr p0, Pt2dr p1, Line_St LAxe, Line_St LPointe, REAL size_pointe, REAL pos , REAL teta ) { draw_seg(p0,p1,LAxe); Pt2dr q0 = barry(1-pos,p0,p1); Pt2dr dir_pte = Pt2dr::FromPolar(1.0,teta); Pt2dr tgt = vunit(p0-p1) * size_pointe; draw_seg(q0,q0+tgt*dir_pte,LPointe); draw_seg(q0,q0+tgt/dir_pte,LPointe); }
static void set_refract_ray_prim(t_env *e, t_env *refract) { t_vector n; refract->ray.loc = vadd(e->ray.loc, vmult(e->ray.dir, e->t)); n = get_normal(e, refract->ray.loc); if (refract->flags & RAY_INSIDE) { n = vunit(vsub((t_vector){0.0, 0.0, 0.0}, n)); if (refract_prim(e, refract, n)) refract->flags &= ~RAY_INSIDE; else set_reflect_ray(e, refract); } else { if (refract_prim(e, refract, n)) refract->flags |= RAY_INSIDE; else set_reflect_ray(e, refract); } }
void cMEPCoCentrik::Test(const ElPackHomologue & aPack,const ElMatrix<REAL> & aMat,const ElRotation3D * aRef,double anEcart) { if (mShow) { std::cout << " ============== ROTATION PURE =============\n"; } ComputePlanBase(aMat); Pt3dr aN1 = ComputeBase(); Pt3dr aN2 = ComputeNormBase(); Pt3dr aN3 = vunit(aN1^aN2); ElRotation3D aRInit(aN3,aMat,true); mSolVraiR = OneTestMatr(aMat,aN3,anEcart); mCostVraiR = ProjCostMEP(aPack,mSolVraiR,0.1); mPMed = MedianNuage(aPack,mSolVraiR); if (mPMed.z < 0) { if (mShow) std::cout << " ------------- Z Neg in Pur rot : swap -------------\n"; mSolVraiR = ElRotation3D(-mSolVraiR.tr(),mSolVraiR.Mat(),true); mPMed = MedianNuage(aPack,mSolVraiR); aN3 = -aN3; } if ( mShow) { if (aRef) { std::cout << "Ref " << vunit(aRef->tr()) << " Norm " << aN3 << "\n"; std::cout << "COST REF " << ProjCostMEP(aPack,*aRef,0.1)* mFoc << "\n"; ElRotation3D aSol2 = OneTestMatr(aRef->Mat(),aRef->tr(),anEcart); std::cout << "C REFOPT " << ProjCostMEP(aPack,aSol2,0.1)* mFoc << "\n"; // ShowMatr("REF/Mat",aRef->Mat()*aMat.transpose()); } std::cout << " ###### Co-Centrik COST-Init " << ProjCostMEP(aPack,aRInit,0.1)* mFoc << "\n"; std::cout << "Sol , Cost " << mCostVraiR * mFoc << " Tr " << mSolVraiR.tr() << " Med " << mPMed << "\n"; } // ShowMatr("REF/Sol",aRef->Mat()*aSol.Mat().transpose()); if (mDoPly) { std::list<std::string> aVCom; std::vector<const cElNuage3DMaille *> aVNuage; cElNuage3DMaille::PlyPutFile ( "Base.ply", aVCom, aVNuage, &mPtsPly, &mColPly, true ); } }
Pt3dr cMEPCoCentrik::ComputeNormBase() { // Valeur initiale par Ransac double aSomMin = 1e30; Pt3dr aBestNorm(0,0,0); for (int aCpt=0 ; aCpt<NbCpleBase ; ) { int aKA = NRrandom3(mVPlanBase.size()); int aKB = NRrandom3(mVPlanBase.size()); if (aKA!=aKB) { Pt3dr aN = mVPlanBase[aKA] ^ mVPlanBase[aKB]; if (euclid(aN) !=0) { aN = vunit(aN); double aSom = 0; for (int aK=0 ; aK< int(mVPlanBase.size()) ; aK++) { aSom += ElAbs(scal(aN,mVPlanBase[aK])); } aSom /= mVPlanBase.size(); if (aSom < aSomMin) { aSomMin = aSom; aBestNorm = aN; } aCpt++; } } } // Pt3dr aN0 = aBestNorm; // Affinaga par LSQ // Ort . (aBestNor + b B + c C) =0 for (int aCpt=0 ; aCpt < 3 ; aCpt++) { Pt3dr aB,aC; MakeRONWith1Vect(aBestNorm,aB,aC); L2SysSurResol aSysLin2(2); aSysLin2.GSSR_Reset(false); double aSomP = 0; double aSomE = 0; for (int aK=0 ; aK< int(mVPlanBase.size()) ; aK++) { double aCoeff[2]; Pt3dr aP = mVPlanBase[aK]; double aCste = scal(aP,aBestNorm); aCoeff[0] = scal(aP,aB); aCoeff[1] = scal(aP,aC); double aPds = 1 / (1+ElSquare(aCste/aSomMin)); aSysLin2.GSSR_AddNewEquation(aPds,aCoeff,-aCste,0); aSomE += aPds * ElSquare(aCste); aSomP += aPds; } Im1D_REAL8 aSol = aSysLin2.GSSR_Solve (0); double * aData = aSol.data(); aBestNorm = vunit(aBestNorm+aB*aData[0] + aC*aData[1]); // std::cout << "RESIDU " << sqrt(aSomE/aSomP) << "\n"; } if (mDoPly) { Pt3di aBleu(0,0,255); int aNb=1000; for (int aK=-aNb ; aK<= aNb ; aK++) { Pt3dr aN = aBestNorm * ((aK*1.2) / aNb); // std::cout << "AXE " << aN << "\n"; mPtsPly.push_back(aN); mColPly.push_back(aBleu); } Pt3dr aB,aC; MakeRONWith1Vect(aBestNorm,aB,aC); aNb=30; Pt3di aCyan(0,255,255); for (int aKb=-aNb ; aKb<= aNb ; aKb++) { for (int aKc=-aNb ; aKc<= aNb ; aKc++) { // Pt3dr aP = aB * (aKb*1.1)/aNb + aC * (aKc*1.1)/aNb; // mPtsPly.push_back(aP); // mColPly.push_back(aCyan); } } } return aBestNorm; }
void cOriFromBundle::TestTeta(double aT1,int aSign0,int aSign1) { static int aCpt =0 ; aCpt++; // std::cout << "Test Teta " << aCpt << " " << mVSols.size() << "\n"; bool Bug = false;// (aCpt==67); cOFB_Sol1T aRes; aRes.mTeta = aT1; // L'image de mDir1B par la rot est dans le plan |_ a mBase et mDir1A donc // dans mX12 mY1 double aC1 = cos(aT1); double aS1 = sin(aT1); Pt3dr aV1 = mX12 * aC1 + mY1 * aS1; // std::cout << aV1 << "\n"; // On V2 = mX12 cos(T2) + mY2 sin (T2) et V2.V1 = mSc12 par conservation // V2 = mX12 C2 + (mCosY2 mY1 + mSinY2 m Z1) S2 // V1.V2 = C1 C2 + S1 S2 mCosY2 = mSc12 = cos(Teta12) double aSP1 = aS1 * mCosY2; double aNorm = sqrt(ElSquare(aC1) + ElSquare(aSP1)); aRes.mNorm = aNorm; if (Bug) { std::cout << "N " << aNorm << " S " << mSc12 << " Rrr= " << aNorm/(ElAbs(mSc12)) -1 << "\n"; } if ((aNorm> 1e-15 ) && (ElAbs(mSc12)<=(aNorm*0.999999)) ) { aRes.mOK = true; double aA3 = atan2(aSP1/aNorm,aC1/aNorm); double aTeta12 = acos(mSc12/aNorm); if (Bug) { std::cout << "N " << aNorm << " S " << mSc12 << " T12 " << aTeta12 << " A " << aA3 << "\n"; } // V1.V2 /aNorm = cos(T2-A3) = Sc12/ Norm = cos(Teta12) => T2 = A3 +/- Teta12 for (int aK=aSign0 ; aK<aSign1 ; aK++) { int aS = (aK==0) ? -1 : 1 ; aRes.mSols[aK].mSign = aS; double aT2 = aA3 + aS * aTeta12; double aC2 = cos(aT2); double aS2 = sin(aT2); Pt3dr aV2 = mX12 * aC2 + mY2 * aS2; Pt3dr aZ = vunit(aV1^aV2); Pt3dr aY = vunit(aZ^aV1); Pt3dr aV3 = aV1*mSc3X + aY*mSc3Y + aZ*mSc3Z ; double aS3 = scal(aV3,mDirOr3A); aRes.mSols[aK].mScal = aS3; aRes.mSols[aK].mV1 = aV1; aRes.mSols[aK].mV2 = aV2; aRes.mSols[aK].mV3 = aV3; if (Bug) std::cout << "TTtttt " << aV1 << aV2 << aV3 << "\n"; } } else { aRes.mOK = false; } mVSols.push_back(aRes); }
void cNewO_OrInit2Im::CalcSegAmbig() { mDirAmbig = vunit(mIA ^ mBestSol.tr()); // Le vecteur |_ au plan (0 , Base, Inter) mSegAmbig = ElSeg3D(mIA,mIA+mDirAmbig); }
static Triangle tunit(const Triangle t) { const Triangle u = { vunit(t.a), vunit(t.b), vunit(t.c) }; return u; }
cOriFromBundle::cOriFromBundle ( Pt3dr aBase, Pt3dr aDir1A, Pt3dr aDir2A, Pt3dr aDir3A, Pt3dr aDir1B, Pt3dr aDir2B, Pt3dr aDir3B ) : mBase (vunit(aBase)) , mDir1A (vunit(aDir1A)), mDir2A (vunit(aDir2A)), mDir3A (vunit(aDir3A)), mDirOr1A (vunit(mBase^mDir1A)), mDirOr2A (vunit(mBase^mDir2A)), mDirOr3A (vunit(mBase^mDir3A)), mDir1B (vunit(aDir1B)), mDir2B (vunit(aDir2B)), mDir3B (vunit(aDir3B)), mSc12 (scal(mDir1B,mDir2B)), mZB (vunit(mDir1B^mDir2B)), mYB (vunit(mZB^mDir1B)), mSc3X (scal(mDir3B,mDir1B)), mSc3Y (scal(mDir3B,mYB)), mSc3Z (scal(mDir3B,mZB)), mX12 (vunit(mDirOr1A^mDirOr2A)), mY1 (vunit(mDirOr1A^mX12)), mZ1 (mX12 ^ mY1), mY2 (vunit(mDirOr2A^mX12)), mCosY2 (scal(mY2,mY1)), mSinY2 (scal(mY2,mZ1)) { }
void correctPlanarPolygon( vector<Pt3dr> &aPolygon ) { if (aPolygon.size() < 3) ELISE_ERROR_EXIT("aPolygon.size() = " << aPolygon.size() << " < 3"); Pt3dr u = vunit(aPolygon[1] - aPolygon[0]), minV = vunit(aPolygon[2] - aPolygon[0]); size_t minI = 2; REAL minScal = ElAbs(scal(u, minV)); for (size_t i = 3; i < aPolygon.size(); i++) { Pt3dr v = vunit(aPolygon[i] - aPolygon[0]); REAL d = ElAbs(scal(u, v)); if (d < minScal) { minScal = d; minI = i; minV = v; } } cout << "minI = " << minI << endl; cout << "minScal = " << minScal << endl; cout << "minV = " << minV << endl; Pt3dr n = u ^ minV; cout << "minV = " << minV << endl; ElMatrix<REAL> planToGlobalMatrix = MatFromCol(u, minV, n); if (planToGlobalMatrix.Det() < 1e-10) ELISE_ERROR_EXIT("matrix is not inversible"); ElRotation3D planToGlobalRot(aPolygon[0], planToGlobalMatrix, true); ElRotation3D globalToPlanRot = planToGlobalRot.inv(); //~ const size_t nbVertices = aPolygon.size(); //~ ostringstream ss; //~ static int ii = 0; //~ const REAL extrudSize = 1e4; //~ ss << "polygon_" << (ii++) << ".ply"; //~ ofstream f(ss.str().c_str()); //~ f << "ply" << endl; //~ f << "format ascii 1.0" << endl; //~ f << "element vertex " << 4 * nbVertices << endl; //~ f << "property float x" << endl; //~ f << "property float y" << endl; //~ f << "property float z" << endl; //~ f << "property uchar red" << endl; //~ f << "property uchar green" << endl; //~ f << "property uchar blue" << endl; //~ f << "element face " << nbVertices << endl; //~ f << "property list uchar int vertex_indices" << endl; //~ f << "end_header" << endl; REAL zDiffSum = 0.; for (size_t i = 0; i < aPolygon.size(); i++) { Pt3dr p = globalToPlanRot.ImAff(aPolygon[i]); zDiffSum += ElAbs(p.z); aPolygon[i] = planToGlobalRot.ImAff(Pt3dr(p.x, p.y, 0.)); //~ Pt3dr p0 = (i == 0) ? aPolygon[aPolygon.size() - 1] : aPolygon[i - 1], p1 = aPolygon[i], p2 = p1 + n * extrudSize, p3 = p0 + n * extrudSize; //~ f << p0.x << ' ' << p0.y << ' ' << p0.z << " 128 128 128" << endl; //~ f << p1.x << ' ' << p1.y << ' ' << p1.z << " 128 128 128" << endl; //~ f << p2.x << ' ' << p2.y << ' ' << p2.z << " 128 128 128" << endl; //~ f << p3.x << ' ' << p3.y << ' ' << p3.z << " 128 128 128" << endl; } //~ for (size_t i = 0; i < aPolygon.size(); i++) //~ f << 4 << ' ' << i * 4 << ' ' << i * 4 + 1 << ' ' << i * 4 + 2 << ' ' << i* 4 + 3 << endl; //~ f.close(); cout << "zDiffSum = " << zDiffSum << endl; }
Pt2dr EpipolaireCoordinate::DirEpip(Pt2dr aP,REAL anEpsilon) { return vunit(TransOnLineEpip(aP,anEpsilon)-aP); }
cNewO_CombineCple::cNewO_CombineCple(const cStructMergeTieP< cFixedSizeMergeTieP<2,Pt2dr> > & aMap,ElRotation3D * aTestSol) : mCurStep (1<<NbPow2), mNbStepTeta (4 * NbDecoup0PIS2 * mCurStep), mCurRot (3,3), mW (0) { // REDONDANT AVEC FONCTION GLOBALES FAITE APRES .... PackReduit /******************************************************/ /* */ /* A- Selection des sommets */ /* */ /******************************************************/ //------------------------------------------------------------------------ // A- 1- Preselrection purement aleatoire d'un nombre raisonnable depoints //------------------------------------------------------------------------ const std::list<tMerge *> & aLM = aMap.ListMerged(); RMat_Inertie aMat; { cRandNParmiQ aSelec(NbMaxInit, (int)aLM.size()); for (std::list<tMerge *>::const_iterator itM=aLM.begin() ; itM!=aLM.end() ; itM++) { if (aSelec.GetNext()) { mVAllCdt.push_back(cCdtCombTiep(*itM)); Pt2dr aP1 = (*itM)->GetVal(0); aMat.add_pt_en_place(aP1.x,aP1.y); } } } aMat = aMat.normalize(); int aNbSomTot = int(mVAllCdt.size()); double aSurfType = sqrt (aMat.s11()* aMat.s22() - ElSquare(aMat.s12())); double aDistType = sqrt(aSurfType/aNbSomTot); double aSzW = 800; if (1) { mP0W = aMap.ValInf(0); Pt2dr aP1 = aMap.ValSup(0); Pt2dr aSz = aP1-mP0W; mP0W = mP0W - aSz * 0.1; aP1 = aP1 + aSz * 0.1; aSz = aP1-mP0W; mScaleW = aSzW /ElMax(aSz.x,aSz.y) ; mW = Video_Win::PtrWStd(round_ni(aSz*mScaleW)); } //------------------------------------------------------------------------ // A-2 Calcul d'une fonction de deponderation //------------------------------------------------------------------------ for (int aKS1 = 0 ; aKS1 <aNbSomTot ; aKS1++) { for (int aKS2 = aKS1 ; aKS2 <aNbSomTot ; aKS2++) { // sqrt pour attenuer la ponderation double aDist = sqrt(dist48( mVAllCdt[aKS1].mP1-mVAllCdt[aKS2].mP1) / 2.0); // aDist=1; // double aDist = (dist48( mVAllCdt[aKS1].mP1-mVAllCdt[aKS2].mP1) / 2.0); double aPds = 1 / (aDistType+aDist); mVAllCdt[aKS1].mPdsOccup += aPds; mVAllCdt[aKS2].mPdsOccup += aPds; } if (mW) mW->draw_circle_abs(ToW( mVAllCdt[aKS1].mP1),2.0,mW->pdisc()(P8COL::blue)); } for (int aKSom = 0 ; aKSom <aNbSomTot ; aKSom++) { cCdtCombTiep & aCdt = mVAllCdt[aKSom]; aCdt.mPdsOccup *= ElSquare(aCdt.mMerge->NbArc()); } int aNbSomSel = ElMin(aNbSomTot,NbTieP); //------------------------------------------------------------------------ // A-3 Calcul de aNbSomSel points biens repartis //------------------------------------------------------------------------ ElTimer aChrono; for (int aKSel=0 ; aKSel<aNbSomSel ; aKSel++) { // Recherche du cdt le plus loin double aMaxDMin = 0; cCdtCombTiep * aBest = 0; for (int aKSom = 0 ; aKSom <aNbSomTot ; aKSom++) { cCdtCombTiep & aCdt = mVAllCdt[aKSom]; double aDist = aCdt.mDMin * aCdt.mPdsOccup; if ((!aCdt.mTaken) && (aDist > aMaxDMin)) { aMaxDMin = aDist; aBest = & aCdt; } } ELISE_ASSERT(aBest!=0,"cNewO_CombineCple"); for (int aKSom = 0 ; aKSom <aNbSomTot ; aKSom++) { cCdtCombTiep & aCdt = mVAllCdt[aKSom]; aCdt.mDMin = ElMin(aCdt.mDMin,dist48(aCdt.mP1-aBest->mP1)); } aBest->mQ1 = vunit(Pt3dr(aBest->mP1.x,aBest->mP1.y,1.0)); Pt2dr aP2 = aBest->mMerge->GetVal(1); aBest->mQ2Init = vunit(Pt3dr(aP2.x,aP2.y,1.0)); mVCdtSel.push_back(aBest); if (mW) mW->draw_circle_abs(ToW( aBest->mP1),3.0,mW->pdisc()(P8COL::red)); } /******************************************************/ /* */ /* B- Calcul des arcs */ /* */ /******************************************************/ // B-1 Au max le nombre d'arc possible int aNbA = NbCple; while (aNbA > ((aNbSomSel * (aNbSomSel-1)) /2)) aNbA--; int aNbIter = (aNbA-1) / aNbSomSel + 1; cRandNParmiQ aSelec(aNbA- (aNbIter-1) * aNbSomSel,aNbSomSel); int aNbAMaj = aNbIter * aNbSomSel; std::vector<int> aPermut = RandPermut(aNbA); // B-2 Recherche des arsc int aKA=0; for (int aCptAMaj = 0 ; aCptAMaj < aNbAMaj ; aCptAMaj++) { // Tous les sommets sont equi repartis, sauf a la fin on choisit a hasard bool aSelK = true; if ( (aCptAMaj/aNbSomSel)== (aNbIter-1)) // Si derniere iter, test special { aSelK = aSelec.GetNext(); } if (aSelK) { int aKP1 = (aCptAMaj%aNbSomSel); double aTeta = (aPermut[aKA] * 2 * PI) / aNbA; Pt2dr aDir = Pt2dr::FromPolar(1.0,aTeta); // std::cout << "teta " << aTeta << "\n"; double aBestSc=-1.0; int aBestK=-1; for (int aKP2 = 0 ; aKP2 < aNbSomSel ; aKP2++) { if (aKP2!=aKP1) { Pt2dr aV = (mVCdtSel[aKP2]->mP1- mVCdtSel[aKP1]->mP1) / aDir; Pt2dr aU = vunit(aV); // Favorise les llongs arc et homogeneise les directions double aSc = NRrandom3() * euclid(aV) * (1/(1+ElSquare(5.0*aU.y))); if ((aSc>aBestSc) && (aKP2!=aKP1)) { aBestSc= aSc; aBestK = aKP2; } } } ELISE_ASSERT((aBestK>=0),"No Best Arc"); mLArcs.push_back(Pt2di(aKP1,aBestK)); if (mW) { mW->draw_seg(ToW( mVCdtSel[aKP1]->mP1),ToW( mVCdtSel[aBestK]->mP1),mW->pdisc()(P8COL::green)); } aKA++; } } /******************************************************/ /* */ /* */ /* */ /******************************************************/ if (mW) mW->clik_in(); if (aTestSol) { ElRotation3D aR = * aTestSol; // Le sens corret a ete retabli (j'espere !!) // SetCurRot(aR.Mat()); // std::cout << "Test Externe : " << CalculCostCur() <<"\n"; // aR = aR.inv(); SetCurRot(aR.Mat()); std::cout << "Test Externe I : " << CalculCostCur() <<"\n"; std::cout << "CostBase " << CostOneBase(aR.tr()) << "\n"; // ElRotation3D * } std::cout << "cNewO_CombineCple::cNewO_CombineCple " << aNbSomTot << "\n"; Pt3di aP; std::list<Pt3di> aLPMin; double aCostMin = 1e10; Pt3di aPMin(1000,1000,1000); for (aP.x = -NbDecoup0PIS2 ; aP.x <= NbDecoup0PIS2 ; aP.x ++) { std::cout << "DECx " << aP.x << "\n"; for (aP.y = -NbDecoup0PIS2 ; aP.y <= NbDecoup0PIS2 ; aP.y ++) { for (aP.z = - (2*NbDecoup0PIS2) ; aP.z < (2*NbDecoup0PIS2) ; aP.z ++) { double aVC = GetCost(aP*mCurStep); bool IsMinLoc = (aVC < GetCost((aP+Pt3di( 1,0,0)) * mCurStep)) && (aVC < GetCost((aP+Pt3di(-1,0,0)) * mCurStep)) && (aVC < GetCost((aP+Pt3di(0, 1,0)) * mCurStep)) && (aVC < GetCost((aP+Pt3di(0,-1,0)) * mCurStep)) && (aVC < GetCost((aP+Pt3di(0,0, 1)) * mCurStep)) && (aVC < GetCost((aP+Pt3di(0,0,-1)) * mCurStep)); int aDelta = 2; for (int aDx=-aDelta ; (aDx<=aDelta) && IsMinLoc ; aDx++) { for (int aDy=-aDelta ; (aDy<=aDelta) && IsMinLoc ; aDy++) { for (int aDz=-aDelta ; (aDz<=aDelta) && IsMinLoc ; aDz++) { if ((aDx!=0) || (aDy!=0) || (aDz!=0)) { IsMinLoc = IsMinLoc && (aVC<GetCost( (aP+Pt3di(aDx,aDy,aDz))*mCurStep)); } } } } if (IsMinLoc) { std::cout << " IisssMinn " << aP << " " << aVC << "\n"; aLPMin.push_back(aP*mCurStep); } if (aVC<aCostMin) { aPMin = aP*mCurStep; aCostMin = aVC; } } } } std::cout << "COST " << aCostMin << " PMIN " << PInt2Tetas(aPMin ) << " NbMinLoc " << aLPMin.size() << "\n"; Pt3dr aTeta = PInt2Tetas(aPMin); ElMatrix<double> aR = ElMatrix<double>::Rotation(aTeta.z,aTeta.y,aTeta.x); for (int aY=0 ; aY<3 ; aY++) { for (int aX=0 ; aX<3 ; aX++) { std::cout << aR(aX,aY) << " "; } std::cout << "\n"; } /* std::cout << "Sssz " << aLPMin.size() << "\n"; if ( aLPMin.size()>0) std::cout << "PP00 " << *(aLPMin.begin()) << "\n"; */ }
//----------------------------------------------------------------------------- void RunGame() { // Control main ship if (g_gs == GS_VICTORY || g_gs == GS_STARTING) { if (MAIN_SHIP.vel.y < SHIP_CRUISE_SPEED) { MAIN_SHIP.vel.y = SAFEADD(MAIN_SHIP.vel.y, SHIP_INC_SPEED, SHIP_CRUISE_SPEED); } MAIN_SHIP.fuel = SAFESUB(MAIN_SHIP.fuel, FRAME_FUEL_COST); } // Heal main ship if (g_gs != GS_DYING) { if (MAIN_SHIP.energy < MAX_ENERGY && MAIN_SHIP.fuel > MIN_FUEL_FOR_HEAL) { MAIN_SHIP.energy = SAFEADD(MAIN_SHIP.energy, ENERGY_HEAL_PER_FRAME, MAX_ENERGY); MAIN_SHIP.fuel = SAFESUB(MAIN_SHIP.fuel, FUEL_HEAL_PER_FRAME); LOG(("- energy: %f, fuel: %f\n", MAIN_SHIP.energy, MAIN_SHIP.fuel)); } } // Move entities for (int i = MAX_ENTITIES - 1; i >= 0; i--) { if (g_entities[i].type != E_NULL) { g_entities[i].pos = vadd(g_entities[i].pos, g_entities[i].vel); // Remove entities that fell off screen if (g_entities[i].pos.y < g_camera_offset - G_HEIGHT) g_entities[i].type = E_NULL; } } // Advance "stars" for (int i = 0; i < MAX_ENTITIES; i++) { if (g_entities[i].type == E_STAR) g_entities[i].gfxscale *= 1.008f; } // Dont let steering off the screen if (MAIN_SHIP.pos.x < MAINSHIP_RADIUS) MAIN_SHIP.pos.x = MAINSHIP_RADIUS; if (MAIN_SHIP.pos.x > G_WIDTH - MAINSHIP_RADIUS) MAIN_SHIP.pos.x = G_WIDTH - MAINSHIP_RADIUS; // Check collisions if (g_gs == GS_PLAYING) { // Check everything against ship for (int i = 1; i < MAX_ENTITIES; i++) { // Should check against ship? if (g_entities[i].type == E_ROCK || g_entities[i].type == E_JUICE || g_entities[i].type == E_MINE || g_entities[i].type == E_DRONE) { float distance = vlen2(vsub(g_entities[i].pos, MAIN_SHIP.pos)); // Distance from object to ship float crash_distance = CORE_FSquare(g_entities[i].radius + MAIN_SHIP.radius); // Minimum allowed distance before crash if (distance < crash_distance) { switch (g_entities[i].type) { case E_ROCK: if (g_entities[i].energy > 0) { MAIN_SHIP.energy = SAFESUB(MAIN_SHIP.energy, ROCK_CRASH_ENERGY_LOSS); MAIN_SHIP.vel.y = SHIP_START_SPEED; // Set rock velocity vec2 vel_direction = vsub(g_entities[i].pos, MAIN_SHIP.pos); // direction of rock velocity, away from ship vec2 normalized_vel_direction = vunit(vel_direction); // normalize vec2 vel = vscale(normalized_vel_direction, CRASH_VEL); // Scale, ie give the rock correct speed. g_entities[i].vel = vel; g_entities[i].energy = 0; } break; case E_JUICE: MAIN_SHIP.fuel = SAFEADD(MAIN_SHIP.fuel, JUICE_FUEL, MAX_FUEL); g_entities[i].type = E_NULL; break; case E_MINE: MAIN_SHIP.energy = SAFESUB(MAIN_SHIP.energy, MINE_CRASH_ENERGY_LOSS); MAIN_SHIP.vel.y = SHIP_START_SPEED; g_entities[i].type = E_NULL; break; case E_DRONE: MAIN_SHIP.energy = SAFESUB(MAIN_SHIP.energy, MINE_CRASH_ENERGY_LOSS); MAIN_SHIP.vel.y = SHIP_START_SPEED; g_entities[i].type = E_NULL; break; default: break; } } } else if (g_entities[i].type == E_ROCKET) { // Check all hit-able objects against this rocket for (int j = 1; i < MAX_ENTITIES; j++) { // Should check against rocket? if (g_entities[j].type == E_ROCK || g_entities[j].type == E_MINE || g_entities[j].type == E_DRONE) { float distance = vlen2(vsub(g_entities[i].pos, g_entities[j].pos)); float crash_distance = CORE_FSquare(g_entities[i].radius + g_entities[j].radius); if (distance < crash_distance) { // Impact! g_entities[i].type = E_NULL; g_entities[j].type = E_NULL; break; } } } } } } // Generate new level elements as we advance GenNextElements(); // Possibly insert new juice if (g_gs == GS_PLAYING) { float trench = MAIN_SHIP.pos.y - g_current_race_pos; // How much advanced from previous frame if (CORE_RandChance(trench * JUICE_CHANCE_PER_PIXEL)) { vec2 pos = vmake(CORE_FRand(0.f, G_WIDTH), g_camera_offset + G_HEIGHT + GEN_IN_ADVANCE); // Random x, insert 400y above window vec2 vel = vmake(CORE_FRand(-1.f, +1.f), CORE_FRand(-1.f, +1.f)); // Random small velocity to make rocks "float" InsertEntity(E_JUICE, pos, vel, JUICE_RADIUS, g_juice, false, true); } } // Set camera to follow the main ship g_camera_offset = MAIN_SHIP.pos.y - G_HEIGHT / 8.f; g_current_race_pos = MAIN_SHIP.pos.y; if (g_gs == GS_PLAYING) { if (g_current_race_pos >= RACE_END) // Check if victory { g_gs = GS_VICTORY; g_gs_timer = 0.f; MAIN_SHIP.gfxadditive = true; } } // Advance game mode g_gs_timer += FRAMETIME; switch (g_gs) { case GS_STARTING: if (g_gs_timer >= STARTING_TIME) // Start delay before starting to play { g_gs = GS_PLAYING; g_gs_timer = 0.f; } break; case GS_DYING: if (g_gs_timer >= DYING_TIME) { ResetNewGame(); } break; case GS_PLAYING: if (MAIN_SHIP.energy <= 0.f || MAIN_SHIP.fuel <= 0.f) // No energy or fuel --> die { g_gs = GS_DYING; g_gs_timer = 0.f; MAIN_SHIP.gfx = g_ship_RR; } break; case GS_VICTORY: if (CORE_RandChance(1.f / 10.f)) { InsertEntity(E_STAR, MAIN_SHIP.pos, vadd(MAIN_SHIP.vel, vmake(CORE_FRand(-5.f, 5.f), CORE_FRand(-5.f, 5.f))), 0, g_star, false, true); } if (g_gs_timer >= 8.f) // Should use VICTORY_TIME, but stupid VS dont want me to... { ResetNewGame(); } break; } g_time_from_last_rocket += FRAMETIME; }