| 128 | |
| 129 | template < index_t dimension > |
| 130 | IntersectionResult< absl::InlinedVector< Point< dimension >, 2 > > |
| 131 | line_sphere_intersection( const InfiniteLine< dimension >& line, |
| 132 | const Sphere< dimension >& sphere ) |
| 133 | { |
| 134 | // The sphere is (X-C)^T*(X-C)-1 = 0 and the line is X = P+t*D. |
| 135 | // Substitute the line equation into the sphere equation to obtain a |
| 136 | // quadratic equation Q(t) = t^2 + 2*a1*t + a0 = 0, where a1 = |
| 137 | // D^T*(P-C), |
| 138 | // and a0 = (P-C)^T*(P-C)-1. |
| 139 | const Vector< dimension > diff{ sphere.origin(), line.origin() }; |
| 140 | const auto a0 = diff.dot( diff ) - sphere.radius() * sphere.radius(); |
| 141 | const auto a1 = line.direction().dot( diff ); |
| 142 | |
| 143 | // Intersection occurs when Q(t) has real roots. |
| 144 | const auto discr = a1 * a1 - a0; |
| 145 | if( discr > GLOBAL_EPSILON ) // positive |
| 146 | { |
| 147 | absl::InlinedVector< Point< dimension >, 2 > results; |
| 148 | const auto root = std::sqrt( discr ); |
| 149 | results.reserve( 2 ); |
| 150 | results.emplace_back( |
| 151 | line.origin() + line.direction() * ( -a1 - root ) ); |
| 152 | results.emplace_back( |
| 153 | line.origin() + line.direction() * ( -a1 + root ) ); |
| 154 | typename CorrectnessInfo< absl::InlinedVector< Point< dimension >, |
| 155 | 2 > >::Correctness first_correctness; |
| 156 | first_correctness.first = |
| 157 | point_line_distance( results.front(), line ) <= GLOBAL_EPSILON; |
| 158 | first_correctness.second.push_back( |
| 159 | point_line_projection( results.front(), line ) ); |
| 160 | first_correctness.first = |
| 161 | first_correctness.first |
| 162 | && point_line_distance( results.back(), line ) |
| 163 | <= GLOBAL_EPSILON; |
| 164 | first_correctness.second.push_back( |
| 165 | point_line_projection( results.back(), line ) ); |
| 166 | typename CorrectnessInfo< absl::InlinedVector< Point< dimension >, |
| 167 | 2 > >::Correctness second_correctness; |
| 168 | const auto front_output = |
| 169 | point_sphere_distance( results.front(), sphere ); |
| 170 | second_correctness.first = |
| 171 | std::get< 0 >( front_output ) <= GLOBAL_EPSILON; |
| 172 | second_correctness.second.push_back( |
| 173 | std::get< 1 >( front_output ) ); |
| 174 | const auto back_output = |
| 175 | point_sphere_distance( results.back(), sphere ); |
| 176 | second_correctness.first = |
| 177 | second_correctness.first |
| 178 | && std::get< 0 >( back_output ) <= GLOBAL_EPSILON; |
| 179 | second_correctness.second.push_back( std::get< 1 >( back_output ) ); |
| 180 | return { std::move( results ), |
| 181 | { first_correctness, second_correctness } }; |
| 182 | } |
| 183 | else if( discr > -GLOBAL_EPSILON ) // zero |
| 184 | { |
| 185 | absl::InlinedVector< Point< dimension >, 2 > results; |
| 186 | results.reserve( 1 ); |
| 187 | results.emplace_back( line.origin() - line.direction() * a1 ); |