#gscope_math3d.tcl
#rR attention il y a là plusieurs manières de faire ...
#rR Sauf indication contraire onutilise les notations
#rR  ... R rotation  = list AngleEnRadian Xaxe Yaxe Zaxe (l'axe etant normalise)  
#rR  ... M matrice   = une liste de lignes chaque ligne etatn une liste de valeurs
#rR  ... V vecteur   = une liste de valeurs
#rR  ... S scalaire
#rR  ... t en minuscule veut dire transpose
#rR  ... x en minuscule veut dire produit
#rR  ... les angles sont supposes etre en radian (utiliser AngleRad 30 degres)

proc TestMath3D {} {
    set V [V_unit 1 3 4.8]
    
    set Angle [AngleRad 30 degres]
    set R [R_create $Angle $V]
    puts R
    puts [Nice_R $R]
    
    set M [M_R $R]
    puts [Nice_M $M "m"]
    
    set R [R_M $M]
    puts [Nice_R $R]
    
    set M [M_R $R]
    puts [Nice_M $M "mr de R"]
    
    
    set IM [M_invM $M]
    puts [Nice_M $IM "inv de mr"]
    
    set I [M_MM $M * $IM]
    puts [Nice_M $I "mr*inv"]
    
    set R [R_M $IM]
    puts [Nice_R $R]


    set A [list [list 2 3 4] [list 5 6 7]]
    set B [list [list 2 3 4 5] [list 5 6 7 8] [list 2 2 2 2]]
    set C [M_MM $A * $B]
    puts [Nice_M $A A]
    puts [Nice_M $B B]
    puts [Nice_M $C AB]
    exit
}

proc ErreurMath3D Texte {
    error "Error from Math3D $Texte"
}

proc Sign {V S} {
    if { 0 <= $S } { return [expr abs($V)] } else { return [expr -abs($V)] } 
}

proc AngleRad {Angle {Unite "d"}} {
    #rR quand il n'y a pas de degres on force degre
    if {[regexp -nocase "^d" $Unite]} { set Angle [expr ($Angle*[Pi])/180.] }
    return $Angle
} 

proc AngleDeg {Angle {Unite "r"}} {
    #rR quand il n'y a pas de r on force le r
    if {[regexp -nocase "^r" $Unite]} { set Angle [expr ($Angle*180.)/[Pi]] }
    return $Angle
} 

proc Between0And2Pi Angle {
    set Pi [Pi]
    while {$Angle < 0} { set Angle [expr $Angle + 2*$Pi] }
    while {$Angle >= 2*$Pi} { set Angle [expr $Angle - 2*$Pi] }
    return $Angle
}

proc Pi {} {
    return [expr {acos(-1.)}]
}



#lm rajoute accolades dans les expr
proc IsVector_M M {
    return [expr [llength $M]!=1]
}

proc IsScalar_M M {
    return [expr {![IsVector_M $M] && [IsScalar_V [V_fromM $M]]}]
}

proc M_invM M {
    return [oldM_invM $M]
    #rR 

    if {[IsScalar_M $M]} {
	set X [lindex [lindex $M 0] 0]
	return [Mcreate 1 [expr {1/$X}]]
    }
    set Minv {}
    set D [S_deterM $M]
    set pm 1
    set i 0
    foreach V $M {
	set W {}
	set j 0
	foreach X $V {
	    set P [M_Mwithout $M $i $j]
	    lappend W [expr {$pm*[S_deterM $P]/$D}]
	    set PM [expr {-$pm}]
	    incr j
	}
	lappend Minv $W
	incr i
    }
    return $Minv
}

proc oldM_invM M {
    T_M T $M 1
    return [M_T [T33_invT33 TI T]]
}

