
    h                     |    d dl Zd dlmZ d dlZd dlZd dlmZ d dl	m
Z
 d dl	mZ d dlmZ d dlZ G d d          ZdS )    N)
TW_Utility)TW_Point)
TW_Segment)TW_Linear_regressionc                       e Zd Zd ZddZ	 ed             Zd Zd Zd Z	d Z
	 dd
Z	 d Zd Zd Z	 ddZ	 d Zd Zd ZdS )TW_Segment_Regressorc                 L    g | _         d | _        || _        || _        || _        d S N)segmentscurrentSegmentXYthresold)selfr   r   r   s       Rc:\Users\Fabien\Documents\Programmation\testPython\classes\TW_Segment_Regressor.py__init__zTW_Segment_Regressor.__init__   s*    "     Nc                    ||| _         ||| _        | j         }| j        }t          |          dk    rt          d          t          |j                  dk    rt          d          d }d }d }t          dt          |                    D ]B}t          j        |          rHt          ||         ||                   }t          |          }||_
        |                    |           n1t          ||         ||                   }|                    |           d }t          j        |          sst          t          j        |                                          t          j        |                                                    }|                                 ||_        t          j        |          st          |          |dz   k    rt          ||dz            ||dz                      }|                    |j         g          }	t)          |	|j        z
            dz  |j        z  }
|
| j        k    rc| j                            |           t          ||         ||                   }d }t          |          }||_
        |                    |           t          |          |dz   k    r| j                            |           D| j        S )Nr   z
Empty data   z%3 dimensionals data not yet supported   d   )r   r   len	Exceptionshaperanger   empty_or_noner   r   startaddPointr   nparraygetXgetYprocess
regressionpredictabsr   r   append)r   r   r   segmentFirstPointsegmentNextPointnextPointRegressionir   segmentLastPointpredictedLastPointdiffs              r   r#   zTW_Segment_Regressor.process   s    	DF 	DFFFq66Q; 	*L)))qw<<1
 	ECDDD "3q66""  	5  	5A'(9:: :$,QqT1Q4$8$8!!+A'8$''(9::::#+AaD!A$#7#7 ''(8999"&+,<== @&:28NDWDWDYDY;Z;Z[][cdrdwdwdydy[z[z&{&{##++---,?)+,<== 5#a&&1a4- 5#+Aa!eHaAh#?#? %8%@%@BRBTAU%V%V".1A1CCDDKP`Pbb$-' ?M((888(01qt(<(<%'+$%/]]N+<N("++,=>>>Q1q5 5$$^444}r   c                    d }t          j        |          r"t          |          dk    rt          d          t          j        |          s|g}t	          dt          |                     D ]}| |                                         }|d         }|t          |          dz
           }t          |          dk     r=t          j        |d                   s#|d         |k    r|d         |k    r
| |         } nt|d         }t          |          }d}	t	          dt          |                    D ]+}
||
         ||
         k    r||
         ||
         k    r|	dz   }	,|	|k    r
| |         } n|S )Nr   XX must be a unique feature with 1 Dimension or an array of one feature with N Dimensionsr   r   )r   is_arrayr   r   r   r!   )r   r   matchedSegmentr+   segmentXsegmentXMinsegmentXMaxvalueToComparenbrDimensionsToConsidernbrDimensionsMatchedjs              r   getSegmentFittingCoordinatesz1TW_Segment_Regressor.getSegmentFittingCoordinates{   s   q!! 	xc!ffqj 	xvwww"1%% 	AqX'' 	 	A{''))H"1+K"3x==!#34K1vvz *"5ad";"; Q4;& 1Q4;+> %-a[NE!"1*-n*=*=''($#n"5"566 H HA%a(KN: H~a?PT_`aTb?b H/Ca/G,'+BB %-a[NE r   c                     ||dz
           S )Nr    )r   
