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
00037
00038 #include <float.h>
00039 #include <iostream>
00040 #include "MdcRecoUtil/BesVectorErr.h"
00041 #include "MdcRecoUtil/DifNumber.h"
00042 #include "MdcRecoUtil/DifArray.h"
00043 using std::istream;
00044 using std::ostream;
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056 BesError BesVectorErr::covRTPMatrix() const{
00057
00058 double xv = x()==0 ? FLT_MIN : x();
00059 double yv = y()==0 ? FLT_MIN : y();
00060 double zv = z()==0 ? FLT_MIN : z();
00061 DifNumber xDF(xv,X+1,3), yDF(yv,Y+1,3), zDF(zv,Z+1,3);
00062 DifArray pars(3,NUM_PCOORDINATES);
00063 pars[Rho] = sqrt(xDF*xDF + yDF*yDF + zDF*zDF);
00064 pars[Phi] = atan2(yDF,xDF);
00065 pars[Theta] = acos(zDF/pars[Rho]);
00066 return covMatrix().similarity(pars.jacobian());
00067 }
00068
00069 BesError BesVectorErr::covRZPMatrix() const{
00070
00071 double xv = x()==0 ? FLT_MIN : x();
00072 double yv = y()==0 ? FLT_MIN : y();
00073 double zv = z()==0 ? FLT_MIN : z();
00074 DifNumber xDF(xv,X+1,3), yDF(yv,Y+1,3), zDF(zv,Z+1,3);
00075 DifArray pars(3,NUM_CCOORDINATES);
00076 pars[C_Rho] = sqrt(xDF*xDF + yDF*yDF);
00077 pars[C_Phi] = atan2(yDF,xDF);
00078 pars[C_Zeta] = zDF;
00079 return covMatrix().similarity(pars.jacobian());
00080 }
00081
00082 double BesVectorErr::determineChisq(const Hep3Vector& refVector) const
00083 {
00084 HepVector temp(NUM_COORDINATES, 0);
00085 temp[0] = refVector.x()-this->x();
00086 temp[1] = refVector.y()-this->y();
00087 temp[2] = refVector.z()-this->z();
00088 return _covMatrix.determineChisq(temp);
00089 }
00090
00091
00092 BesVectorErr operator + (const BesVectorErr& v, const BesVectorErr& w){
00093 BesVectorErr ve(Hep3Vector(v.x()+w.x(),v.y()+w.y(),v.z()+w.z()),
00094 (v.covMatrix()+w.covMatrix()));
00095 return ve;
00096 }
00097
00098 BesVectorErr operator - (const BesVectorErr& v, const BesVectorErr& w){
00099 BesVectorErr ve(Hep3Vector(v.x()-w.x(),v.y()-w.y(),v.z()-w.z()),
00100 (v.covMatrix()+w.covMatrix()));
00101 return ve;
00102 }
00103
00104
00105 BesVectorErr operator * (const BesVectorErr & p, double a) {
00106 BesVectorErr ve(Hep3Vector(a*p.x(), a*p.y(), a*p.z()),
00107 (p.covMatrix()*a*a));
00108 return ve;
00109 }
00110
00111 BesVectorErr operator * (double a, const BesVectorErr & p) {
00112 BesVectorErr ve(Hep3Vector(a*p.x(), a*p.y(), a*p.z()),
00113 (p.covMatrix()*a*a));
00114 return ve;
00115 }
00116
00117
00118 ostream & operator<<(ostream & stream, const BesVectorErr & verr) {
00119 stream << (const Hep3Vector&)verr
00120 << ", " << verr.covMatrix();
00121
00122 return stream;
00123 }
00124
00125 istream & operator>>(istream & stream, BesVectorErr & verr) {
00126 BesError mat(verr.SIZE);
00127 stream >> (Hep3Vector&)verr >> mat;
00128 verr.setCovMatrix(mat);
00129
00130 return stream;
00131 }
00132