| 2451 | Cx.push_back( Cx_local ); |
| 2452 | Cy.push_back( Cy_local ); |
| 2453 | } |
| 2454 | |
| 2455 | return; |
| 2456 | } |
| 2457 | |
| 2458 | void trackHyperbola( std::vector< std::vector<double> > &Cx, std::vector< std::vector<double> > &Cy, |
| 2459 | const double *const PHI, const std::vector<double> &zx, const std::vector<double> &zy, |
| 2460 | const double *const Vx, const double *const Vy ) |
| 2461 | { |
| 2462 | const double EPS = 1e-10; |
| 2463 | const double &lambda1 = PHI[0]; const double &lambda2 = PHI[1]; |
| 2464 | const double &D = PHI[6]; const double &E = PHI[7]; const double &F = PHI[8]; |
| 2465 | |
| 2466 | assert( lambda1 * lambda2 < 0 ); |
| 2467 | assert( fabs(lambda1) + fabs(lambda2) > EPS ); |
| 2468 | |
| 2469 | const double &ev1x = PHI[2]; const double &ev1y = PHI[3]; |
| 2470 | const double &ev2x = PHI[4]; const double &ev2y = PHI[5]; |
| 2471 | const double P[2][2] = { { ev1x, ev2x }, { ev1y, ev2y } }; |
| 2472 | const double PT[2][2] = { { P[0][0], P[1][0] }, { P[0][1], P[1][1] } }; |
| 2473 | |
| 2474 | assert( zx.size() == zy.size() ); |
| 2475 | #if 1 |
| 2476 | std::vector<double> Zx, Zy; |
| 2477 | for(size_t i = 0; i < zx.size(); i++){ |
| 2478 | Zx.push_back( PT[0][0]*zx[i] + PT[0][1]*zy[i] + D/(2*lambda1) ); |
| 2479 | Zy.push_back( PT[1][0]*zx[i] + PT[1][1]*zy[i] + E/(2*lambda2) ); |
| 2480 | } |
| 2481 | #endif |
| 2482 | |
| 2483 | if( lambda1*F > 0 ){ |
| 2484 | |
| 2485 | // lambda1*(X')^2 + lambda2*(Y')^2 + F = 0 <=> Y' = \pm sqrt( (-lambda1*(X')^2 - F)/lambda2 ) |
| 2486 | const double a = -lambda1/lambda2; // Y' = \pm sqrt( a(X')^2 + b ) |
| 2487 | const double b = -F/lambda2; |
| 2488 | |
| 2489 | std::vector<double> Zx_plus, Zx_minus; |
| 2490 | for(size_t i = 0; i < Zy.size(); i++){ |
| 2491 | if( Zy[i] > 0 ){ |
| 2492 | Zx_plus.push_back( Zx[i] ); |
| 2493 | } else { |
| 2494 | Zx_minus.push_back( Zx[i] ); |
| 2495 | } |
| 2496 | } |
| 2497 | |
| 2498 | // Y' = + sqrt( a(X')^2 + b ) |
| 2499 | trackHyperbolaCore( Cx, Cy, +1, a, b, Zx_plus, Vx, Vy ); |
| 2500 | |
| 2501 | // Y' = - sqrt( a(X')^2 + b ) |
| 2502 | trackHyperbolaCore( Cx, Cy, -1, a, b, Zx_minus, Vx, Vy ); |
| 2503 | |
| 2504 | } else { |
| 2505 | static int count = 0; |
| 2506 | if (!( lambda2*F > 0 ) && verbosity && count++ <3 ) |
| 2507 | cout << " plotPDF: bizarre bug "<<lambda2 << " "<< F << endl; |
| 2508 | |
| 2509 | // lambda1*(X')^2 + lambda2*(Y')^2 + F = 0 <=> X' = \pm sqrt( (-lambda2*(Y')^2 - F) / lambda1 ) |
| 2510 | const double a = -lambda2/lambda1; // X' = \pm sqtt( a(Y')^2 + b ) |
no test coverage detected