00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include <math.h>
00017 #include <stdlib.h>
00018 #include "TrkExtAlg/Ext_xp_err.h"
00019
00020
00021
00022
00023
00024 static const double Cinv( 3335.640952 );
00025
00026 static const double C( 0.002997924582 );
00027 static const double Small( 0.01 );
00028 static const double Eps(1.0e-12);
00029 static const double Infinite( 1.0e10 );
00030
00031 static const int Ndim_herr( 5 );
00032
00033
00034 Ext_xp_err::Ext_xp_err() :
00035 m_xv(3), m_pv(3), m_xp_jcb(Ndim_err,Ndim_err,0),
00036 m_h2xp_jcb(Ndim_err,Ndim_herr,0), m_q(0), m_mass2(0){}
00037
00038
00039 Ext_xp_err::Ext_xp_err( const Ext_xp_err &err ) :
00040 Ext_errmx( (Ext_errmx)err ), m_xv(err.m_xv), m_pv(err.m_pv),
00041 m_xp_jcb(err.m_xp_jcb), m_h2xp_jcb(err.m_h2xp_jcb), m_q( err.m_q ),
00042 m_mass2( err.m_mass2 ){}
00043
00044
00045
00046
00047 void Ext_xp_err::set_err( const HepSymMatrix &err, const Hep3Vector &xv,
00048 const Hep3Vector &pv,
00049 const double &q, const double &mass )
00050
00051
00052
00053
00054
00055
00056
00057 {
00058 Ext_errmx *errmx_ptr = (Ext_errmx *)this;
00059 errmx_ptr->put_err( err );
00060
00061 m_xv = xv;
00062 m_pv = pv;
00063 m_q = q;
00064 m_mass2 = mass * mass;
00065
00066
00067
00068 }
00069
00070
00071
00072
00073
00074
00075 bool Ext_xp_err::move( const Hep3Vector &xv1, const Hep3Vector &pv1,
00076 const Hep3Vector &B, const int ms_on,
00077 const double chi_cc)
00078 {
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090 double dx( ( xv1 - m_xv ).mag() );
00091 double dp( ( pv1 - m_pv ).mag() );
00092 double p2( m_pv.mag2() );
00093 double p_abs( sqrt( p2 ) );
00094
00095 double p_inv;
00096 if( p_abs > Small && pv1.mag() > Small ){
00097 p_inv = 1.0 / p_abs;
00098 } else {
00099 p_inv = Infinite;
00100 return 0;
00101 }
00102
00103 double ms_coeff( 2.557 * chi_cc );
00104 bool with_B( ( B.mag() > Small ) ? 1 : 0 );
00105 double p2inv( p_inv * p_inv );
00106 double p3inv( p2inv * p_inv );
00107 double fdx( dx * p3inv );
00108 double cx( 100.*m_q * C * fdx );
00109 double fdp( dp * p_inv );
00110
00111 double px( m_pv.x() );
00112 double py( m_pv.y() );
00113 double pz( m_pv.z() );
00114 double px2( px * px );
00115 double py2( py * py );
00116 double pz2( pz * pz );
00117 double pxy( px * py );
00118 double pyz( py * pz );
00119 double pzx( pz * px );
00120 double Bx( B.x() );
00121 double By( B.y() );
00122 double Bz( B.z() );
00123
00124 m_xp_jcb( 1, 1 ) = 1.0;
00125 m_xp_jcb( 1, 4 ) = fdx * ( py2 + pz2 );
00126 m_xp_jcb( 1, 5 ) = - fdx * pxy;
00127 m_xp_jcb( 1, 6 ) = - fdx * pzx;
00128
00129 m_xp_jcb( 2, 2 ) = 1.0;
00130 m_xp_jcb( 2, 4 ) = - fdx * pxy;
00131 m_xp_jcb( 2, 5 ) = fdx * ( pz2 + px2 );
00132 m_xp_jcb( 2, 6 ) = - fdx * pyz;
00133
00134 m_xp_jcb( 3, 3 ) = 1.0;
00135 m_xp_jcb( 3, 4 ) = - fdx * pzx;
00136 m_xp_jcb( 3, 5 ) = - fdx * pyz;
00137 m_xp_jcb( 3, 6 ) = fdx * ( px2 + py2 );
00138
00139 m_xp_jcb( 4, 4 ) = 1.0 - fdp;
00140 m_xp_jcb( 5, 5 ) = 1.0 - fdp;
00141 m_xp_jcb( 6, 6 ) = 1.0 - fdp;
00142
00143 if( with_B && m_q!=0. ){
00144 m_xp_jcb( 4, 4 ) += - cx * ( pxy * Bz - pzx * By );
00145 m_xp_jcb( 4, 5 ) = cx * ( ( pz2 + px2 ) * Bz + pyz * By );
00146 m_xp_jcb( 4, 6 ) = - cx * ( ( px2 + py2 ) * By + pyz * Bz );
00147
00148 m_xp_jcb( 5, 4 ) = - cx * ( ( py2 + pz2 ) * Bz + pzx * Bx );
00149 m_xp_jcb( 5, 5 ) += - cx * ( pyz * Bx - pxy * Bz );
00150 m_xp_jcb( 5, 6 ) = cx * ( ( px2 + py2 ) * Bx + pzx * Bz );
00151
00152 m_xp_jcb( 6, 4 ) = cx * ( ( py2 + pz2 ) * By + pxy * Bx );
00153 m_xp_jcb( 6, 5 ) = - cx * ( ( pz2 + px2 ) * Bx + pxy * By );
00154 m_xp_jcb( 6, 6 ) += - cx * ( pzx * By - pyz * Bx );
00155 }
00156
00157
00158
00159 m_err = m_err.similarity( m_xp_jcb );
00160
00161
00162
00163 if( ms_on ){
00164 double beta_p_inv( ( m_mass2 + p2 ) / ( p2 * p2 ) );
00165 double th2( 100000.0 * ms_coeff * ms_coeff * beta_p_inv * dx );
00166
00167 double m11( th2 * dx * dx /3.0);
00168 double m12( 0.5 * th2 * dx * p_abs );
00169 double m22( th2 * p2 );
00170
00171 if( p_abs > Eps ){
00172 double c1( px*p_inv );
00173 double c2( py*p_inv );
00174 double c3( pz*p_inv );
00175 double c12( - c1*c2 );
00176 double c13( - c1*c3 );
00177 double c23( - c2*c3 );
00178 double s1s( 1 - c1*c1 );
00179 double s2s( 1 - c2*c2 );
00180 double s3s( 1 - c3*c3 );
00181
00182 m_err.fast( 1, 1 ) += m11*s1s;
00183
00184 m_err.fast( 2, 1 ) += m11*c12;
00185 m_err.fast( 2, 2 ) += m11*s2s;
00186
00187 m_err.fast( 3, 1 ) += m11*c13;
00188 m_err.fast( 3, 2 ) += m11*c23;
00189 m_err.fast( 3, 3 ) += m11*s3s;
00190
00191 m_err.fast( 4, 1 ) += m12*s1s;
00192 m_err.fast( 4, 2 ) += m12*c12;
00193 m_err.fast( 4, 3 ) += m12*c13;
00194 m_err.fast( 4, 4 ) += m22*s1s;
00195
00196 m_err.fast( 5, 1 ) += m12*c12;
00197 m_err.fast( 5, 2 ) += m12*s2s;
00198 m_err.fast( 5, 3 ) += m12*c23;
00199 m_err.fast( 5, 4 ) += m22*c12;
00200 m_err.fast( 5, 5 ) += m22*s2s;
00201
00202 m_err.fast( 6, 1 ) += m12*c13;
00203 m_err.fast( 6, 2 ) += m12*c23;
00204 m_err.fast( 6, 3 ) += m12*s3s;
00205 m_err.fast( 6, 4 ) += m22*c13;
00206 m_err.fast( 6, 5 ) += m22*c23;
00207 m_err.fast( 6, 6 ) += m22*s3s;
00208 }
00209
00210 }
00211
00212
00213 m_xv = xv1;
00214 m_pv = pv1;
00215 return 1;
00216 }
00217
00218
00219 Ext_xp_err &Ext_xp_err::operator=( const Ext_xp_err &err )
00220 {
00221 if( this != &err ){
00222 *((Ext_errmx *)this) = err;
00223 m_xv = err.m_xv;
00224 m_pv = err.m_pv;
00225 m_xp_jcb = err.m_xp_jcb;
00226 m_h2xp_jcb = err.m_h2xp_jcb;
00227 m_q = err.m_q;
00228 m_mass2 = err.m_mass2;
00229 }
00230 return *this;
00231 }
00232
00233
00234
00235
00236 std::ostream &operator<<( std::ostream &s, const Ext_xp_err &err )
00237 {
00238 s << "m_err: " << (Ext_errmx)err
00239 << std::endl
00240 << "m_xv: " << err.m_xv << std::endl << " abs(xv)= " << err.m_xv.mag()
00241 << std::endl
00242 << "m_pv: " << err.m_pv << std::endl << " abs(pv)= " << err.m_pv.mag()
00243 << std::endl
00244 << "m_xp_jcb: " << err.m_xp_jcb
00245 << std::endl
00246 << "m_h2xp_jcb: " << err.m_h2xp_jcb
00247 << std::endl
00248 << "m_q: " << err.m_q
00249 << std::endl
00250 << "m_mass: " << sqrt(err.m_mass2)
00251 << std::endl;
00252 return s;
00253 }