bool findIntersection(const Point& p1, const Point& p2, const Point& d1, const Point& d2, Point& intersection)
{
float pOffset = 0;
float pSlope = findSlope(p1, p2, pOffset);
float dOffset = 0;
float dSlope = findSlope(d1, d2, dOffset);
if (dSlope == pSlope)
return false;
if (pSlope == std::numeric_limits<float>::infinity()) {
intersection.setX(p1.x());
intersection.setY(dSlope * intersection.x() + dOffset);
return true;
}
if (dSlope == std::numeric_limits<float>::infinity()) {
intersection.setX(d1.x());
intersection.setY(pSlope * intersection.x() + pOffset);
return true;
}
// Find x at intersection, where ys overlap; x = (c' - c) / (m - m')
intersection.setX((dOffset - pOffset) / (pSlope - dSlope));
intersection.setY(pSlope * intersection.x() + pOffset);
return true;
}
double * findLine(waypoint currentPos, waypoint newPos, double * mb){ //mb needs to have space for two doubles
mb[0] = findSlope(currentPos, newPos);
mb[1] = currentPos.y - mb[0]*currentPos.x;
return mb;
}
double quickOptAllAntSteps(double *par, double *eventNumberIndex, double *thetaWaveIndex, double *phiWaveIndex, int *antIndex1, int *antIndex2, double *maxCorrTimeIndex, bool *adjacent){
fGeomTool->useKurtAnita3Numbers(1);
const int BIGNUMBER = 30000;//18500;
AnitaPol::AnitaPol_t pol = AnitaPol::kHorizontal;
// AnitaPol::AnitaPol_t pol = AnitaPol::kVertical;
Double_t sourceLat, sourceLon, sourceAlt, timeOffset;
char cpol[100];
if (pol == AnitaPol::kVertical){
sourceLat = - (79 + (27.93728/60));
sourceLon = -(112 + (6.74974/60));
sourceAlt = 1813.42;
timeOffset = + 92.8;
// timeOffset = -99756.6;
sprintf(cpol, "VPOL");
}else{
sourceLat = - (79 + (27.94097/60));
sourceLon = -(112 + (6.76208/60));
sourceAlt = 1819.62;
timeOffset = + 92.8;
sprintf(cpol, "HPOL");
}
Double_t deltaR[MAX_ANTENNAS]={0};
Double_t deltaZ[MAX_ANTENNAS]={0};
Double_t deltaPhi[MAX_ANTENNAS]={0};
Double_t deltaCableDelays[MAX_ANTENNAS]={0};
for(unsigned int i = 0; i<MAX_ANTENNAS;++i){
deltaR[i]=par[i];
deltaZ[i]=par[i+MAX_ANTENNAS];
deltaPhi[i]=par[i+MAX_ANTENNAS*2];
deltaCableDelays[i] = par[i+MAX_ANTENNAS*3];
// cout << "ant" << i << ": " << deltaR[i] << " " << deltaZ[i] << " " << deltaPhi[i] << " " << deltaCableDelays[i] << endl;
}
double **AdjacentAntDeltaT;
double **AdjacentAntPhiWave;
double **VerticalAntDeltaT;
double **VerticalAntPhiWave;
AdjacentAntDeltaT = new double*[48]; // Top ring 0-15, Middle ring 16-31, Bottom ring 32-47
AdjacentAntPhiWave = new double*[48];
VerticalAntDeltaT = new double*[48]; // Top-Middle ring 0-15, Middle-Bottom ring 16-31, Top-Bottom ring 32-47
VerticalAntPhiWave = new double*[48];
for (unsigned int i=0;i<48;++i){
AdjacentAntDeltaT[i] = new double[BIGNUMBER];
AdjacentAntPhiWave[i] = new double[BIGNUMBER];
VerticalAntDeltaT[i] = new double[BIGNUMBER];
VerticalAntPhiWave[i] = new double[BIGNUMBER];
}
Int_t countAdjacent[48]={0};
Int_t countVertical[48]={0};
Int_t countHorz[48]={0};
for (unsigned int ant=0;ant<MAX_ANTENNAS;++ant){
antPhi[ant] = fGeomTool->getAntPhiPositionRelToAftFore(ant, pol);
// meanPhi[ant] = fGeomTool->getAntPhiPosition(ant, pol);
}
Double_t additionalPhi = 22.5*TMath::DegToRad();
Double_t phiWave, thetaWave, deltaTExpected, maxCorrTime;
Float_t lower, upper;
Float_t TwoPi = TMath::Pi()*2;
unsigned int ant1, ant2, vert;
int arrayCount =0;
Int_t entry =0;
while(antIndex1[entry]!=-999) {
// std::cout << entry << "% \r" << flush;
thetaWave = thetaWaveIndex[entry];
phiWave = phiWaveIndex[entry];
maxCorrTime = maxCorrTimeIndex[entry];
ant1 = antIndex1[entry];
ant2 = antIndex2[entry];
// deltaTExpected=usefulPat.getDeltaTExpectedOpt(ant1, ant2, sourceLon, sourceLat, sourceAlt, deltaR, deltaZ, deltaPhi) + deltaCableDelays[ant1] - deltaCableDelays[ant2];
deltaTExpected = getDeltaTExpectedOpt(ant1, ant2, thetaWave, phiWave, deltaR, deltaZ, deltaPhi) + deltaCableDelays[ant1] - deltaCableDelays[ant2];
// if ((maxCorrTime-deltaTExpected)*(maxCorrTime-deltaTExpected)>1){
// entry++;
// continue;
// }
// lower = antPhi[ant1] - additionalPhi ;
// upper = antPhi[ant2] + additionalPhi ;
// if (lower<0) lower+=TwoPi;
// if (upper>TwoPi) upper-=TwoPi;
// if (lower>upper){
// if (phiWave<TwoPi*0.5) lower-=TwoPi;
// else upper+=TwoPi;
// }
// if (phiWave<lower || phiWave>upper){
// entry++;
// continue;
// }
if (adjacent[entry]){
AdjacentAntDeltaT[ant1][countAdjacent[ant1]] = (maxCorrTime - deltaTExpected);
AdjacentAntPhiWave[ant1][countAdjacent[ant1]] = (phiWave);
countAdjacent[ant1]++;
}else {
if (ant1<16){
if (ant2<32) vert = ant1;
else vert = ant2;
} else {
vert = ant1;
}
VerticalAntDeltaT[vert][countVertical[vert]] = (maxCorrTime - deltaTExpected);
VerticalAntPhiWave[vert][countVertical[vert]] = (phiWave);
countVertical[vert]++;
}
entry++;
}
Double_t sumMeanAdj = getMean(AdjacentAntDeltaT, countAdjacent)*10000;
Double_t sumMeanVert = getMean(VerticalAntDeltaT, countVertical)*10000;
Double_t sumRMSAdj = 0; //getRMS(AdjacentAntDeltaT, countAdjacent)*100;
Double_t sumRMSVert = 0;//getRMS(VerticalAntDeltaT, countVertical)*100;
Double_t sumGRADAdj = findSlope(AdjacentAntPhiWave, AdjacentAntDeltaT, countAdjacent)*100;
Double_t sumGRADVert = findSlope(VerticalAntPhiWave, VerticalAntDeltaT, countVertical)*100;
for(unsigned int ants = 0; ants < MAX_ANTENNAS; ++ants){
delete [] AdjacentAntPhiWave[ants];
delete [] AdjacentAntDeltaT[ants];
delete [] VerticalAntPhiWave[ants];
delete [] VerticalAntDeltaT[ants];
}
delete [] AdjacentAntDeltaT;
delete [] AdjacentAntPhiWave;
delete [] VerticalAntDeltaT;
delete [] VerticalAntPhiWave;
cout << sumMeanAdj << " " << sumRMSAdj << " " << sumGRADAdj << " " << sumMeanVert << " " << sumRMSVert << " " << sumGRADVert << endl;
return (sumMeanAdj+sumRMSAdj+sumGRADAdj+sumMeanVert+sumRMSVert+sumGRADVert);
}
