102 let p = self.to_cartesian(); |
102 let p = self.to_cartesian(); |
103 let np2 = p.norm2_squared(); |
103 let np2 = p.norm2_squared(); |
104 let nt2 = t.norm2_squared(); |
104 let nt2 = t.norm2_squared(); |
105 let r2 = r * r; |
105 let r2 = r * r; |
106 let d = p.dot(t); |
106 let d = p.dot(t); |
107 assert!(np2 <= r2); |
107 assert!(np2 <= r2 + f64::EPSILON, "‖{p}‖ = {} > {r}", np2.sqrt()); |
108 let s = (-d + (d*d + nt2 * (r2 - np2)).sqrt()) / nt2; |
108 let s = (-d + (d*d + nt2 * (r2 - np2)).sqrt()) / nt2; |
109 if s < 1.0 { |
109 if s < 1.0 { |
110 (Self::from_cartesian(p + s * t), Some((1.0-s) * t)) |
110 (Self::from_cartesian(p + s * t), Some((1.0-s) * t)) |
111 } else { |
111 } else { |
112 (Self::from_cartesian(p + t), None) |
112 (Self::from_cartesian(p + t), None) |