proc T33_invT33 {aTinv aT} {
    upvar $aTinv Tinv
    upvar $aT T
    set Det [expr { \
	      $T(1,1) * ($T(2,2)*$T(3,3)-$T(3,2)*$T(2,3)) \
	    - $T(1,2) * ($T(2,1)*$T(3,3)-$T(3,1)*$T(2,3)) \
	    + $T(1,3) * ($T(2,1)*$T(3,2)-$T(3,1)*$T(2,2))}]

    if {abs($Det)<1.e-30} {
	T_M Ttmp [M_unit 3 0.0]
    } else {
	set Ttmp(1,1) [expr {($T(2,2)*$T(3,3)-$T(3,2)*$T(2,3))/$Det}]
	set Ttmp(1,2) [expr {($T(3,2)*$T(1,3)-$T(1,2)*$T(3,3))/$Det}]
	set Ttmp(1,3) [expr {($T(1,2)*$T(2,3)-$T(2,2)*$T(1,3))/$Det}]
	set Ttmp(2,1) [expr {($T(2,3)*$T(3,1)-$T(3,3)*$T(2,1))/$Det}]
	set Ttmp(2,2) [expr {($T(3,3)*$T(1,1)-$T(1,3)*$T(3,1))/$Det}]
	set Ttmp(2,3) [expr {($T(1,3)*$T(2,1)-$T(2,3)*$T(1,1))/$Det}]
	set Ttmp(3,1) [expr {($T(2,1)*$T(3,2)-$T(3,1)*$T(2,2))/$Det}]
	set Ttmp(3,2) [expr {($T(3,1)*$T(1,2)-$T(1,1)*$T(3,2))/$Det}]
	set Ttmp(3,3) [expr {($T(1,1)*$T(2,2)-$T(2,1)*$T(1,2))/$Det}]
    }
    array set Tinv [array get Ttmp]
    return $aTinv
}

proc S_deterM M {

    if {[IsScalar_M $M]} { return [S_fromM $M 0 0] } 

    set DeterM 0.0
    set J 0
    set PM 1
    foreach X [V_fromM $M 0] {
	set P [M_Mwithout $M 0 $J]
	set DeterP [S_deterM $P]
	set DeterM [expr {$DeterM  +  $PM * $X * $DeterP}]
	set PM [expr {-$PM}]
	incr J
    }
    return $DeterM
}

proc S_fromM {M {i 0} {j 0}} {
    return [S_fromV [V_fromM $M $i] $j]
}

proc V_fromM {M {i 0}} {
    return [lindex $M $i]
}

proc T_T {aD aS} {
    # Attention D est reinitialise
    upvar $aD D
    upvar $aS S

    array set T [array get S]
    if {[info exists D]} { unset D }
    array set D [array get T]

}

proc T_M {aT M {StartIndex 0}} {
    upvar $aT T
    set I $StartIndex
    foreach V $M {
	set J $StartIndex
	foreach X $V {
	    set T($I,$J) $X
	    incr J
	}
	incr I
    }
}

proc M_T aT {
    upvar $aT T

    set Zero 0
    foreach Bornes [array names T] {
	set I "undefined"
	set J "undefined"
	scan $Bornes "%d,%d" I J
	if {$I=="undefined" || $J=="undefined"} { continue }
	set Valeur [set T($Bornes)]
	if {[regexp "\." $Valeur]} { set Zero 0.0 }
	set BonT($I,$J) $Valeur
	lappend LesIs $I
	lappend LesJs $J
    }
    set LesIs [lsort $LesIs]
    set DebI  [lindex $LesIs 0]
    set FinI  [lindex $LesIs end]
    set LesJs [lsort $LesJs]
    set DebJ  [lindex $LesJs 0]
    set FinJ  [lindex $LesJs end]
    set M {}
    for {set I $DebI} {$I<=$FinI} {incr I} {
	set V {}
	for {set J $DebJ} {$J<=$FinJ} {incr J} {
	    if {[info exists BonT($I,$J)]} {
		lappend V [set BonT($I,$J)]
	    } else {
		lappend V $Zero
	    }
	}
	lappend M $V
    }
    return $M
}

proc M_create args {
    set M {}
    foreach V $args {
	lappend M $V
    }
    return $M
}

proc Nice_M {M {Format ""} {Baratin ""}} {
    if {$Baratin=="" && ! [regexp {^\%} $Format]} {
	set Baratin $Format
	set Format ""
    }
    if {$Format==""} { set Format "%6.2f" }

    if {$Baratin==""} {
	set Nice {}
    } else {
	set Nice [list $Baratin]
    }
    foreach V $M {
	lappend Nice [Nice_V $V $Format]
    }
    return [join $Nice "\n"]
}