nbrColumnsr   s      r   _getPointForDimensionz*TW_Segment_Regressor._getPointForDimension   s    Qr   c                 `   d}t          j        |          r"t          |          dk    rt          d          t          j        |          s|g}t          |          dk     rAt          j        |d                   s'|d         |                                d         k     rd}nt          j        |          }|d         }|                                d         }t          j        t          j        |          |          }t          j        t          j        |          |          }||k     }|S )NFr   r0   r   r   T)	r   r1   r   r   r!   np_number_columns_euclidianDistancer   zeros)	r   firstSegmentr   coordinateBeforeFirstSegmentr=   r6   point2euclidianDistanceBetweenOriginAndFirstSegmentPoint+euclidianDistanceBetweenOriginAndCoordinates	            r   _isCoordinateBeforeFirstSegmentz4TW_Segment_Regressor._isCoordinateBeforeFirstSegment   s4   ',$q!! 	xc!ffqj 	xvwww"1%% 	Aq66A: 
	Lj1!A$77 
	Ltl''))!,, 4/3,#5a88JqTN %%''*EAKA^_a_ghr_s_suzA{A{>:D:WXZX`akXlXln|:}:}7+V  ZL  ,L(++r   c                    d}|                                 }|d         }|t          |          dz
           }t          j        |          r"t          |          dk    rt	          d          t          j        |          s|g}t          |          dk     r)t          j        |d                   s|d         |k    rd}nt          j        |          }|d         }t          j        |          }	|j        d                                          }|j        t          |j                  dz
                                            }t          j	        |	|          }
t          j	        |	|          }||
k    }|S )NFr   r   r0   r   T)
r!   r   r   r1   r   r@   r   rB   pointsrA   )r   lastSegmentr   coordinateAfterLastSegmentlastSegmentPointslastSegmentFirstPointlastSegmentLastPointr=   r6   origin5euclidianDistanceBetweenOriginAndLastSegmentLastPoint&euclidianDistanceBetweenOriginAndPoints               r   _isCoordinateAfterLastSegmentz2TW_Segment_Regressor._isCoordinateAfterLastSegment   s   %*"',,.. 1! 405F1G1G1JKq!! 	xc!ffqj 	xvwww"1%% 	Aq66A: 	Hj1!A$77 	Ht** 2-1*#5a88JqTNXj))F$/$6q$9$>$>$@$@!#.#5c+:L6M6Mq6P#Q#V#V#X#X  EODabhj~DDA5?5RSY[i5j5j2)O  SH  *H&))r   c                    t          j        |          r"t          |          dk    rt          d          t          j        |          s|g}d }t                              | j        |          }|| j        d         }| j        t          | j                  dz
           }|                     ||          r|}n|                     ||          r|}ni }t          dt          | j                  dz
            D ]}| j        |         }|
                                }	|	d         }
