# 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)}] }