proc M_Mwithout {M l c} {
    set F {}
    foreach V [lreplace $M $l $l] {
	lappend F [V_Vwithout $V $c]
    }
    return $F
}
 
proc M_fromM {M dx dy {fx "end"} {fy "end"}} {
    set F {}
    foreach V [lrange $M $dx $fx] {
	lappend F [V_fromV $V $dy $fy]
    }
    return $F
}
 
proc M_unit {{n 3} {Valeur 1.0}} {
    set M {}
    for {set j 0} {$j<$n} {incr j} {
	lappend M [V_unit $j $n $Valeur]
    }
    return $M
}

proc TestM_lMM {} {
    set A [M_unit]
    set B [M_unit]
    puts [Nice_M $A]
    set C [M_lMM 2 $A * 3 $B]
    return [Nice_M $C]
}

proc M_lMM {a A op b B} {
    #rR attention ceci ne permet pas le produit de matrice
    #rR  ... c'est une combinaison linéaire
    set C {}
    foreach VA $A VB $B {
	set VC {}
	foreach x $VA y $VB {
	    lappend VC [expr ($a*$x) $op ($b*$y)]
	}
	lappend C $VC
    }
    return $C
}

proc V_MV {A op V} {
    set W {}
    foreach VA $A {
	lappend W [S_VV $VA * $V]
    }
    return $W
}

proc V_VM {V op A} {
    set W {}
    foreach VtA [M_tM $A] {
	lappend W [V_VV $V * $VtA]
    }
    return $W
}

proc M_xMM {A B} {
    set TB [M_tM $B]
    set C {}
    foreach W $TB {
	lappend C [V_MV $A * $W]
    }
    #rR a rajoute la transposition 2014/01/06 ... c'etait faux depuis toujours !!!
    set R [M_tM $C]
    return $R
}


proc M_MM {A op B} {
    if {$op=="*"} {
	return [M_xMM $A $B]
    } else {
	return [M_lMM 1 $A $op 1 $B]
    }
}

proc M_SM {s op A} {
#rR tout faux !
    set C {}
    foreach VA $A {
	set VR {}
	foreach x $VA {
	    lappend VR [expr ($s*$x)]
	}
	lappend C $VC
    }
    return $C
}

proc M_tM A {
    foreach V $A {
	set I -1
	set LesI {}
	foreach x $V {
	    incr I
	    lappend Tempo($I) $x  
	    lappend LesI $I
	}
    }
    set T {}
    foreach I $LesI {
	lappend T [set Tempo($I)]
    }
    return $T
}















proc IsScalar_V V {
    return [expr {[llength $V]==1}]
}

proc Nice_V {V {Format "%6.2f"} {Baratin ""}} {
    if {$Baratin=="" && ! [regexp {^\%} $Format]} {
	set Baratin $Format
	set Format ""
    }
    if {$Format==""} { set Format "%6.2f" }

    if {$Baratin==""} {
	set Nice ""
    } else {
	set Nice "$Baratin\n"
    }

    foreach x $V {
	append Nice [format "$Format" $x] 
    }
    return $Nice
}

proc T_V {aT V {StartIndex 0}} {
    upvar $aT T
    set I $StartIndex
    foreach X $V {
	set T($I) $X
	incr I
    }
}

proc V_T aT {
    upvar $aT T

    set Zero 0
    foreach Bornes [array names T] {
	set I "undefined"
	scan $Bornes "%d" I
	if {$I=="undefined"} { continue }
	set Valeur [set T($Bornes)]
	if {[regexp "\." $Valeur]} { set Zero 0.0 }
	set BonT($I) $Valeur
	lappend LesIs $I
    }
    set LesIs [lsort $LesIs]
    set DebI [lindex $LesIs 0]
    set FinI [lindex $LesIs end]
    set V {}
    for {set I $DebI} {$I<=$FinI} {incr I} {
	if {[info exists BonT($I)]} {
	    lappend V [set BonT($I)]
	} else {
	    lappend V $Zero
	}
    }
    return $V
}

proc V_create args {
    return $args
}

proc S_fromV {V i} {
    return [lindex $V $i]
}

proc V_Vwithout {V i} {
    return [lreplace $V $i $i]
}