|	t          |	          dz
           }| j        |dz            }|
                                }|d         }|t          |          dz
           }t          |          dk     rt          j        |d                   st          j        ||
          |g|t          |          dz   <   t          j        ||          |g|t          |          dz   <   t          j        ||          |g|t          |          dz   <   t          j        ||          |g|t          |          dz   <   _|d         }t          j        ||
          |g|t          |          dz   <   t          j        ||          |g|t          |          dz   <   t          j        ||          |g|t          |          dz   <   t          j        ||          |g|t          |          dz   <   t          t          |                                d	 
                    }|t!          t#          |                             d         }||j                            |          }||fS )Nr   r0   r   r   _currentSegmentStart_currentSegmentEnd_nextSegmentStart_nextSegmentEndc                     | d         d         S )Nr   r   r<   )items    r   <lambda>z.TW_Segment_Regressor.predict.<locals>.<lambda>#  s    jnopjqrsjt r   )key)r   r1   r   r   r   r:   r   rH   rS   r   r!   rA   strdictsorteditemsnextiterr$   r%   )r   r   predictedValuer2   rC   rK   euclidianDistancesr+   r   currentSegmentPointscurrentSegmentFirstPointcurrentSegmentLastPointnextSegmentnextSegmentPointsnextSegmentFirstPointnextSegmentLastPointr6   euclidianDistanceMinRangeErrorss                     r   r%   zTW_Segment_Regressor.predict   s   q!! 	xc!ffqj 	xvwww"1%% 	A-JJ4=Z[\\ :	d=+L-DM(:(:A(=>K33L!DD 6d!-33KCC 4d!,%'"qT]!3!3Q!677 - -A%)]1%5N+9+>+>+@+@(/CA/F,.B3G[C\C\^_C_.`+"&-A"6K(3(8(8(:(:%,=a,@)+<SAR=S=SUV=V+W(1vvz "**=ad*C*C "&9!=UVV*O*3q664J+JK
 '9!=TUU*M*3q664H+HI
 '9!=RSS'L*3q664G+GH
 '9!=QRR'J*3q664E+EFF
 *+1&9.Jbcc*O*3q664J+JK
 '9.Jabb*M*3q664H+HI
 '9.J_``'L*3q664G+GH
 '9.J^__'J*3q664E+EFF
 37v>P>V>V>X>X^t^t7u7u7u2v2v/!3D>]9^9^4_4_!`ab!c 	B+6>>qAAN~--r   &.>c                    t          j        | t                    } t          j        |t                    }t          j        |t                    }t          j        |t                    }|| z
  }||z
  }|| z
  }|d         |d         z  |d         |d         z  z
  }t          |          |k     rX|d         |d         z  |d         |d         z  z
  }	t          |	          |k     rt          j        ||          }
|
|k     rPt           j                            || z
            |k     r(t           j                            || z
            |k     r| ndS d}d}t          j        || z
  |          |
z  }t          j        || z
  |          |
z  }t          ||          }t          ||          }t          ||          }t          ||          }|||z   k    r/t          ||z
            |k     r	| ||z  z   gS | ||z  z   | ||z  z   gS dS dS |d         |d         z  |d         |d         z  z
  |z  }|d         |d         z  |d         |d         z  z
  |z  }d|z
  |cxk    r	d|z   k    r#n n d|z
  |cxk    r	d|z   k    rn n
| ||z  z   }|S dS u  
        Calcule le point d'intersection de deux segments 2D.
        Renvoie le point d'intersection [x, y] si une intersection unique existe.
        Renvoie None si les segments sont parallèles, colinéaires (non chevauchants) ou ne se coupent pas.
        Renvoie une liste de deux points si les segments sont colinéaires et se chevauchent (le segment d'intersection).
        dtyper   r   N	r   r    floatr&   dotlinalgnormminmaxp1p2p3p4epsilonv1v2dpdetdet_collinear	len_sq_v1t_min_s1t_max_s1	s_proj_p3	s_proj_p4min_s2max_s2overlap_minoverlap_maxtuintersection_points                         r   intersect_segments_2dz*TW_Segment_Regressor.intersect_segments_2dZ      Xb&&&Xb&&&Xb&&&Xb&&& "W"W "W ebembebem+ s88g 2	 qEBqEMBqEBqEM9M=!!G+ - F2rNN	w& s!#R!8!87!Brry~~VX[]V]G^G^ahGhr22nrrF27B//);	F27B//);	
 Y	22Y	22 "(F33 "(F33 +"77   ;455? N "[2%5 566 "[2%5 5rK"<L7LMM4t
 URU]RURU]*c1URU]RURU]*c1
 w;! 	 	 	 	q7{ 	 	 	 	 	q7{a 	 	 	 	1w; 	 	 	 	 	!#a"f%% 4r   c                 :   t          j        |           } t          j        |          }t          j        |          }t          j        |          }|| z
  }||z
  }t          j        ||          }t           j                            |          dk     r| |z
  }t          j        ||          }t           j                            |          dk     rKd}	t          j        ||          dk    r.t          j        || z
  |          t          j        ||          z  nd}
t          j        ||          dk    r.t          j        || z
  |          t          j        ||          z  nd}t          j        ||          dk    r.t          j        || z
  |          t          j        ||          z  nd}t          |	|
g          \  }}t          ||g          \  }}t          ||          }t          ||          }||k    r||k    r| ||z  z   S | ||z  z   | ||z  z   gS dS dS t          j        || z
  |          }t          |          dk    rdS t          j        |d         |d          g|d         |d          gg          }t          j        |d         | d         z
  |d         | d         z
  g          }	 t           j        
                    ||          }|d         }|d         }n# t           j        j        $ r t          j        |d         |d          g|d         |d          gg          }t          j        |d         | d         z
  |d         | d         z
  g          }	 t           j        
                    ||          }|d         }|d         }n# t           j        j        $ r Y Y dS w xY wY nw xY wt          j        | d         ||d         z  z   |d         ||d         z  z             sdS d|cxk    rdk    rn nd|cxk    rdk    rn n
| ||z  z   }|S dS u  
        Calcule le point d'intersection de deux segments 3D, s'il existe.

        Args:
            p1 (np.array): Premier point du premier segment [x1, y1, z1].
            p2 (np.array): Deuxième point du premier segment [x2, y2, z2].
            p3 (np.array): Premier point du deuxième segment [x3, y3, z3].
            p4 (np.array): Deuxième point du deuxième segment [x4, y4, z4].

        Returns:
            np.array or None: Le point d'intersection si les segments se coupent,
                            sinon None.
        rm           r   Nr   r   r   r    crossru   rv   rt   r_   rx   rw   r&   solveLinAlgErroriscloserz   r{   r|   r}   r   r   cross_product_vv_p1_p3collinear_checkt0t1	t_p3_proj	t_p4_proj
t_seg1_min
t_seg1_max
t_seg2_min
t_seg2_maxr   r   mixed_productABt_ur   r   A_yzB_yzr   s                               r   intersect_segments_3dz*TW_Segment_Regressor.intersect_segments_3d      Xb\\Xb\\Xb\\Xb\\ "W"W (2r**9>>/**T1 )	 2gG hw33Oy~~o..5 # =?VB^^q=PYRVBGR((26"b>>99VYDFF2rNNVWDW`BF27B//"&R..@@]`	DFF2rNNVWDW`BF27B//"&R..@@]`	 *0R)9)9&
J)/I0F)G)G&
J "*j99!*j99+-  
 #k1 N!K"$444 !#[2%5 5rK"<L7LMM4t
 R/::}$ 	4 HURUFOURUFO
  
 HqEBqEMqEBqEM
  	)//!Q''CAAAAAy$ 	 	 	 8AAAA  D 81111  DioodD11FF9(   ttt #	2 z"Q%!be)+RUQAY->?? 	4  	 	 	 	Q 	 	 	 	 	1 	 	 	 	Q 	 	 	 	 	!#a"f%% 47   50K& &A<N3#0NN3N-(N3,N--N32N3c                 .   t          j        | t                    } t          j        |t                    }t          j        |t                    }t          j        |t                    }| j        |j        cxk    r|j        cxk    r|j        k    sn t	          d          | j        d         }|| z
  }||z
  }t          j        || f          }|| z
  }	 t           j                            ||d          \  }	}
}}|	d         }|	d         }n# t           j        j        $ r Y dS w xY w|
j	        dk    r%t           j        
                    |
          dk    rdS |dk     r| |z
  }t          j        ||f          }t           j                            |d	          dk    rDt          j        ||          }|dk     rbt           j        
                    || z
            dk     r(t           j        
                    || z
            dk     r| S t           j        
                    || z
            dk     rbdt          j        ||z
  |          t          j        ||          z  cxk    rdk    r(n nt           j        
                    |          dk    r| S nt           j        
                    || z
            dk     rddt          j        ||z
  |           t          j        | |           z  cxk    rdk    r(n n%t           j        
                    |          dk    r| S dS d
}t          j        || z
  |          |z  }t          j        || z
  |          |z  }t          j        || z
  |          |z  }t          ||g          \  }}t          ||g          \  }}t          ||          }t          ||          }||dz   k    r t!          ||z
            dk     r| ||z  z   S dS dS dS d|cxk    rdk    rn nd|cxk    rdk    rn n
