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;
}