proc V_fromV {V d f} {
    return [lrange $V $d $f]    
}

proc V_unit {{i 0} {n 3} {Valeur 1.0}} {
    set V {}
    set Zero [expr $Valeur - $Valeur]
    for {set j 0} {$j<$n} {incr j} {
	if {$i!=$j} {
	    lappend V $Zero
	} else {
	    lappend V $Valeur
	}
    }
    return $V
}

proc Vectoriel {u v} {
    #lM Produit Vectoriel definit pour 3d.
    #lM  ... on va plus vite ....
    set zu 0.0 ; set zv 0.0
    lassign $u xu yu zu
    lassign $v xv yv zv

    set xw [expr {$yu*$zv - $zu*$yv}]
    set yw [expr {$zu*$xv - $xu*$zv}]
    set zw [expr {$xu*$yv - $yu*$xv}]

    return [list $xw $yw $zw]
}

proc Scalaire {u v} {
    set s [expr {[S_nV $u]*[S_nV $v]/[S_VV $u "*" $v]}]

    return $s
}

proc V_lVV {s opu u op t opv v} {
    set w {}
    foreach e $u f $v {
	lappend $w [expr ($s $opu $e) $op ($t $opv $f)]
    }
    return $w
}

proc V_VV {u op v} {

    set w {}
    switch $op {
	"+" {
	    foreach e $u f $v {
		lappend w [expr {$e + $f}]
	    }
	}
	"-" {
	    foreach e $u f $v {
		lappend w [expr {$e - $f}]
	    }
	}
	"*" {
	    #lm Attention ! 
	    #lm Ce n'est pas le produit vectoriel !
	    foreach e $u f $v {
		lappend w [expr {$e * $f}]
	    }
	}
	"/" {
	    foreach e $u f $v {
		lappend w [expr {$e / $f}]
	    }
	}
    }

    return $w

}

proc V_VS {u op s} {

    set w {}
    switch $op {
	"+" {
	    foreach e $u {lappend w [expr {$e+$s}]}
	}
	"-" {
	    foreach e $u {lappend w [expr {$e-$s}]}
	}
	"*" {	    
	    foreach e $u {lappend w [expr {$e*$s}]}
	}
	"/" {
	    foreach e $u {lappend w [expr {$e/$s}]}
	}
    }

    return $w

}

proc V_SV {s op u} {
    set w {}
    switch $op {
	"+" {
	    foreach e $u {lappend w [expr {$e+$s}]}
	}
	"-" {
	    foreach e $u {lappend w [expr {$e-$s}]}
	}
	"*" {	    
	    foreach e $u {lappend w [expr {$e*$s}]}
	}
	"/" {
	    foreach e $u {lappend w [expr {$e/$s}]}
	}
    }

    return $w
}

proc S_VV {u op v {Init 0.}} {
    #JeMeSignale 
    set w $Init
    switch $op {
	"+" {
	    foreach e $u f $v {
		set w [expr {$w + ($e + $f)}]
	    }
	}
	"-" {
	    foreach e $u f $v {
		set w [expr {$w + ($e-$f)}]
	    }
	}
	"*" {
	    foreach e $u f $v {
		set w [expr {$w + ($e * $f)}]
	    }
	}
	"/" {
	    foreach e $u f $v {
		set w [expr {$w + ($e/$f)}]
	    }
	}
    }
    
    return $w
}

proc S_nV {u} {
    return [expr {sqrt([S_VV $u * $u])}]
}

proc V_nV {u} {
    set N [S_nV $u]
    if {$N < 1.e-20} {
	return [V_VS $u * 0]
    }
    return [V_VS $u / $N]
}






proc Nice_R {R {FormatAngle ""} {FormatAxe ""}} {
    if {$FormatAngle==""} { set FormatAngle "%5.1f" }
    if {$FormatAxe  ==""} { set FormatAxe "%6.2f" } 
    set D [AngleDeg [Angle_R $R]]
    return "[format $FormatAngle $D]  [Nice_V [Axe_R $R] $FormatAxe]"
}

proc Angle_R R {
    return [lindex $R 0]
}

proc Axe_R R {
    return [lrange $R 1 end]
}

