| 157 | |
| 158 | // Compute the difference between two angles in [-PI, PI] |
| 159 | template<class T> static T |
| 160 | angleDiff(T a, T b) |
| 161 | { |
| 162 | T diff = std::fabs(a - b); |
| 163 | if (diff > PI) |
| 164 | return (T) (2.0 * PI - diff); |
| 165 | else |
| 166 | return diff; |
| 167 | } |
| 168 | |
| 169 | template<class T> static T |
| 170 | min4(T a, T b, T c, T d) |