| ||z  z   }|S dS u  
        Calcule le point d'intersection de deux segments en N dimensions, s'il existe.

        Args:
            p1 (np.array): Premier point du premier segment.
            p2 (np.array): Deuxième point du premier segment.
            p3 (np.array): Premier point du deuxième segment.
            p4 (np.array): Deuxième point du deuxième segment.

        Returns:
            np.array or None: Le point d'intersection si les segments se coupent,
                            sinon None.
        rp   u1   Tous les points doivent avoir la même dimension.r   N)rcondr   rm   r   )tolr   g&.g0D   ?r   r    rs   r   
ValueErrorcolumn_stackru   lstsqr   sizerv   matrix_rankrt   r_   rx   rw   r&   rz   r{   r|   r}   n_dimr   r   r   r   r   	residualsranksr   r   r   test_collinear_matrix
v1_norm_sqr   r   r   r   r   r   r   r   r   r   r   s                                r   intersect_segments_ndz*TW_Segment_Regressor.intersect_segments_nde      Xb&&&Xb&&&Xb&&&Xb&&& BH 	R 	R 	R 	R 	R 	R 	R 	RBH 	R 	R 	R 	RPQQQ "W"W OR"I&&G
		 ')iooa$o&G&G#CD!AAAAAy$ 	 	 	 44	 >A 	")..";";d"B 	4
 !8 1	 2gG %'ORM$B$B!y$$%:$EEJ %  VB^^
