# vector.tcl - vector math routines
# Jonas Beskow 1999-2000

namespace eval vec {
 variable PI
 set PI [expr {4*atan(1.0)}]
}

# -----------------------------------------------------------------------------
# operations on vectors of arbitrary length
# -----------------------------------------------------------------------------

# zero - generate a vector of zeroes
proc vec::zero {dim} {
 for {set i 0} {$i<$dim} {incr i} {lappend vr 0}
 set vr
}

# add - add two or more vectors
proc vec::add {v1 args} {
 set res $v1
 foreach vn $args {
  set res [add2 $res $vn]
 }
 set res
}

# add2 - add two vectors
proc vec::add2 {v1 v2} {
 if {[llength $v1]!=[llength $v2]} {
  error "Vectors must be of equal length"
 }
 foreach c1 $v1 c2 $v2 {lappend vr [expr {$c1+$c2}]}
 set vr
}

# sub - subtract one vector from another
proc vec::sub {v1 v2} {
 if {[llength $v1]!=[llength $v2]} {
  error "Vectors must be of equal length"
 }
 foreach c1 $v1 c2 $v2 {lappend vr [expr {$c1-$c2}]}
 set vr
}

# len - get scalar length of a vector
proc vec::len {v1} {
 set d 0
 foreach c1 $v1 {set d [expr {$d + $c1*$c1}]}
 expr {sqrt($d)}
}

# dist - get scalar distance between two vectors
proc vec::dist {v1 v2} {
 if {[llength $v1]!=[llength $v2]} {
  error "Vectors must be of equal length"
 }
 len [sub $v1 $v2]
}

# scale - multiply a vector with a scalar
proc vec::scale {v1 m} {
 foreach c1 $v1 {lappend vr [expr {$c1*$m}]}
 set vr
}

# norm - normalize a vector
proc vec::norm {v1} {
 set l [len $v1]
 if {$l==0.0} {set v1} else {scale $v1 [expr {1.0/$l}]}
}

# dot - calculate scalar dot product of two vectors
proc vec::dot {v1 v2} {
 if {[llength $v1]!=[llength $v2]} {
  error "Vectors must be of equal length"
 }
 set d 0.0
 foreach c1 $v1 c2 $v2 {set d [expr {$d+$c1*$c2}]}
 set d
}

# -----------------------------------------------------------------------------
# operations on 3d-vectors
# -----------------------------------------------------------------------------

# cross - cross product between two 3d-vectors
proc vec::cross {v1 v2} {
 if {[llength $v1]!=3 || [llength $v2]!=3} {
  error "Vectors must be of length 3"
 }
 set v1x [lindex $v1 0]
 set v1y [lindex $v1 1]
 set v1z [lindex $v1 2]
 set v2x [lindex $v2 0]
 set v2y [lindex $v2 1]
 set v2z [lindex $v2 2]
 list \
   [expr {$v1y*$v2z-$v1z*$v2y}] \
   [expr {$v1z*$v2x-$v1x*$v2z}] \
   [expr {$v1x*$v2y-$v1y*$v2x}]
}

# rotx - rotate a 3d-vector around the x-axis
proc vec::rotx {v1 angle} {
 if {[llength $v1]!=3} {
  error "Vector must be of length 3"
 }
 set sa [expr {sin($angle)}]
 set ca [expr {cos($angle)}]
 set v1x [lindex $v1 0]
 set v1y [lindex $v1 1]
 set v1z [lindex $v1 2]
 
 list $v1x [expr {$v1y*$ca-$v1z*$sa}] [expr {$v1y*$sa+$v1z*$ca}]
}

# roty - rotate a 3d-vector around the y-axis
proc vec::roty {v1 angle} {
 if {[llength $v1]!=3} {
  error "Vector must be of length 3"
 }
 set sa [expr {sin($angle)}]
 set ca [expr {cos($angle)}]
 set v1x [lindex $v1 0]
 set v1y [lindex $v1 1]
 set v1z [lindex $v1 2]
 
 list [expr {$v1z*$sa+$v1x*$ca}] $v1y [expr {$v1z*$ca-$v1x*$sa}]
}

# rotz - rotate a 3d-vector around the z-axis
proc vec::rotz {v1 angle} {
 if {[llength $v1]!=3} {
  error "Vector must be of length 3"
 }
 set sa [expr {sin($angle)}]
 set ca [expr {cos($angle)}]
 set v1x [lindex $v1 0]
 set v1y [lindex $v1 1]
 set v1z [lindex $v1 2]
 
 list [expr {$v1x*$ca-$v1y*$sa}] [expr {$v1x*$sa+$v1y*$ca}] $v1z
}

# irotx - inverse rotation around the x-axis
#
# returns the angle of v2's projection relative to v1's projection
# on the y-z-plane. Alternative interpretation:
# returns the angle that v1 should be rotated to align with
# v2 in the y-z-plane

proc vec::irotx {v1 v2} {
 variable PI
 foreach {v1x v1y v1z} $v1 {v2x v2y v2z} $v2 break
 set a [expr {$PI + fmod(atan2($v2y,-$v2z) - atan2($v1y,-$v1z)-$PI,2*$PI)}]
}

# iroty: as irotx but in x-z-plane

proc vec::iroty {v1 v2} {
 variable PI
 foreach {v1x v1y v1z} $v1 {v2x v2y v2z} $v2 break
 set a [expr {$PI + fmod(atan2($v2x,$v2z)-atan2($v1x,$v1z) - $PI, 2*$PI)}]
}

# irotz: as irotx but in x-y-plane

proc vec::irotz {v1 v2} {
 variable PI
 foreach {v1x v1y v1z} $v1 {v2x v2y v2z} $v2 break
 set a [expr {$PI + fmod(atan2($v2y,$v2x)-atan2($v1y,$v1x) - $PI, 2*$PI)}]
}