// COCOA class implementation file //Id: OptOCops.cc //CAT: Model // // History: v1.0 // Pedro Arce #include "OptOCOPS.h" #include "LightRay.h" #include "Measurement.h" #include "Model.h" #include "DeviationsFromFileSensor2D.h" #include "ALIVRMLMgr.h" #include "ALILine.h" #include "ALIPlane.h" #include #include #include #include // I have added a new library for isnan() function //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ //@@ Make measurement as distance to previous object 'screen' //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ void OptOCOPS::defaultBehaviour( LightRay& lightray, Measurement& meas ) { if(Utils::debug >= 5) cout << "***** OptOCOPS::defaultBehaviour" <= 4) cout << "***** OptOCOPS::makeMeasurement(lightray, meas) " << endl; //---------- Centre of COPS is at dowel point 2 Hep3Vector dowel2 = centreGlob(); //---------- Coordinates of dowel point 1 are given with respect to dowel point 2 (in local reference frame) ALIdouble posx12 = findExtraEntryValue("dowel1X"); // Changed default value to .045 from .03 if(posx12 == 0. ) posx12 = 0.045; Hep3Vector dowel1(posx12,findExtraEntryValue("dowel1Y"), 0.); HepRotation rmt = rmGlob(); dowel1 = rmt*dowel1; dowel1 += dowel2; if (Utils::debug >= 3) { Utils::dump3v(dowel2, " dowel2"); Utils::dump3v(dowel1, " dowel1"); } //---------- Get line joining dowel1-dowel2 and perpendicular to it inside cops Hep3Vector line_dowel21 = - (dowel1-dowel2 ); ////// Hep3Vector ZAxis(0.,0,1.); ZAxis = rmt * ZAxis; Hep3Vector line_dowel21_perp = ZAxis.cross( line_dowel21 ); if (Utils::debug >= 3) { Utils::dump3v(line_dowel21," line_dowel21"); Utils::dump3v(line_dowel21_perp," line_dowel21_perp"); } //---------- Position four CCDs (locally, i.e. with respect to centre) //----- Get first CCD length, that will be used to give a default placement to the CCDs ALIdouble CCDlength = findExtraEntryValue("CCDlength"); if( CCDlength == 0. ) CCDlength = 2048*14 * 1.E-6; // (in meters, the default unit) // global / local output of ccd location in RF was reversed, I am swapping //----- Upper CCD (leftmost point & direction dowel1-dowel2) if(Utils::debug>= 3) cout << endl << "***** UP CCD *****" << endl << "******************" << endl << endl; ALIdouble posX = findExtraEntryValue("upCCDXtoDowel2"); ALIdouble posY; ALIbool eexists = findExtraEntryValueIfExists("upCCDYtoDowel2", posY); if(!eexists) posY = CCDlength + 0.004; Hep3Vector posxy( posX, posY, 0); if(Utils::debug>= 3) cout << " %%%% CCD distances to Dowel2: " << endl; if(Utils::debug>= 3) cout << " up ccd in local RF " << posxy << endl; posxy = rmt * posxy; if(Utils::debug>= 3) cout << " up ccd in global RF " << posxy << endl; ALILine upCCD( dowel2 + posxy, -line_dowel21 ); ccds[0] = ALILine( posxy, -line_dowel21 ); //----- Lower CCD (leftmost point & direction dowel2-dowel1) if(Utils::debug>= 3) cout << endl << "***** DOWN CCD *****" << endl << "********************" << endl << endl ; posX = findExtraEntryValue("downCCDXtoDowel2"); eexists = findExtraEntryValueIfExists("downCCDYtoDowel2", posY); if(!eexists) posY = 0.002; posxy = Hep3Vector( posX, posY, 0); if(Utils::debug>= 3) cout << " down ccd in local RF " << posxy << endl; posxy = rmt * posxy; if(Utils::debug>= 3) cout << " down ccd in global RF " << posxy << endl; ALILine downCCD( dowel2 + posxy, -line_dowel21 ); ccds[1] = ALILine( posxy, -line_dowel21 ); //----- left CCD (uppermost point & direction perpendicular to dowel2-dowel1) if(Utils::debug>= 3) cout << endl << "***** LEFT CCD *****" << endl << "********************" << endl << endl; eexists = findExtraEntryValueIfExists("leftCCDXtoDowel2", posX); if(!eexists) posX = -0.002; posY = findExtraEntryValue("leftCCDYtoDowel2"); posxy = Hep3Vector( posX, posY, 0); if(Utils::debug>= 3) cout << " left ccd in local RF " << posxy << endl; posxy = rmt * posxy; if(Utils::debug>= 3) cout << " left ccd in global RF " << posxy << endl; ALILine leftCCD( dowel2 + posxy, line_dowel21_perp ); ccds[2] = ALILine( posxy, line_dowel21_perp ); //----- right CCD (uppermost point & direction perpendicular to dowel2-dowel1) if(Utils::debug>= 3) cout << endl << "***** RIGHT CCD *****" << endl << "*********************" << endl<< endl ; eexists = findExtraEntryValueIfExists("rightCCDXtoDowel2", posX); if(!eexists) posX = -CCDlength - 0.004; posY = findExtraEntryValue("rightCCDYtoDowel2"); posxy = Hep3Vector( posX, posY, 0); if(Utils::debug>= 3) cout << " right ccd in local RF " << posxy << endl; posxy = rmt * posxy; if(Utils::debug>= 3) cout << " right ccd in global RF " << posxy << endl << endl; ALILine rightCCD( dowel2 + posxy, line_dowel21_perp ); ccds[3] = ALILine( posxy, line_dowel21_perp ); if (Utils::debug >= 3) { cout << " %%% Positions of CCDs in global RF: " << endl<< endl; cout << " upCCD: " << upCCD << endl; cout << " downCCD: " << downCCD << endl; cout << " leftCCD: " << leftCCD << endl; cout << " rightCCD: " << rightCCD << endl << endl; } //---------- Intersect x-hair laser with COPS if (Utils::debug >= 3) cout << " %%% Intersecting x-hair laser with COPS: " << endl; ALIPlane copsPlane(centreGlob(), ZAxis); lightray.intersect( *this ); Hep3Vector inters = lightray.point(); if (Utils::debug >= 3) { Utils::dump3v(inters, " Intersection of x-hair laser with COPS "); } //---------- Get cross of x-hair laser: if (Utils::debug >= 5) cout << "1. Get the OptO x-hair laser from the measurement list of OptOs" << endl; OpticalObject* xhairOptO = *(meas.OptOList().begin()); if (Utils::debug >= 35) cout << "2. Get the Y of the laser and project it on the COPS" << endl; Hep3Vector YAxis_xhair(0.,1.,0.); HepRotation rmtx = xhairOptO->rmGlob(); YAxis_xhair = rmtx * YAxis_xhair; ALILine Yline_xhair( inters, copsPlane.project( YAxis_xhair )); if (Utils::debug >= 3) { cout << " %%%% Projecting x-hair laser on COPS: " << endl; Utils::dump3v(YAxis_xhair, " Y direction of laser "); cout << " Y line of laser projected on COPS " << Yline_xhair << endl; } if (Utils::debug >= 5) cout << " 3. Get the X of the laser (correct it if cross is not 90o) and project it on the COPS" << endl; ALIdouble anglebx; eexists = xhairOptO->findExtraEntryValueIfExists("angleBetweenAxis", anglebx); if(!eexists) anglebx = PI/2.; Hep3Vector XAxis_xhair = YAxis_xhair; // if (Utils::debug >= 3) Utils::dump3v(XAxis_xhair," X of laser1 "); ZAxis = Hep3Vector(0.,0.,1.); ZAxis = rmtx * ZAxis; XAxis_xhair.rotate(anglebx, ZAxis ); ALILine Xline_xhair( inters, copsPlane.project( XAxis_xhair ) ); if (Utils::debug >= 3) { cout << "angleBetweenAxis = " << anglebx << endl; Utils::dump3v(XAxis_xhair," X direction of laser "); cout << " X line of laser projected on COPS " << Xline_xhair << endl; } //---------- Get measurement as intersection with four CCDs if(Utils::debug >= 3) cout << " Getting measurements as intersection with four CCDs: " << endl; if(Utils::debug >= 4)cout << "intersecting with upCCD " << endl; ALIdouble measv[4][2]; // swap Y and X line_xhair by exchanging second index if(Utils::debug >= 5)cout << "$@S@ measv[0][0] upccd " << endl; measv[0][0] = getMeasFromInters( Yline_xhair, upCCD, line_dowel21 ); if(Utils::debug >= 5)cout << "$@$@ measv[0][1] upccd " << endl; measv[0][1] = getMeasFromInters( Xline_xhair, upCCD, line_dowel21 ); //---- check if postive or negative: if(Utils::debug >= 4) cout << "intersecting with downCCD " << endl; measv[1][0] = getMeasFromInters(Yline_xhair, downCCD, line_dowel21 ); measv[1][1] = getMeasFromInters(Xline_xhair, downCCD, line_dowel21 ); // if(Utils::debug >= 4) cout << "intersecting with leftCCD " << endl; measv[2][0] = getMeasFromInters(Xline_xhair, leftCCD, line_dowel21_perp ); measv[2][1] = getMeasFromInters(Yline_xhair, leftCCD, line_dowel21_perp ); if(Utils::debug >= 4) cout << "intersecting with rightCCD " << endl; measv[3][0] = getMeasFromInters(Xline_xhair, rightCCD, line_dowel21_perp ); measv[3][1] = getMeasFromInters(Yline_xhair, rightCCD, line_dowel21_perp ); /* Both X and Y axis of the x-laser are intersected with each CCD and it checks that one of the two is inside the CCD(less than CCDlength/2). If no one is inside, it will give an exception. If both are inside (a strange case where, for example, the laser centre is very close and forms 45 degrees with the CCD) it will also make an exception (if you prefer, I can put a warning saying that you have two measurements, but I guess this should never happen for you, so I better give an exception and you don't risk to overpass this warning). Then it is mandatory that you put the CCDlength parameter (I could put a default one if you prefer). ALIbool measInCCD[2]; for( uint ii = 0; ii < 4; ii++ ) { for( uint jj = 0; jj < 2; jj++ ) { measInCCD[jj] = fabs( measv[ii][jj] ) < CCDlength/2; } if (Utils::debug >= 2) cout << "$@$@ CHECK CCD = " << ii << endl; if( measInCCD[0] && measInCCD[1] ){ cerr << "!!!EXITING: both lasers lines of x-hair laser intersect with same CCD " << measNames[ii] << " one at " << measv[ii][0] << " another one at " << measv[ii][1] << "CCDhalfLegth " << CCDlength/2 << endl; exit(1); } else if( !(measInCCD[0] || measInCCD[1]) ){ cerr << "!!!EXITING: none of the lasers lines of x-hair laser intersect with CCD " << measNames[ii] << ", one at " << measv[ii][0] << " another one at " << measv[ii][1] << "CCDhalfLegth " << CCDlength/2 << endl; exit(1); } else { measInCCD[0] ? meas.setValueSimulated( ii, measv[ii][0] ) : meas.setValueSimulated( ii, measv[ii][1] ); } } */ string measNames[4] ={"up","down","left","right"}; ALIbool laserLine; if (Utils::debug >= 2) cout << endl << "--> Now comparing measurement in ccds by x and y laser lines (will always choose the smaller one) " <= 2) cout << "\tmeas CCD " << measNames[ii] << "\t Values: Ylaser meas: " << fabs( measv[ii][0] ) << " (isnan "<< isnan(measv[ii][0]) << ") Xlaser meas: " << fabs( measv[ii][1] ) << " (isnan " << isnan(measv[ii][1]) << ")" << endl; if( meas.xlaserLine( ii ) != -1 ) { laserLine = ALIbool( meas.xlaserLine( ii ) ); } else { // Problem here !!! // // Somehow measv[][1] can occasionally return value of 'nan' // which is interpretted as less than any real value // if(isnan(measv[ii][1]) != 0){ if (Utils::debug >= 2) cout << " --> Swapping measv["<= 2) cout << measv[ii][1] << endl; } // Added Aug 24 2001 by rl if(isnan(measv[ii][0]) != 0){ if (Utils::debug >= 2) cout << " --> Swapping measv["<< ii<<"][0] value of " << measv[ii][0] << " to " ; measv[ii][1] = 1e9; if (Utils::debug >= 2) cout << measv[ii][0] << endl; } laserLine = fabs( measv[ii][0] ) < fabs( measv[ii][1] ); meas.setXlaserLine( ii, int(laserLine) ); } laserLine ? meas.setValueSimulated( ii, measv[ii][0] ) : meas.setValueSimulated( ii, measv[ii][1] ); } if (Utils::debug >= 2) cout << endl; //Keep format of debug output reasonable // try to identify pathological cases: up and down CCD are intersected by the same // laser line (say X) and the same for the left and right CCD if(Utils::debug >= 2) cout << "***** OptOCOPS::makeMeasurement - identify pathological cases U and D intersected by same line" <= 5) cout << "***** OptOCOPS::makeMeasurement - now output sim values" << endl; if (Utils::debug >= 1) { ALIstring chrg = ""; cout << "REAL value: " << chrg <<"U: " << 1000*meas.value()[0] << chrg << " D: " << 1000*meas.value()[1] << " L: " << 1000*meas.value()[2] << " R: " << 1000*meas.value()[3] << " (mm) " << (this)->name() << endl; ALIdouble detU = 1000*meas.valueSimulated(0); if(fabs(detU) <= 1.e-7 ) detU = 0.; ALIdouble detD = 1000*meas.valueSimulated(1); if(fabs(detD) <= 1.e-7 ) detD = 0.; ALIdouble detL = 1000*meas.valueSimulated(2); if(fabs(detL) <= 1.e-7 ) detL = 0.; ALIdouble detR = 1000*meas.valueSimulated(3); if(fabs(detR) <= 1.e-7 ) detR = 0.; cout << "SIMU value: " << chrg << "U: " // << setprecision(3) << setw(4) << detU << chrg << " D: " << detD << chrg << " L: " << detL << chrg << " R: " << detR << " (mm) " << (this)->name() << endl; } } //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ //@@ Fast simulation of Light Ray traverses //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ void OptOCOPS::fastTraversesLightRay( LightRay& lightray ) { if(Utils::debug >= 5) cout << "***** OptOCOPS::fastTraversesLightRay" <= 2) cout << "LR: FAST TRAVERSES COPS " << name() << endl; //---------- Get intersection Hep3Vector ZAxis(0.,0,1.); HepRotation rmt = rmGlob(); ZAxis = rmt * ZAxis; lightray.intersect( ALIPlane(centreGlob(), ZAxis) ); Hep3Vector inters = lightray.point(); lightray.setPoint( inters ); if (Utils::debug >= 2) { lightray.dumpData(" after COPS "); } } //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ //@@ //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ ALIdouble* OptOCOPS::convertPointToLocalCoordinates( const Hep3Vector& point) { if(Utils::debug >= 1) cout << "***** OptOCOPS::convertPointToLocalCoordinates" <= 5) Utils::dump3v( (this)->centreGlob(), "centre glob sensor2D" ); if( Utils::debug >= 5) Utils::dumprm( rmt, "rotation matrix sensor2D" ); //t Utils::dump3v(point - (this)->centreGlob() , "inters - (this)->centreGlob()"); //t Utils::dump3v(XAxism , "XAxism"); interslc[0] = (point - (this)->centreGlob() ) * XAxism; //----- Y value Hep3Vector YAxism(0.,1.,0.); YAxism*=rmt; //t Utils::dump3v(YAxism , "YAxism"); interslc[1] = (point - (this)->centreGlob() ) * YAxism; if( Utils::debug >=5 ) { cout << " intersection in local coordinates: X= " << interslc[0] << " Y= " << interslc[1] << endl; } return interslc; } void OptOCOPS::fillVRML() { ALIVRMLMgr& vrmlmgr = ALIVRMLMgr::getInstance(); //---------- Position four CCDs (locally, i.e. with respect to centre) //----- Get first CCD length, that will be used to give a default placement to the CCDs ALIdouble CCDlength = findExtraEntryValue("CCDlength"); if( CCDlength == 0. ) CCDlength = 2048*14 * 1.E-6; // (in meters, the default unit) //Original CCDdim value was 10; .05 0r .040 works well ALIdouble CCDdim = .80; // ALIdouble CCDdim = 2048*14 * 1.E-6; //Original was divided by 10 ALIdouble CCDwidth = CCDdim/10.; // ALIdouble CCDwidth = .005; //my change from .005 to .01 if(Utils::debug >= 4) cout << " ccds0 " << ccds[0] << "ccds1 " << ccds[1] << endl; ALIColour colup( 1.,0.,0., 0.); ALIColour coldown( 0.,1.,0., 0.); ALIColour colleft( 0.,0.,1., 0.); ALIColour colright( 0.,1.,1., 0.); //----- Upper CCD (leftmost point & direction dowel1-dowel2) //original ccdsize was 50; 1 works ok ALIdouble ccdsize = 20; // VRML objects are drawn 'after rotation into system'; so to make all CCDs look the // same in the drawing, CCDwidth parameters must be the same // x, y , z , color // vrmlmgr.AddBoxDisplaced( *this, CCDdim, CCDwidth, CCDwidth, ccdsize*(ccds[0].pt()+0.5*CCDlength*ccds[0].vec()), &colup); vrmlmgr.AddBoxDisplaced( *this, CCDdim, CCDwidth, CCDwidth, ccdsize*(ccds[0].pt()+0.5*CCDlength*ccds[0].vec()), &colup); //----- Lower CCD (leftmost point & direction dowel2-dowel1) vrmlmgr.AddBoxDisplaced( *this, CCDdim, CCDwidth, CCDwidth, ccdsize*(ccds[1].pt()+0.5*CCDlength*ccds[1].vec()), &coldown ); //----- left CCD (uppermost point & direction perpendicular to dowel2-dowel1) vrmlmgr.AddBoxDisplaced( *this, CCDwidth, CCDdim, CCDwidth, ccdsize*(ccds[2].pt()+0.5*CCDlength*ccds[2].vec()), &colleft ); //----- right CCD (uppermost point & direction perpendicular to dowel2-dowel1) vrmlmgr.AddBoxDisplaced( *this, CCDwidth, CCDdim, CCDwidth, ccdsize*(ccds[3].pt()+0.5*CCDlength*ccds[3].vec()), &colright ); vrmlmgr.SendReferenceFrame( *this, CCDdim); vrmlmgr.SendName( *this, 0.01 ); } ALIdouble OptOCOPS::getMeasFromInters( ALILine& line_xhair, ALILine& ccd, Hep3Vector& cops_line ) { if(Utils::debug >= 5) cout << "***** OptOCOPS::getMeasFromInters" <