$ 	 y~~b2g..5 &")..b:Q:QTX:X &!	R0047 &rBw 3 3bfRnn D & & & & & & & & &binn]_N`N`cgNg &#%IR0047 &rBw 4 4rvrcB37G7G G & & & &1 & & & & &QSQZQ_Q_`bQcQcfjQj &#%I4VBGR((:5F27B//*<	F27B//*<	)/R)9)9&
J)/I0F)G)G&
J!*j99!*j99+"44  ;455< $!K"$444#t4t
 q 	 	 	 	H 	 	 	 	 	Q 	 	 	 	( 	 	 	 	 	!#a"f%% 4   7D D*)D*c                 2   d}t          | j                  D ]j\  }}| j        |         j                            | j        |                                         | j        |                                                   }||z   }k|t          | j                  z  S )Nr   )	enumerater   r$   lossFunctionr!   r"   r   )r   
totalErrorr+   segmentsegmentErrors        r   r   zTW_Segment_Regressor.totalError  s    
#DM22 	3 	3JAw=+6CCDMRSDTDYDYD[D[]a]jkl]m]r]r]t]tuuL#l2JJC....r   c                    t          j        | t                    } t          j        |t                    }t          j        |t                    }t          j        |t                    }|| z
  }||z
  }|| z
  }|d         |d         z  |d         |d         z  z
  }t          |          |k     rX|d         |d         z  |d         |d         z  z
  }	t          |	          |k     rt          j        ||          }
|
|k     rPt           j                            || z
            |k     r(t           j                            || z
            |k     r| ndS d}d}t          j        || z
  |          |
z  }t          j        || z
  |          |
z  }t          ||          }t          ||          }t          ||          }t          ||          }|||z   k    r/t          ||z
            |k     r	| ||z  z   gS | ||z  z   | ||z  z   gS dS dS |d         |d         z  |d         |d         z  z
  |z  }|d         |d         z  |d         |d         z  z
  |z  }d|z
  |cxk    r	d|z   k    r#n n d|z
  |cxk    r	d|z   k    rn n
