@@ -191,88 +191,6 @@ float_type line_segment_plane_intersect(LineSegment3D segment, const Point3D pla
191191}
192192
193193
194- //float_type arc_segment_plane_intersect(const ArcSegment3D segment, const Point3D plane_point, const Point3D normal)
195- ///*
196- // Return a parameter `t` such that the point at `t * segment.angle` is the point on the
197- // segment at which `plane` intersects the `segment`. The plane is defined as a
198- // pose, such that if the pose is an identity, the plane lies in the X-Y plane (z=0), or, in
199- // other words, `plane @ [0, 0, 1]^T` is the plane normal.
200- //*/
201- //{
202- // const float_type eps = APER_PRECISION;
203- // const float_type length = segment.length;
204- //
205- // if (length <= eps) return 0.f;
206- //
207- // // Extract translation T from pose (local -> world)
208- // const float_type t_x = plane_point.x;
209- // const float_type t_y = plane_point.y;
210- // const float_type t_z = plane_point.z;
211- //
212- // // Extract 3rd column of rotation R (plane normal in world)
213- // const float_type n_x = normal.x;
214- // const float_type n_y = normal.y;
215- // const float_type n_z = normal.z;
216- //
217- // // Let A := segment start, compute A - T
218- // const Point3D p_start = arc_segment_point_at(segment, 0);
219- // const float_type ta_x = p_start.x - t_x;
220- // const float_type ta_y = p_start.y - t_y;
221- // const float_type ta_z = p_start.z - t_z;
222- //
223- // // Let B := segment end, compute B - T
224- // const Point3D p_end = arc_segment_point_at(segment, 1);
225- // const float_type tb_x = p_end.x - t_x;
226- // const float_type tb_y = p_end.y - t_y;
227- // const float_type tb_z = p_end.z - t_z;
228- //
229- // /*
230- // For numerical stability we will use bisection to find the intersection point.
231- // In principle there might be two such points, but is this is not expected in
232- // practice, we go on assuming there is just one.
233- //
234- // We iteratively converge on an interval where n * (A - T) and n * (B - T),
235- // the signed distances between points A, B and the plane, have different signs.
236- // */
237- // float_type n_dot_ta = n_x * ta_x + n_y * ta_y + n_z * ta_z;
238- // float_type n_dot_tb = n_x * tb_x + n_y * tb_y + n_z * tb_z;
239- //
240- // /* Short-circuit if already on one of the points */
241- // if (fabs(n_dot_ta) <= eps) return 0;
242- // if (fabs(n_dot_tb) <= eps) return 1;
243- //
244- // /* If no sign change assume no intersection (we assume max one intersection point) */
245- // if (signbit(n_dot_ta) == signbit(n_dot_tb)) return NAN;
246- //
247- // /* Bisect */
248- // float_type d_lo = 0;
249- // float_type d_hi = 1;
250- // float_type d_mid = 0.5;
251- //
252- // for (int i = 0; i < 34; i++) {
253- // d_mid = 0.5f * (d_lo + d_hi);
254- // const Point3D p_mid = arc_segment_point_at(segment, d_mid);
255- // const float_type n_dot_t_mid = n_x * (p_mid.x - t_x) + n_y * (p_mid.y - t_y) + n_z * (p_mid.z - t_z);
256- //
257- // /* Solution found within precision */
258- // if (fabs(n_dot_t_mid) <= eps || (d_hi - d_lo) <= eps) {
259- // return d_mid;
260- // }
261- //
262- // if (signbit(n_dot_t_mid) == signbit(n_dot_ta)) {
263- // /* If projections have the same sign, bisect on [d_mid, d_hi] */
264- // d_lo = d_mid;
265- // n_dot_ta = n_dot_t_mid;
266- // } else {
267- // /* Otherwise bisect on the interval [d_lo, d_mid] */
268- // d_hi = d_mid;
269- // }
270- // }
271- //
272- // return d_mid;
273- //}
274-
275-
276194static inline void plane_to_arc_local (
277195 const ArcSegment3D segment ,
278196 const Point3D plane_point ,
0 commit comments