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