int CLeftView::ReadInodeData(dir *d1[],DWORD InodeNumber){
extern CLawApp theApp;
char *ReadInode=new char[theApp.BlockSize];
BOOL bResult;
DWORD size,NewOffset;
int DirCnt=0;
unsigned long BlockGNumber = ((InodeNumber - 1) / theApp.InodesPerGroup);
unsigned long InodeIndex = ((InodeNumber - 1) % theApp.InodesPerGroup);
theApp.num.quad = theApp.QuadOff + (theApp.GDesc[BlockGNumber]->bg_inode_table * theApp.BlockSize);
char* temp6=new char[sizeof(inode)];
int InBNumber = (InodeIndex) / (theApp.BlockSize / (sizeof(inode)));
int InIndex = (InodeIndex ) % (theApp.BlockSize / (sizeof(inode)));
theApp.num.quad += (InBNumber * theApp.BlockSize);
NewOffset = SetFilePointer(theApp.dsk,theApp.num.low,&(theApp.num.high),FILE_BEGIN);
bResult = ReadFile(
theApp.dsk, // handle of file to read
(LPVOID)ReadInode, // pointer to buffer that receives data
theApp.BlockSize, // number of bytes to read
&size, // pointer to number of bytes read
(LPOVERLAPPED)NULL // pointer to structure for data
); //workin readfile
DWORD e = ((InIndex)*(sizeof(inode))),t=0;
DWORD z = e + sizeof(inode);
for( ; e < z ; e++,t++){
temp6[t]=ReadInode[e];
}
Inode=(inode *)temp6;
DWORD Blocks2Read = ((Inode->i_blocks * 512) / theApp.BlockSize);
int Dirnum;
DWORD DirsPerBlock = (theApp.BlockSize / 32);
#define FirstLimit DirsPerBlock + 12
#define DoubleLimit (DirsPerBlock * DirsPerBlock) + FirstLimit
#define TripleLimit (DirsPerBlock * DirsPerBlock *DirsPerBlock ) + DoubleLimit
if(Blocks2Read <= 12)
Dirnum=Direct(d1,Blocks2Read);
else
if(Blocks2Read > 12 && Blocks2Read <= FirstLimit ){
flag = 3;
Direct(d1,12);
flag=1;
Dirnum=FirstIndirect(d1,Blocks2Read);
flag=0;
}
delete []ReadInode;
return Dirnum;
}
REAL cDbleGrid::Focale()
{
Pt2dr aPP = PP();
Pt2dr aP1 = Direct(aPP+Pt2dr(1,0));
Pt2dr aP2 = Direct(aPP+Pt2dr(-1,0));
return 2.0 / euclid(aP1,aP2);
}
Pt2dr EpipolaireCoordinate::TransOnLineEpip
(
Pt2dr aP,
REAL aParal
)
{
return Inverse(Direct(aP)+Pt2dr(aParal,0.0));
}
int CLeftView::FirstIndirect(dir *d1[],DWORD Blocks2Read ){
BlockNumbers=new DWORD[theApp.BlockSize/sizeof(DWORD)];
FillBlocks(Inode->i_block[12]);
flag = 1;
Blocks2Read-=13;
int DirCnt=Direct(d1,Blocks2Read);
flag=0;
delete []BlockNumbers;
return DirCnt;
}
/** Calculate full nonbonded energy with PME */
double Ewald_ParticleMesh::CalcEnergy(Frame const& frameIn, AtomMask const& maskIn, double& e_vdw)
{
t_total_.Start();
Matrix_3x3 ucell, recip;
double volume = frameIn.BoxCrd().ToRecip(ucell, recip);
double e_self = Self( volume );
double e_vdw_lr_correction;
pairList_.CreatePairList(frameIn, ucell, recip, maskIn);
// TODO make more efficient
int idx = 0;
coordsD_.clear();
for (AtomMask::const_iterator atm = maskIn.begin(); atm != maskIn.end(); ++atm, ++idx) {
const double* XYZ = frameIn.XYZ( *atm );
coordsD_.push_back( XYZ[0] );
coordsD_.push_back( XYZ[1] );
coordsD_.push_back( XYZ[2] );
}
// MapCoords(frameIn, ucell, recip, maskIn);
double e_recip = Recip_ParticleMesh( frameIn.BoxCrd() );
// TODO branch
double e_vdw6self, e_vdw6recip;
if (lw_coeff_ > 0.0) {
e_vdw6self = Self6();
e_vdw6recip = LJ_Recip_ParticleMesh( frameIn.BoxCrd() );
if (debug_ > 0) {
mprintf("DEBUG: e_vdw6self = %16.8f\n", e_vdw6self);
mprintf("DEBUG: Evdwrecip = %16.8f\n", e_vdw6recip);
}
e_vdw_lr_correction = 0.0;
} else {
e_vdw6self = 0.0;
e_vdw6recip = 0.0;
e_vdw_lr_correction = Vdw_Correction( volume );
}
e_vdw = 0.0;
double e_direct = Direct( pairList_, e_vdw );
if (debug_ > 0)
mprintf("DEBUG: Eself= %20.10f Erecip= %20.10f Edirect= %20.10f Evdw= %20.10f\n",
e_self, e_recip, e_direct, e_vdw);
e_vdw += (e_vdw_lr_correction + e_vdw6self + e_vdw6recip);
t_total_.Stop();
return e_self + e_recip + e_direct;
}
Box2dr EpipolaireCoordinate::ImageOfBox(Box2dr aBoxIm)
{
Pt2dr Corners[4];
REAL xEpi[4];
REAL yEpi[4];
aBoxIm.Corners(Corners);
for (INT aK=0 ; aK<4 ; aK++)
{
Pt2dr aPEpi = Direct(Corners[aK]);
xEpi[aK] = aPEpi.x;
yEpi[aK] = aPEpi.y;
}
return Box2dr(xEpi,yEpi,4);
}
/** Calculate Ewald energy.
* Slow version that does not use pair list. Note that pair list is still
* called since we require the fractional and imaged coords.
* FIXME No Eadjust calc.
*/
double Ewald::CalcEnergy_NoPairList(Frame const& frameIn, Topology const& topIn,
AtomMask const& maskIn)
{
t_total_.Start();
Matrix_3x3 ucell, recip;
double volume = frameIn.BoxCrd().ToRecip(ucell, recip);
double e_self = Self( volume );
// Place atoms in pairlist. This calcs frac/imaged coords.
pairList_.CreatePairList(frameIn, ucell, recip, maskIn);
double e_recip = Recip_Regular( recip, volume );
double e_direct = Direct( ucell, topIn, maskIn );
//mprintf("DEBUG: Eself= %20.10f Erecip= %20.10f Edirect= %20.10f\n",
// e_self, e_recip, e_direct);
t_total_.Stop();
return e_self + e_recip + e_direct;
}
void HoGExtractor::Preprocess(const cv::Mat& img) {
uchar *data = img.data;
int height = img.size().height;
int width = img.size().width;
int step = img.step1();
memset((void *)integrals, 0, sizeof(feat) * widthStep);
memset((void *)bins, 0, sizeof(feat) * widthStep);
for (int i = 1; i < height - 1; i++) {
for (int j = 1; j < width - 1; j++) {
feat tempX = (feat)data[(i - 1)*step + (j + 1)] - (feat)data[(i - 1)*step + (j - 1)]
+ 2 * (feat)data[(i)*step + (j + 1)] - 2 * (feat)data[(i)*step + (j - 1)]
+ (feat)data[(i + 1)*step + (j + 1)] - (feat)data[(i + 1)*step + (j - 1)];
feat tempY = (feat)data[(i + 1)*step + (j - 1)] - (feat)data[(i - 1)*step + (j - 1)]
+ 2 * (feat)data[(i + 1)*step + (j)] - 2 * (feat)data[(i - 1)*step + (j)]
+ (feat)data[(i + 1)*step + (j + 1)] - (feat)data[(i - 1)*step + (j + 1)];
int direction = Direct(tempX, tempY);
feat temp = SQRT(tempX*tempX + tempY*tempY);
int index = (i - 1) * widthStep + (j - 1) * 9;
if (i == 1) {
bins[index + direction] = temp;
}
else {
memcpy((void *)(bins + index), (void *)(bins + index - widthStep), sizeof(feat) * 9);
bins[index + direction] += temp;
}
memcpy((void *)(integrals + index), (void *)(bins + index), sizeof(feat) * 9);
if (j > 1) {
for (int k = 0; k < 9; k++) {
integrals[index + k] += integrals[index + k - 9];
}
}
}
}
}
void *MainWindow::CMDGetterEntry(void *a)
{
MainWindow* main = (MainWindow*)a;
CMDDATA cmddata;
long long int tstamp;
long long int interval;
while(1){
// get commands from ROS.
Getter(cmddata);
// get time in milliseconds.
tstamp = (long long int) getTime();
// update robocar state.
Update(main);
// set mode and gear.
SetState(cmddata.mode, cmddata.gear, main);
if (!mode_is_setting && !gear_is_setting) {
#ifdef DIRECT_CONTROL
// directly set accel, brake, and steer.
Direct(cmddata.accel, cmddata.brake, cmddata.steer, main);
#else
// control accel, brake, and steer.
Control(cmddata.vel, main);
#endif
}
// get interval in milliseconds.
interval = cmd_rx_interval - (getTime() - tstamp);
if (interval > 0) {
cout << "sleeping for " << interval << "ms" << endl;
usleep(interval * 1000); // not really real-time...
}
}
return NULL;
}
void cModeleAnalytiqueComp::SolveHomographie(const ElPackHomologue & aPackHom)
{
mHomogr = cElHomographie(aPackHom,mModele.HomographieL2().Val());
mBoxPoly = Box2dr(Pt2dr(0,0),Pt2dr(0,0));
if (mDegrPolAdd >0)
{
ElPackHomologue aPck2 = aPackHom;
Pt2dr aPMin(1e5,1e5);
Pt2dr aPMax(-1e5,-1e5);
for
(
ElPackHomologue::iterator iT = aPck2.begin();
iT != aPck2.end();
iT++
)
{
iT->P1() = mHomogr.Direct(iT->P1());
aPMin.SetInf(iT->P1());
aPMax.SetSup(iT->P1());
}
double anAmpl = ElMax(dist8(aPMin),dist8(aPMax));
mBoxPoly = Box2dr(aPMin,aPMax);
bool aPL2 = mModele.PolynomeL2().Val();
mPolX = aPck2.FitPolynome(aPL2,mDegrPolAdd,anAmpl,true);
mPolY = aPck2.FitPolynome(aPL2,mDegrPolAdd,anAmpl,false);
}
// Polynome2dReal ElPackHomologue::FitPolynome
{
cElXMLFileIn aFileXML(mNameXML);
aFileXML.PutElHomographie(mHomogr,"Homographie");
if (mDegrPolAdd >0)
{
cElXMLFileIn::cTag aTag(aFileXML,"PolynomeCompl"); aTag.NoOp();
aFileXML.PutPoly(mPolX,"XPoly");
aFileXML.PutPoly(mPolY,"YPoly");
aFileXML.PutPt2dr(mBoxPoly._p0,"PMinBox");
aFileXML.PutPt2dr(mBoxPoly._p1,"PMaxBox");
}
}
MakeInverseModele();
// Verification de la correction du calcul de l'inverse
if (0)
{
int aNb=10;
double aEps = 0.05;
for (int aKx=0 ; aKx<= aNb ; aKx++)
{
double aPdsX = ElMax(aEps,ElMin(1-aEps,aKx/double(aNb)));
for (int aKy=0 ; aKy<= aNb ; aKy++)
{
double aPdsY = ElMax(aEps,ElMin(1-aEps,aKy/double(aNb)));
Pt2dr aPRas = Pt2dr (mSzGl.x*aPdsX, mSzGl.x*aPdsY);
Pt2dr aPTer = mGeoTer.RDiscToR2(aPRas);
Pt2dr aP1 = Direct(Pt2dr(aPTer));
Pt2dr aQ2 = Inverse(aP1);
double anEr = euclid(aPTer,aQ2);
if (anEr>0.05)
{
std::cout << "Erreur = " << anEr << "\n";
std::cout << aPRas << aPTer << "\n";
Pt2dr aP0 = aPTer;
Pt2dr aP1 = mGeom2.CorrigeDist1(aP0);
Pt2dr aP2 = CorrecDirecte(aP1);
Pt2dr aP3 = mGeom2.InvCorrDist2(aP2);
Pt2dr aQ0 = mGeom2.InvCorrDist1(aP1);
std::cout << "pq0= " << euclid(aP0,aQ0) << aP0 << aQ0 << "\n";
Pt2dr aQ1 = CorrecInverse(aP2);
std::cout << "pq1= " << euclid(aP1,aQ1) << aP1 << aQ1 << "\n";
Pt2dr aQ2 = mGeom2.CorrigeDist2(aP3);
std::cout << "pq2= " << euclid(aP2,aQ2) << aP2 << aQ2 << "\n";
std::cout << "P3 " << aP3 << "\n";
ELISE_ASSERT(false,"MakeInverseModele Pb!!");
}
}
}
}
if (mModele.ExportImage().Val() || mModele.ReuseResiduelle().Val() )
{
Im2D_REAL4 anImX(mSzGl.x,mSzGl.y);
TIm2D<REAL4,REAL8> aTImX(anImX);
Im2D_REAL4 anImY(mSzGl.x,mSzGl.y);
TIm2D<REAL4,REAL8> aTImY(anImY);
Pt2di aPRas;
double aPX0[2] = {0.0,0.0};
for (aPRas.x=0 ; aPRas.x<mSzGl.x ; aPRas.x++)
{
for (aPRas.y=0 ; aPRas.y<mSzGl.y ; aPRas.y++)
{
Pt2dr aPTer = mGeoTer.DiscToR2(aPRas);
Pt2dr aP1 = Direct(Pt2dr(aPTer));
Pt2dr aP2 ;
if (mModele.ReuseResiduelle().Val())
aP2 = mGeom2.Objet2ImageInit_Euclid(Pt2dr(aPTer),aPX0);
else
aP2 = mGeom2.InvCorrDist2(mGeom2.CorrigeDist1(aPTer));
double aPx[2];
aPx[0] = aP1.x- aP2.x;
aPx[1] = aP1.y- aP2.y;
mGeoTer.PxReel2PxDisc(aPx,aPx);
aTImX.oset(aPRas,aPx[0]);
aTImY.oset(aPRas,aPx[1]);
}
}
if (mModele.ExportImage().Val())
{
Tiff_Im::Create8BFromFonc(mNameImX,mSzGl,ToUC(anImX.in()));
Tiff_Im::Create8BFromFonc(mNameImY,mSzGl,ToUC(anImY.in()));
}
if (mModele.ReuseResiduelle().Val())
{
Tiff_Im::Create8BFromFonc(mNameResX,mSzGl,ToUC(anImX.in()-ImPx(0)));
Tiff_Im::Create8BFromFonc(mNameResY,mSzGl,ToUC(anImY.in()-ImPx(1)));
}
}
if (mExpModeleGlobal)
{
const cGeomDiscFPx & aG = mAppli.GeomDFPxInit() ;
Box2dr aBox = aG.BoxEngl();
double aME =mModele.MailleExport().Val();
cDbleGrid aGrid
(
true, // P0P1 Direct par defaut maintien du comp actuel
true,
aBox._p0,aBox._p1,
Pt2dr(aME,aME),
*this,
"toto"
);
std::string aNameXML = mAppli.ICNM()->Assoc1To2
(
mModele.FCND_ExportModeleGlobal().Val(),
mAppli.PDV1()->Name(),
mAppli.PDV2()->Name(),
true
);
aGrid.SaveXML(mAppli.FullDirResult() + aNameXML);
}
}
void cElHomographie::AddForInverse(ElPackHomologue & aPack,Pt2dr aP) const
{
aPack.Cple_Add(ElCplePtsHomologues(Direct(aP),aP,1.0));
}