| 2136 | |
| 2137 | Cx.push_back( std::vector<double> { X0, X0+h/3, X1-h/3, X1 } ); |
| 2138 | Cy.push_back( std::vector<double> { P0y, P1y, P2y, P3y } ); |
| 2139 | } |
| 2140 | return; |
| 2141 | } |
| 2142 | |
| 2143 | void trackParabola( std::vector< std::vector<double> > &Cx, std::vector< std::vector<double> > &Cy, |
| 2144 | const double *const PHI, const std::vector<double> &zx, const std::vector<double> &zy, |
| 2145 | const double *const Vx, const double *const Vy ) |
| 2146 | |
| 2147 | { |
| 2148 | const double EPS = 1e-10; |
| 2149 | |
| 2150 | // PHI(X,Y) = lambda1*X*X + lambda2*Y*Y + D*X + E*Y + f |
| 2151 | // = lambda1 * (X + D/(2*lambda1))^2 + lambda2 * (Y + E/(2*lambda2))^2 |
| 2152 | // + ( -D*D/(4*lambda1) - E*E/(4*lambda2) + f) |
| 2153 | // = lambda1*(X + D/(2*lambda1))^2 + lambda2*(Y + E/(2*lambda2))^2 + F |
| 2154 | // X' = X + D/(2*lambda1), Y' = Y + E/(2*lambda2) |
| 2155 | // They are new variables, (not derivatives of X and Y) |
| 2156 | const double &lambda1 = PHI[0]; const double &lambda2 = PHI[1]; |
| 2157 | const double &D = PHI[6]; const double &E = PHI[7]; const double &F = PHI[8]; |
| 2158 | |
| 2159 | const double &ev1x = PHI[2]; const double &ev1y = PHI[3]; |
| 2160 | const double &ev2x = PHI[4]; const double &ev2y = PHI[5]; |
| 2161 | const double P[2][2] = { { ev1x, ev2x }, { ev1y, ev2y } }; |
| 2162 | const double PT[2][2] = { { P[0][0], P[1][0] }, { P[0][1], P[1][1] } }; |
| 2163 | |
| 2164 | assert( zx.size() == zy.size() ); |
| 2165 | #if 1 |
| 2166 | std::vector<double> Zx, Zy; |
| 2167 | for(size_t i = 0; i < zx.size(); i++){ |
| 2168 | Zx.push_back( PT[0][0]*zx[i] + PT[0][1]*zy[i] ); |
| 2169 | Zy.push_back( PT[1][0]*zx[i] + PT[1][1]*zy[i] ); |
| 2170 | } |
| 2171 | #endif |
| 2172 | if( fabs(lambda1) > EPS ){ |
| 2173 | |
| 2174 | // lambda1*X^2 + D*X + E*Y + F = 0 |
| 2175 | // <=> lambda1*( X + D/(2*lambda1) )^2 + E*Y + F - D*D/(4*lambda1) = 0 |
| 2176 | // <=> Y = (-lambda1/E) * (X')^2 - F'/E |
| 2177 | if( fabs(E) < EPS ) return; |
| 2178 | |
| 2179 | for(std::vector<double>::iterator itr = Zx.begin(); itr != Zx.end(); itr++) |
| 2180 | *itr += D/(2*lambda1); |
| 2181 | |
| 2182 | const double a = -lambda1/E; // Y' = a*(X')^2 + b |
| 2183 | const double b = -F/E; |
| 2184 | |
| 2185 | trackParabolaCore( Cx, Cy, a, b, Zx, Vx, Vy ); |
| 2186 | |
| 2187 | } else { |
| 2188 | |
| 2189 | assert( fabs(lambda2) > EPS ); |
| 2190 | |
| 2191 | // D*X + lambda2*Y^2 + E*Y + F = 0 <=> X = (-lambda2/D)*Y^2 - (E/D)*Y - F/D |
| 2192 | // <=> D*X + lambda2*(Y - E/(2*lambda2))^2 + F - E*E/(4*lambda2) = 0 |
| 2193 | // <=> X = (-lambda2/D)*(Y')^2 - F'/D |
| 2194 | if( fabs(D) < EPS ) return; |
| 2195 | |