| ||z  z   }|S dS ro   rr   ry   s                         r   r   z*TW_Segment_Regressor.intersect_segments_2d  r   r   c                 :   t          j        |           } t          j        |          }t          j        |          }t          j        |          }|| z
  }||z
  }t          j        ||          }t           j                            |          dk     r| |z
  }t          j        ||          }t           j                            |          dk     rKd}	t          j        ||          dk    r.t          j        || z
  |          t          j        ||          z  nd}
t          j        ||          dk    r.t          j        || z
  |          t          j        ||          z  nd}t          j        ||          dk    r.t          j        || z
  |          t          j        ||          z  nd}t          |	|
g          \  }}t          ||g          \  }}t          ||          }t          ||          }||k    r||k    r| ||z  z   S | ||z  z   | ||z  z   gS dS dS t          j        || z
  |          }t          |          dk    rdS t          j        |d         |d          g|d         |d          gg          }t          j        |d         | d         z
  |d         | d         z
  g          }	 t           j        
                    ||          }|d         }|d         }n# t           j        j        $ r t          j        |d         |d          g|d         |d          gg          }t          j        |d         | d         z
  |d         | d         z
  g          }	 t           j        
                    ||          }|d         }|d         }n# t           j        j        $ r Y Y dS w xY wY nw xY wt          j        | d         ||d         z  z   |d         ||d         z  z             sdS d|cxk    rdk    rn nd|cxk    rdk    rn n
| ||z  z   }|S dS r   r   r   s                               r   r   z*TW_Segment_Regressor.intersect_segments_3d  r   r   c                 .   t          j        | t                    } t          j        |t                    }t          j        |t                    }t          j        |t                    }| j        |j        cxk    r|j        cxk    r|j        k    sn t	          d          | j        d         }|| z
  }||z
  }t          j        || f          }|| z
  }	 t           j                            ||d          \  }	}
}}|	d         }|	d         }n# t           j        j        $ r Y dS w xY w|
j	        dk    r%t           j        
                    |
          dk    rdS |dk     r| |z
  }t          j        ||f          }t           j                            |d	          dk    rDt          j        ||          }|dk     rbt           j        
                    || z
            dk     r(t           j        
                    || z
            dk     r| S t           j        
                    || z
            dk     rbdt          j        ||z
  |          t          j        ||          z  cxk    rdk    r(n nt           j        
                    |          dk    r| S nt           j        
                    || z
            dk     rddt          j        ||z
  |           t          j        | |           z  cxk    rdk    r(n n%t           j        
                    |          dk    r| S dS d
}t          j        || z
  |          |z  }t          j        || z
  |          |z  }t          j        || z
  |          |z  }t          ||g          \  }}t          ||g          \  }}t          ||          }t          ||          }||dz   k    r t!          ||z
            dk     r| ||z  z   S dS dS dS d|cxk    rdk    rn nd|cxk    rdk    rn n
| ||z  z   }|S dS r   r   r   s                                r   r   z*TW_Segment_Regressor.intersect_segments_nd&  r   r   c                     d S r
   r<   )r   s    r   _extendSegmentIfNecessaryz.TW_Segment_Regressor._extendSegmentIfNecessary  s    tr   )NN)rm   )__name__
__module____qualname__r   r#   staticmethodr:   r>   rH   rS   r%   r   r   r   r   r   r<   r   r   r   r      sN       ! ! !6 6 6 6p)V   \B	 	 	, , ,2* * *>I. I. I.V,^[ [ [ [zDJ J JX} } }~/ / /,^[ [ [ [zDJ J JX} } }~    r   r   )numpyr   matplotlib.pyplotpyplotpltmathoperatorclasses.TW_Utilityr   classes.TW_Segmentr   r   classes.TW_Linear_regressionr   warningsr   r<   r   r   <module>r      s                ) ) ) ) ) ) ' ' ' ' ' ' ) ) ) ) ) ) = = = = = = Y Y Y Y Y Y Y Y Y Yr   