00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include <stdio.h>
00037 #include <float.h>
00038 #include "MdcRecoUtil/DifNumber.h"
00039 #include "MdcRecoUtil/DifArray.h"
00040 #include "MdcRecoUtil/BesPointErr.h"
00041 using std::istream;
00042 using std::ostream;
00043
00044 BesPointErr operator + (const BesPointErr& v, const BesVectorErr& w){
00045 BesPointErr pe(HepPoint3D(v.x()+w.x(),v.y()+w.y(),v.z()+w.z()),
00046 (v.covMatrix()+w.covMatrix()));
00047 return pe;
00048 }
00049
00050 BesPointErr operator + (const BesVectorErr& w, const BesPointErr& v){
00051 BesPointErr pe(HepPoint3D(v.x()+w.x(),v.y()+w.y(),v.z()+w.z()),
00052 (v.covMatrix()+w.covMatrix()));
00053 return pe;
00054 }
00055
00056 BesPointErr operator - (const BesPointErr& v, const BesVectorErr& w){
00057 BesPointErr pe(HepPoint3D(v.x()-w.x(),v.y()-w.y(),v.z()-w.z()),
00058 (v.covMatrix()+w.covMatrix()));
00059 return pe;
00060 }
00061
00062 BesVectorErr operator - (const BesPointErr& v, const BesPointErr& w){
00063 BesVectorErr ve(Hep3Vector(v.x()-w.x(),v.y()-w.y(),v.z()-w.z()),
00064 (v.covMatrix()+w.covMatrix()));
00065 return ve;
00066 }
00067
00068 ostream & operator<<(ostream & stream, const BesPointErr & verr) {
00069 stream << (const HepPoint3D&)verr
00070 << ", " << verr.covMatrix();
00071
00072 return stream;
00073 }
00074
00075 istream & operator>>(istream & stream, BesPointErr & verr) {
00076 BesError mat(verr.SIZE);
00077 stream >> (HepPoint3D&)verr >> mat;
00078 verr.setCovMatrix(mat);
00079
00080 return stream;
00081 }
00082
00083 BesError BesPointErr::covRTPMatrix() const{
00084
00085 double xv = x()==0 ? FLT_MIN : x();
00086 double yv = y()==0 ? FLT_MIN : y();
00087 double zv = z()==0 ? FLT_MIN : z();
00088 DifNumber xDF(xv,X+1,3), yDF(yv,Y+1,3), zDF(zv,Z+1,3);
00089 DifArray pars(3,NUM_PCOORDINATES);
00090 pars[Rho] = sqrt(xDF*xDF + yDF*yDF + zDF*zDF);
00091 pars[Phi] = atan2(yDF,xDF);
00092 pars[Theta] = acos(zDF/pars[Rho]);
00093 return covMatrix().similarity(pars.jacobian());
00094 }
00095
00096 BesError BesPointErr::covRZPMatrix() const{
00097
00098 double xv = x()==0 ? FLT_MIN : x();
00099 double yv = y()==0 ? FLT_MIN : y();
00100 double zv = z()==0 ? FLT_MIN : z();
00101 DifNumber xDF(xv,X+1,3), yDF(yv,Y+1,3), zDF(zv,Z+1,3);
00102 DifArray pars(3,NUM_CCOORDINATES);
00103 pars[C_Rho] = sqrt(xDF*xDF + yDF*yDF );
00104 pars[C_Phi] = atan2(yDF,xDF);
00105 pars[C_Zeta] = zDF;
00106 return covMatrix().similarity(pars.jacobian());
00107 }
00108