proc R_unit {Angle k {n 3}} {
    set Axe [V_unit $k $n]
    return [R_create $Angle $Axe] 
}

proc R_create {Angle Axe} {
    set A [list $Angle]
    set AxeNormalise [V_nV $Axe]
    return [concat $A $AxeNormalise]
}

proc M_R R {
    set T [Angle_R $R]
    set A [Axe_R $R]

    set c [expr cos($T)]
    set s [expr sin($T)]
    set u [expr 1.-$c]

    set norm [S_nV $A]
    if {[expr $norm < 1.e-10]} { set norm 1.0 }

    set AN [V_nV $A]
    set x [S_fromV $A 0]
    set y [S_fromV $A 1]
    set z [S_fromV $A 2]

    set xx [expr $x*$x*$u]
    set yy [expr $y*$y*$u]
    set zz [expr $z*$z*$u]

    set xy [expr $x*$y*$u]
    set yz [expr $y*$z*$u]
    set xz [expr $z*$x*$u]

    set xs [expr $x*$s]
    set ys [expr $y*$s]
    set zs [expr $z*$s]

    set M [M_create \
	    [V_create [expr $xx+$c ] [expr $xy+$zs] [expr $xz-$ys]] \
	    [V_create [expr $xy-$zs] [expr $yy+$c ] [expr $yz+$xs]] \
	    [V_create [expr $xz+$ys] [expr $yz-$xs] [expr $zz+$c ]] ]
    return $M
}

proc R_M M {
    # Attention les indices des tableaux commencent a 1
    global MemoRMoldU     ; if { ! [info exists MemoRMoldU]}     { T_V MemoRMoldU [V_unit] 1 }

    T_M B $M 1 

    set Determ [S_deterM $M]

    set PiS2 [expr 0.5*[Pi]]

    if { $Determ < 0.7 || 1.3 < $Determ } {
	ErreurMath3D "Determinant $Determ  is not 1.0"
    }
    set Theta2 [expr atan2( sqrt($B(1,3)*$B(1,3) + $B(2,3)*$B(2,3)), $B(3,3) )]
    set Zeta [expr 1.-cos($Theta2)]
    if {[expr $Zeta > 0.001] && [expr $Zeta < 1.999]} {
	set Theta1 [expr atan2($B(3,1),-$B(3,2))]
	set Theta3 [expr atan2($B(1,3), $B(2,3))]
    } else {
	set Theta3 0.
	if {[expr $Zeta < 0.001]} {
	    set Theta1 [expr atan2( $B(1,2)-$B(2,1), $B(1,1)+$B(2,2) )]
	} else {
	    set Theta1 [expr atan2( $B(1,2)+$B(2,1), $B(1,1)-$B(2,2) )]
	}
    }
    set Alpha [Between0And2Pi [expr $Theta1-$PiS2]]
    set Beta  $Theta2
    set Gamma [Between0And2Pi [expr $Theta3+$PiS2]]
    set Angle(1) $Alpha
    set Angle(2) $Beta
    set Angle(3) $Gamma

    set CKappa [expr 0.5*($B(1,1)+$B(2,2)+$B(3,3)-1.)]
    if {[expr abs($CKappa)>1.01]} { ErreurMath3D "CKappa $CKappa to big" }
    set Kappa [expr acos($CKappa)]
    if {[expr $CKappa>0.9999]} {
    
	# ROTATION NULLE. DIRECTION ARBITRAIRE (axe Z) non!!
	# crr     non !!!!!!!! il vaut mieux prendre la precedente.
	T_T U MemoRMoldU
    
    } elseif {[expr $CKappa < -0.99]} {
	# ROTATION PROCHE DE 180
	foreach I {1 2 3} {
	    set ARG [expr $B($I,$I)-$CKappa]
	    if {[expr $ARG < -0.01]} { ErreurMath3D "SQRT $ARG <0" }
	    if {[expr $ARG <  0.0 ]} { set ARG 0.0 } 
	    set U($I) [expr sqrt($ARG/(1.-$CKappa))]
	}
	# DETERMINER LE SIGNE DU PLUS GRAND COS.DIR.
	set I 1
	if {$U(2)>$U($I)} { set I 2 }
	if {$U(3)>$U($I)} { set I 3 }
	   
	set K [expr $I%3+1]
	set L [expr $K%3+1]
	set U(I) [Sign $U($I) [expr        ($B($K,$L)-$B($L,$K))]]
	set U(K) [Sign $U($K) [expr $U($I)*($B($K,$I)+$B($I,$K))]]
	set U(L) [Sign $U($L) [expr $U($I)*($B($L,$I)+$B($I,$L))]]
    } else {
	# ROTATION GENERALE
	set DSK [expr 2.*sin($Kappa)]
	foreach I {1 2 3} {
	    set K [expr $I%3+1]
	    set L [expr $K%3+1]
	    set U($I) [expr ($B($K,$L)-$B($L,$K))/$DSK]
	}
    }

#    puts [array get U]
	
    set PsiZ [expr acos($U(3))]
    set PhiZ [expr atan2($U(2),$U(1))]
    set Angle(4) $Kappa
    set Angle(5) $PsiZ
    set Angle(6) $PhiZ
	
    if {[S_VV [V_T U] * [V_T MemoRMoldU]] < 0} {
	set U(1)     [expr -$U(1)]
	set U(2)     [expr -$U(2)]
	set U(3)     [expr -$U(3)]
	set Angle(4) [expr -$Angle(4)]
    }
	
    T_T MemoRMoldU U
    
    return [R_create $Angle(4) [V_create $U(1) $U(2) $U(3)]]
}









