240 {
241
242
243 DifNumber xDF( pos.x(), 1, 6 ), yDF( pos.y(), 2, 6 ), zDF( pos.z(), 3, 6 );
244 DifNumber pxDF( pmom.x(), 4, 6 );
245 DifNumber pyDF( pmom.y(), 5, 6 );
246 DifNumber pzDF( pmom.z(), 6, 6 );
247
248 static DifArray pars( 6, 6 );
249
250 DifNumber pt = pxDF;
251 pt *= pxDF;
252 pt += pyDF * pyDF;
253
254 if ( pt < 1.e-14 ) pt = 1.e-14;
255 if ( fabs( pxDF ) < 1.e-7 ) pxDF = pxDF < 0 ? -1e-7 : 1e-7;
256
257
258 pars( 3 ) = pzDF;
259 pars( 3 ) *= pzDF;
260 pars( 3 ) += pt;
261 pars( 3 ) = sqrt( pars( 3 ) );
262
263 pt = sqrt( pt );
264 DifNumber invpt = 1. / pt;
265
266
267
268
269 DifNumber pvxDF = pxDF;
270 pvxDF *= invpt;
271
272
273 DifNumber pvyDF = pyDF;
274 pvyDF *= invpt;
275
276
277 pars( 5 ) = pzDF;
278 pars( 5 ) *= invpt;
279
280 pars( 2 ) = atan2( pyDF, pxDF );
281
282
283 pars( 1 ) = yDF;
284 pars( 1 ) *= pvxDF;
285 pars( 1 ) -= xDF * pvyDF;
286
287
288 pars( 4 ) = xDF;
289 pars( 4 ) *= pvxDF;
290 pars( 4 ) += yDF * pvyDF;
291
292
293
294 pars( 6 ) = pars( 4 );
295 pars( 6 ) *= pars( 3 );
296 pars( 6 ) *= invpt;
297
298 pars( 4 ) *= -pars( 5 );
299 pars( 4 ) += zDF;
300
301
302
303 static HepSymMatrix posandmomErr( 6 );
304 static HepVector parsVec( 6 );
305
306 int i;
307 for ( i = 1; i <= 3; ++i )
308 {
309 int j;
310 for ( j = 1; j <= i; ++j )
311 {
312
313 posandmomErr.fast( i, j ) = pos.covMatrix().fast( i, j );
314 posandmomErr.fast( i + 3, j + 3 ) = pmom.covMatrix().fast( i, j );
315 }
316 for ( j = 1; j <= 3; ++j ) { posandmomErr.fast( j + 3, i ) = cxp( i, j ); }
317 }
318 for ( i = 1; i <= 6; ++i )
319 {
320
321
322 parsVec( i ) = pars( i ).number();
323 }
324
325 return NeutParams( parsVec, posandmomErr.similarity( pars.jacobian() ) );
326}