@@ -56,6 +56,11 @@ fn validate_points(points: &[Vec<f64>]) -> Result<usize, GeometryError> {
5656 Ok ( dim)
5757}
5858
59+ #[ inline]
60+ fn squared_norm ( point : & [ f64 ] ) -> f64 {
61+ point. iter ( ) . map ( |coord| coord * coord) . sum ( )
62+ }
63+
5964fn determinant ( matrix : & [ Vec < f64 > ] ) -> Result < f64 , GeometryError > {
6065 let n = matrix. len ( ) ;
6166 if n == 0 {
@@ -121,37 +126,63 @@ pub fn matrix_rank(vectors: &[Vec<f64>], tol: f64) -> Result<usize, GeometryErro
121126 let rows = vectors. len ( ) ;
122127 let flat: Vec < f64 > = vectors. iter ( ) . flat_map ( |row| row. iter ( ) . copied ( ) ) . collect ( ) ;
123128 let svd = DMatrix :: from_row_slice ( rows, cols, & flat) . svd ( false , false ) ;
129+ let max_singular = svd
130+ . singular_values
131+ . iter ( )
132+ . copied ( )
133+ . fold ( 0.0_f64 , f64:: max) ;
134+ let tol = if tol. is_sign_positive ( ) {
135+ tol
136+ } else {
137+ f64:: EPSILON * rows. max ( cols) as f64 * max_singular
138+ } ;
124139 Ok ( svd
125140 . singular_values
126141 . iter ( )
127142 . filter ( |value| * * value > tol)
128143 . count ( ) )
129144}
130145
131- pub fn fast_2d_point_in_simplex ( point : & [ f64 ; 2 ] , simplex : & [ [ f64 ; 2 ] ; 3 ] , eps : f64 ) -> bool {
146+ pub fn numpy_matrix_rank ( vectors : & [ Vec < f64 > ] ) -> Result < usize , GeometryError > {
147+ matrix_rank ( vectors, -1.0 )
148+ }
149+
150+ pub fn fast_2d_point_in_simplex (
151+ point : & [ f64 ; 2 ] ,
152+ simplex : & [ [ f64 ; 2 ] ; 3 ] ,
153+ eps : f64 ,
154+ ) -> Result < bool , GeometryError > {
132155 let [ [ p0x, p0y] , [ p1x, p1y] , [ p2x, p2y] ] = * simplex;
133156 let [ px, py] = * point;
134157
135158 let area: f64 = 0.5
136159 * ( -p1y * p2x + p0y * ( p2x - p1x) + p1x * p2y + p0x * ( p1y - p2y) ) ;
137- if area. abs ( ) <= f64 :: EPSILON {
138- return false ;
160+ if area == 0.0 {
161+ return Err ( GeometryError :: DegenerateSimplex ) ;
139162 }
140163
141164 let s = 1.0 / ( 2.0 * area)
142165 * ( p0y * p2x + ( p2y - p0y) * px - p0x * p2y + ( p0x - p2x) * py) ;
143166 if s < -eps || s > 1.0 + eps {
144- return false ;
167+ return Ok ( false ) ;
145168 }
146169
147170 let t = 1.0 / ( 2.0 * area)
148171 * ( p0x * p1y + ( p0y - p1y) * px - p0y * p1x + ( p1x - p0x) * py) ;
149- t >= -eps && s + t <= 1.0 + eps
172+ Ok ( t >= -eps && s + t <= 1.0 + eps)
150173}
151174
152- pub fn point_in_simplex ( point : & [ f64 ] , simplex : & [ Vec < f64 > ] , eps : f64 ) -> bool {
175+ pub fn point_in_simplex ( point : & [ f64 ] , simplex : & [ Vec < f64 > ] , eps : f64 ) -> Result < bool , GeometryError > {
153176 if simplex. is_empty ( ) || simplex. len ( ) != point. len ( ) + 1 {
154- return false ;
177+ return Err ( GeometryError :: InvalidDimensions (
178+ "Simplex dimension mismatch" . to_string ( ) ,
179+ ) ) ;
180+ }
181+ validate_points ( simplex) ?;
182+ if simplex. iter ( ) . any ( |vertex| vertex. len ( ) != point. len ( ) ) {
183+ return Err ( GeometryError :: InvalidDimensions (
184+ "Simplex dimension mismatch" . to_string ( ) ,
185+ ) ) ;
155186 }
156187 if point. len ( ) == 2 && simplex. len ( ) == 3 {
157188 let point = [ point[ 0 ] , point[ 1 ] ] ;
@@ -175,11 +206,8 @@ pub fn point_in_simplex(point: &[f64], simplex: &[Vec<f64>], eps: f64) -> bool {
175206 }
176207 }
177208
178- let Ok ( alpha) = solve_square ( & matrix, & rhs) else {
179- return false ;
180- } ;
181-
182- alpha. iter ( ) . all ( |value| * value > -eps) && alpha. iter ( ) . sum :: < f64 > ( ) < 1.0 + eps
209+ let alpha = solve_square ( & matrix, & rhs) ?;
210+ Ok ( alpha. iter ( ) . all ( |value| * value > -eps) && alpha. iter ( ) . sum :: < f64 > ( ) < 1.0 + eps)
183211}
184212
185213pub fn fast_2d_circumcircle ( points : & [ [ f64 ; 2 ] ; 3 ] ) -> ( [ f64 ; 2 ] , f64 ) {
@@ -267,40 +295,26 @@ pub fn circumsphere(pts: &[Vec<f64>]) -> Result<(Vec<f64>, f64), GeometryError>
267295 return Ok ( ( center. into_iter ( ) . collect ( ) , radius) ) ;
268296 }
269297
270- let rows = pts. len ( ) ;
271- let cols = dim + 2 ;
272- let mut mat = vec ! [ vec![ 0.0 ; cols] ; rows] ;
273- for ( row, point) in pts. iter ( ) . enumerate ( ) {
274- mat[ row] [ 0 ] = point. iter ( ) . map ( |coord| coord * coord) . sum ( ) ;
275- for ( col, coord) in point. iter ( ) . enumerate ( ) {
276- mat[ row] [ col + 1 ] = * coord;
277- }
278- mat[ row] [ cols - 1 ] = 1.0 ;
279- }
298+ let x0 = & pts[ 0 ] ;
299+ let x0_sq = squared_norm ( x0) ;
300+ let mut matrix = vec ! [ vec![ 0.0 ; dim] ; dim] ;
301+ let mut rhs = vec ! [ 0.0 ; dim] ;
280302
281- let denominator: Vec < Vec < f64 > > = mat
282- . iter ( )
283- . map ( |row| row[ 1 ..] . to_vec ( ) )
284- . collect ( ) ;
285- let a = determinant ( & denominator) ?;
286- if a. abs ( ) <= f64:: EPSILON {
287- return Err ( GeometryError :: SingularMatrix ) ;
303+ for row in 0 ..dim {
304+ let point = & pts[ row + 1 ] ;
305+ rhs[ row] = squared_norm ( point) - x0_sq;
306+ for col in 0 ..dim {
307+ matrix[ row] [ col] = 2.0 * ( point[ col] - x0[ col] ) ;
308+ }
288309 }
289310
290- let mut center = Vec :: with_capacity ( dim) ;
291- for column in 1 ..rows {
292- let minor: Vec < Vec < f64 > > = mat
293- . iter ( )
294- . map ( |row| {
295- row. iter ( )
296- . enumerate ( )
297- . filter_map ( |( idx, value) | ( idx != column) . then_some ( * value) )
298- . collect ( )
299- } )
300- . collect ( ) ;
301- let factor = if ( column + 1 ) % 2 == 0 { 1.0 } else { -1.0 } ;
302- center. push ( factor * determinant ( & minor) ? / ( 2.0 * a) ) ;
303- }
311+ let center = match solve_square ( & matrix, & rhs) {
312+ Ok ( center) => center,
313+ Err ( GeometryError :: SingularMatrix ) => {
314+ return Ok ( ( vec ! [ f64 :: NAN ; dim] , f64:: NAN ) ) ;
315+ }
316+ Err ( err) => return Err ( err) ,
317+ } ;
304318
305319 let radius = fast_norm (
306320 & center
@@ -312,6 +326,60 @@ pub fn circumsphere(pts: &[Vec<f64>]) -> Result<(Vec<f64>, f64), GeometryError>
312326 Ok ( ( center, radius) )
313327}
314328
329+ fn slogdet ( matrix : & [ Vec < f64 > ] ) -> Result < ( f64 , f64 ) , GeometryError > {
330+ let n = matrix. len ( ) ;
331+ if n == 0 {
332+ return Ok ( ( 1.0 , 0.0 ) ) ;
333+ }
334+ if matrix. iter ( ) . any ( |row| row. len ( ) != n) {
335+ return Err ( GeometryError :: InvalidDimensions (
336+ "Matrix must be square" . to_string ( ) ,
337+ ) ) ;
338+ }
339+
340+ let mut work = matrix. to_vec ( ) ;
341+ let mut sign = 1.0 ;
342+ let mut log_abs_det = 0.0 ;
343+
344+ for pivot_col in 0 ..n {
345+ let mut pivot_row = pivot_col;
346+ let mut pivot_abs = work[ pivot_col] [ pivot_col] . abs ( ) ;
347+ for row in ( pivot_col + 1 ) ..n {
348+ let candidate = work[ row] [ pivot_col] . abs ( ) ;
349+ if candidate > pivot_abs {
350+ pivot_abs = candidate;
351+ pivot_row = row;
352+ }
353+ }
354+
355+ if pivot_abs == 0.0 {
356+ return Ok ( ( 0.0 , f64:: NEG_INFINITY ) ) ;
357+ }
358+
359+ if pivot_row != pivot_col {
360+ work. swap ( pivot_row, pivot_col) ;
361+ sign = -sign;
362+ }
363+
364+ let pivot = work[ pivot_col] [ pivot_col] ;
365+ if pivot == 0.0 {
366+ return Ok ( ( 0.0 , f64:: NEG_INFINITY ) ) ;
367+ }
368+ sign *= pivot. signum ( ) ;
369+ log_abs_det += pivot. abs ( ) . ln ( ) ;
370+
371+ for row in ( pivot_col + 1 ) ..n {
372+ let factor = work[ row] [ pivot_col] / pivot;
373+ work[ row] [ pivot_col] = 0.0 ;
374+ for col in ( pivot_col + 1 ) ..n {
375+ work[ row] [ col] -= factor * work[ pivot_col] [ col] ;
376+ }
377+ }
378+ }
379+
380+ Ok ( ( sign, log_abs_det) )
381+ }
382+
315383pub fn orientation ( face : & [ Vec < f64 > ] , origin : & [ f64 ] ) -> Result < i32 , GeometryError > {
316384 let dim = validate_points ( face) ?;
317385 if face. len ( ) != dim || origin. len ( ) != dim {
@@ -324,10 +392,10 @@ pub fn orientation(face: &[Vec<f64>], origin: &[f64]) -> Result<i32, GeometryErr
324392 . iter ( )
325393 . map ( |point| point. iter ( ) . zip ( origin) . map ( |( x, y) | x - y) . collect ( ) )
326394 . collect ( ) ;
327- let det = determinant ( & matrix) ?;
328- if det . abs ( ) < ( -50.0f64 ) . exp ( ) {
395+ let ( sign , log_det ) = slogdet ( & matrix) ?;
396+ if sign == 0.0 || log_det < -50.0 {
329397 Ok ( 0 )
330- } else if det . is_sign_positive ( ) {
398+ } else if sign . is_sign_positive ( ) {
331399 Ok ( 1 )
332400 } else {
333401 Ok ( -1 )
@@ -362,6 +430,12 @@ pub fn simplex_volume_in_embedding(vertices: &[Vec<f64>]) -> Result<f64, Geometr
362430 ) ) ;
363431 }
364432
433+ if vertices[ 0 ] . len ( ) == 2 && vertices. len ( ) != 3 {
434+ return Err ( GeometryError :: InvalidDimensions (
435+ "Expected three 2D vertices" . to_string ( ) ,
436+ ) ) ;
437+ }
438+
365439 if vertices. len ( ) == 3 {
366440 let a = squared_distance ( & vertices[ 0 ] , & vertices[ 1 ] ) . sqrt ( ) ;
367441 let b = squared_distance ( & vertices[ 1 ] , & vertices[ 2 ] ) . sqrt ( ) ;
0 commit comments