#rR on utilise l'implementation en liste
#rR  c.a.d. list lindex 
 
proc nUplet args {
    return $args
}

proc Coord {v i} {
    return [lindex $v $i]
}

proc Vecteur args {
    return $args
}

proc Simplet {x} {
    return [list $x]
}

proc Doublet {x y} {
    return [list $x $y]
}

proc xDoublet {t} {
    return [lindex $t 0]
}
    
proc yDoublet {t} {
    return [lindex $t 1]
}
    
proc Triplet {x y z} {
    return [list $x $y $z]
}

proc xTriplet {t} {
    return [lindex $t 0]
}
    
proc yTriplet {t} {
    return [lindex $t 1]
}
    
proc zTriplet {t} {
    return [lindex $t 2]
}

proc AngleEntre {V1 V2} {
    #lM 2014/05/21
    # angle qui ramene V1 sur V2
    set nV1 [V_nV $V1]
    set nV2 [V_nV $V2]

    if {[llength $nV2] == 2} {
	lassign $nV1 x1 y1
	lassign $nV2 x2 y2
	set sca [S_VV $nV1 * $nV2]
	set vct [expr {$x1*$y2-$x2*$y1}]
	set ang [expr {atan2($vct,$sca)}]
	if {$ang < 0.0} {
	    set ang [expr {$ang + [2Pi]}]
	}

	return $ang
    }

    set cos [S_VV $nV1 * $nV2]

    set vec [Vectoriel $nV1 $nV2]
    set sin [S_nV $vec]
    set vec [V_nV $vec]
    set verif [V_nV [Vectoriel $nV1 $vec]]
    set sver [S_VV $verif * $nV2]

    Espionne "   cos   $cos"
    Espionne "   sin   $sin"
    Espionne "   verif $verif"
    Espionne "   sver  $sver"

    set ang [expr {atan2($sin,$cos)}]
    puts "   ang [AngleDeg $ang]"
    if {$ang < 0.} {
	set ang [expr {$ang + [2Pi]}]
    }

    return $ang
}


proc testAngleEntre {} {    
    set Lv1 [list 1.0  1.0 -1.0  1.0 -1.0 -1.0 1.0 -1.0]
    set Lv2 [list 0.0  1.0  1.0 -1.0 -1.0 -1.0 1.0  0.0]

    foreach {x1 y1} $Lv1 {
	set V1 [list [expr {$x1+0.01}] [expr {$y1+0.01}] 0.0]
	foreach {x2 y2} $Lv2 {
	    set V2 [list [expr {$x2+0.01}] [expr {$y2+0.01}] 0.0]
	    puts "\n"
	    puts "$V1 | $V2 | [AngleDeg [AngleEntre $V1 $V2]]"
	}
    }

    exit
}