Replaces all earlier versions
Code: Select all
' Library "matlib" version 30-12-2014
' vector and matrix calculations and some additional thingies.
' produced by "Henko"
' no copyrights and no guaranties
'
' IMPORTANT: all functions are based upon OPTION BASE 1
'
' vec_in (n,v(),x,y) input a vector
' vec_out (n$,n,v(),x,y,l,d) print a vector
' vec_unit (n,v()) make unit vector
' vec_rnd (n,v(),mini,maxi) make random vector
' vec_zero (n,v()) make null vector
' vec_copy (n,from(),into(() copy a vector
' vec_scal (n,v(),s) scale a vector
' vec_len (n,v()) length of a vector
' vec_norm (n,v()) normalize a vector to lenght 1
' vec_plus (n,v1(),v2()) add 2 vectors
' vec_min (n,v1(),v2()) subtract 2 vectors
' inprod (n,v1(),v2()) dot product of 2 vectors
' mat_in (n,m,mat(,),x,y) input a matrix
' mat_out (n$,n,m,mat(,),x,y,l,d) print a matrix
' mat_zero (n,m,mat(,)) make a null matrix
' mat_unit (n,mat(,)) make a unit matrix
' mat_rnd (n,m,mat(,),mini,maxi) make a random matrix
' mat_rot (mat(,),ang) make 2d rotation matrix
' mat_rotx (mat(,),ang) make 3d rotation matrix about x-axis
' mat_roty (mat(,),ang) make 3d rotation matrix about y-axis
' mat_rotz (mat(,),ang) make 3d rotation matrix about y-axis
' mat_scal (n,m,mat(,),s) scale a matrix
' mat_copy (n,m,from(,),into(,)) copy a matrix
' mat_trans (n,m,mat(,),matt(,)) transpose a matrix
' mat_plus (n,m,mata(,),matb(,)) add 2 matrices
' mat_min (n,m,mata(,),matb(,)) subtract 2 matrices
' mat_mul (n,m,mata(,),matb(,),matc(,)) multiply 2 matrices
' mat_inv (n,mata(,),ainv(,)) invert a matrix
' mat_vec (n,m,mat(,),vin(),vout()) multiply a vector by a matrix
' mat_det(n,mat(,)) determinant of a matrix
' det_sub(n,r,a(,)) determinant of a submatrix
' eigen_2 (mat(,),ev(,),lab) eigenvalues and -vectors of a 2d matrix
' eigen_n (n,mat(,),ev()) largest eigenvalue and -vector
' lin_eq2(a(,),x(),b()) solve 2 linear equations
' lin_eqn(nvar,a(,),x(),b()) solve system of linear equations
' gonio_eq1 (a,b,c) solve a*sin(x)+b*cos(x)=c
' ls (n,m,x(),y(),c()) polynomial least squares fit in 2d
' ls_lin3(n,x(),y(),z(),c()) least squares fit in 3d (plane)
' ls_lin6(n,x(),y(),z(),c()) least squares fit in 3d (quadratic)
' rot2(deg,vin(),vout()) rotate 2d vector
' rot_x(deg,vin(),vout()) rotate 3d vector about x-axis
' rot_y(deg,vin(),vout()) rotate 3d vector about y-axis
' rot_z(deg,vin(),vout()) rotate 3d vector about z-axis
' rot_xyz(dgx,dgy,dgz,vin(),vout()) rotate 3d vector about 3 axis
' interp3(pnt(),sp,x) cubic interpolation
' poly (n,coef(),x) value of polynomial for x-value
' surf_n(n,c(,)) surface of a n-polygon
' surf_3(a(),b()) surface of a triangle
' bearing(xs,ys,xe,ye) compass bearing from point s to e
' point_to_line(ndim,p(),s(),r()) distance from point to line in n-dim
' point_to_line2(x,y,f(),d()) distance from point to line in 2d
' intersect(a(),b(),p(),q()) check if 2 line-sections intersect
' factorial(x)
' mod (a,m)
' pi
' rad
' input a n-sized vector v() at screen position x,y
'
def vec_in (n,v(),x,y)
dim b$(20)
for i=1 to n
b$(i)="a" & i ! field b$(i) text b$(i) at x,y+30*(i-1) size 80,25
next i
som=0
loop:
for i=1 to n
if field_changed(b$(i)) then
v(i)=field_text$(b$(i)) ! field b$(i) delete
draw text n2a$(v(i),6,2) at x,y+30*(i-1)
som=som+1
end if
next i
if som<n then goto loop
end def
' print a vector at position x,y with number length l and precision d
'
def vec_out (n$,n,v(),x,y,l,d)
ys=y-30
if n$<>"" then
draw text n$ at x,y
ys=ys+30
end if
for i=1 to n
a$=n2a$(i,3,0) & " " & n2a$(v(i),l,d)
draw text a$ at x-12,ys+25*i
next i
end def
' produce a vector filled with one's
'
def vec_unit (n,v())
for i=1 to n ! v(i)=1 ! next i
end def
' produce a vector filled with random numbers between mini and maxi
'
def vec_rnd (n,v(),mini,maxi)
for i=1 to n ! v(i)=mini+(maxi-mini)*rnd(1) ! next i
end def
' produce a vector filled with zero's
'
def vec_zero (n,v())
for i=1 to n ! v(i)=0 ! next i
end def
' copy vector from() into vector into()
'
def vec_copy (n,from(),into())
for i=1 to n ! into(i)=from(i) ! next i
end def
' multiply a vector with scalar s
'
def vec_scal (n,v(),s)
for i=1 to n ! v(i)=s*v(i) ! next i
end def
' produce the length of a vector
'
def vec_len (n,v()) = sqrt(inprod(n,v,v))
' normalize a vector to unit length
'
def vec_norm (n,v())
vec_scal(n,v,1/vec_len(n,v))
end def
' add vector v2() to vector v1()
'
def vec_plus (n,v1(),v2())
for i=1 to n ! v1(i)=v1(i)+v2(i) ! next i
end def
' subtract vector v2() from vector v1()
'
def vec_min (n,v1(),v2())
for i=1 to n ! v1(i)=v1(i)-v2(i) ! next i
end def
' produce the scalar product of two vectors
'
def inprod (n,v1(),v2())
sum=0
for i=1 to n ! sum=sum+v1(i)*v2(i) ! next i
return sum
end def
' input a nxm matrix at position x,y (left upper corner)
' n is the number of rows, m is the number of columns
'
def mat_in (n,m,mat(,),x,y)
dim b$(m*n)
b$(1)=0
for i=1 to n
for j=1 to m
k=m*(i-1)+j ! b$(k)=" a" & i & j ! tt$=b$(k)
field tt$ text tt$ at x+100*(j-1),y+30*(i-1) size 80,25
next j
next i
som=0
loop1:
for i=1 to n
for j=1 to m
k=m*(i-1)+j
if field_changed(b$(k)) then
mat(i,j)=field_text$(b$(k))
field b$(k) delete
draw text n2a$(mat(i,j),6,2) at x+100*(j-1),y+30*(i-1)
som+=1
end if
next j
next i
if som<n*m then goto loop1
end def
' print a nxm matrix at position x,y left upper corner
' each element having total length l and d decimals
' n$ is an text, printed to identify the matrix (may be empty)
'
def mat_out (n$,n,m,mat(,),x,y,l,d)
ys=y-30
if n$<>"" then
draw text n$ at x,y
ys=ys+30
end if
for i=1 to n
for j=1 to m
a$=n2a$(mat(i,j),l,d) ! draw text a$ at x+12*l*(j-1),ys+25*i
next j
next i
end def
' produce a nxm matrix with zero elements
'
def mat_zero (n,m,mat(,))
for i=1 to n
for j=1 to m ! mat(i,j)=0 ! next j
next i
end def
' produce a unit matrix with one's in the diagonal and zeros elsewhere
'
def mat_unit (n,mat(,))
for i=1 to n
for j=1 to n ! if i=j then mat(i,j)=1 else mat(i,j)=0 ! next j
next i
end def
' produce a matrix with random elements between mini and maxi
'
def mat_rnd (n,m,mat(,),mini,maxi)
for i=1 to n
for j=1 to m ! mat(i,j)=mini+(maxi-mini)*rnd(1) ! next j
next i
end def
' produce a 2d rotation matrix
'
def mat_rot (mat(,),ang)
mat(1,1)=cos(ang) ! mat(1,2)=-sin(ang)
mat(2,1)=sin(ang) ! mat(2,2)=cos(ang)
end def
' produce a 3d rotation matrix about the x-axis
'
def mat_rotx (mat(,),ang)
mat_zero(3,3,mat)
mat(1,1)=1
mat(2,2)=cos(ang) ! mat(2,3)=-sin(ang)
mat(3,2)=sin(ang) ! mat(3,3)=cos(ang)
end def
' produce a 3d rotation matrix about the y-axis
'
def mat_roty (mat(,),ang)
mat_zero(3,3,mat)
mat(2,2)=1
mat(1,1)=cos(ang) ! mat(1,3)=-sin(ang)
mat(3,1)=sin(ang) ! mat(3,3)=cos(ang)
end def
' produce a 3d rotation matrix about the z-axis
'
def mat_rotz (mat(,),ang)
mat_zero(3,3,mat)
mat(3,3)=1
mat(1,1)=cos(ang) ! mat(1,2)=-sin(ang)
mat(2,1)=sin(ang) ! mat(2,2)=cos(ang)
end def
' multiply all elements of a matrix with scalair s
'
def mat_scal (n,m,mat(,),s)
for i=1 to n
for j=1 to m ! mat(i,j)=s*mat(i,j) ! next j
next i
end def
' copy matrix from() into matrix into()
'
def mat_copy (n,m,from(,),into(,))
for i=1 to n
for j=1 to m ! into(i,j)=from(i,j) ! next j
next i
end def
' produce the transpose of matrix mat() into matrix matt()
'
def mat_trans (n,m,mat(,),matt(,))
for i=1 to n
for j=1 to m ! matt(j,i)=mat(i,j) ! next j
next i
end def
' add matb() to mata()
'
def mat_plus (n,m,mata(,),matb(,))
for i=1 to n
for j=1 to m ! mata( i,j)=mata(i,j)+matb(i,j) ! next j
next i
end def
' subtract matb() from mata()
'
def mat_min (n,m,mata(,),matb(,))
for i=1 to n
for j=1 to m ! mata( i,j)=mata(i,j)-matb(i,j) ! next j
next i
end def
' produce product of mata() and matb() giving matc()
def mat_mul (n,m,mata(,),matb(,),matc(,))
for i=1 to n
for j=1 to n
tot=0
for k=1 to m ! tot=tot+mata(i,k)*matb(k,j) ! next k
matc(i,j)=tot
next j
next i
end def
' produce the inverse of square matrix a() giving matrix ainv()
'
def mat_inv (nvar,a(,),ainv(,))
dim w(nvar,2*nvar)
for i=1 to nvar
for j=1 to nvar ! w(i,j)=a(i,j) ! w(i,j+nvar)=0 ! next j
w(i,i+nvar)=1
next i
for piv=1 to nvar
fac=w(piv,piv)
for j=piv to piv+nvar ! w(piv,j)=w(piv,j)/fac ! next j
for i=1 to nvar
if i<>piv then
fac=w(i,piv)
for j=piv to piv+nvar ! w(i,j)=w(i,j)-fac*w(piv,j) ! next j
endif
next i
next piv
for i=1 to nvar
for j=1 to nvar ! ainv(i,j)=w(i,j+nvar) ! next j
next i
end def
' produce product of a matrix and a vector vin(), giving vout()
' the matrix has size nxm, the vin() has size m, vout() has size n
'
def mat_vec (n,m,mat(,),vin(),vout())
dim v(n)
for i=1 to n
tot=0
for j=1 to m ! tot=tot+mat( i,j)*vin(j) ! next j
v(i)=tot
next i
for i=1 to n ! vout(i)=v(i) ! next i
end def
' produce the determinant of a nxn matrix
' the input matrix mat(,) remains unchanged
' the value of the determinant is given back by the function
'
def mat_det(n,mat(,))
dim a(n,n)
mat_copy(n,n,mat,a)
for i=1 to n-1
if i=1 then det=a(i,i) else det=det*a(i,i)
for j=i+1 to n
fac=a(j,i)/a(i,i)
for k=i+1 to n ! a(j,k)=a(j,k)-fac*a(i,k) ! next k
next j
next i
return a(n,n)*det
end def
' produce the determinant of a sub-matrix with
' the 1'st row and r'th column deleted from it
' the proper sign for odd pivot element is accounted for
'
def det_sub(n,r,a(,))
dim mat(n-1,n-1)
for i=2 to n
for j=1 to n
if j=r then continue
if j<r then mat(i-1,j)=a(i,j) else mat(i-1,j-1)=a(i,j)
next j
next i
det=mat_det(n-1,mat) ! if odd(r) then det=-det
return det
end def
' eigenvalues and eigenvectors of a 2x2 matrix
' matrix has real coefficients
' matrix is passed as "mat"
' function returns 0 if no real eigenvalues are found,
' else the function returns 1
' 2 eigenvalues are returned in the vector "lab"
' 2 eigenvectors are returned as colums in the matrix "ev"
' eigenvectors are normalized to a length of 1
' library "matlib" is needed
'
def eigen_2 (mat(,),ev(,),lab())
dim x(2)
discr=(mat(1,1)-mat(2,2))^2+4*mat(1,2)*mat(2,1)
if discr<0 then return 0
discr=sqrt(discr) ! s=mat(1,1)+mat(2,2)
lab(1)=(s+discr)/2 ! lab(2)=(s-discr)/2
for j=1 to 2
x(1)=1
if mat(1,2) then
x(2)=(lab(j)-mat(1,1))/mat(1,2)
else
x(2)=(lab(j)-mat(2,1))/mat(2,2)
end if
vec_norm(2,x)
ev(1,j)=x(1) ! ev(2,j)=x(2)
next j
return 1
end def
' Find largest eigenvalue with eigenvector for nxn matrix,
' using the simplest "power method".
' No results in case of complex or multiple eigenvalues, the
' function will then return a value of 0.
' If the iteration converges, the function returns the eigenvalue
' and the accompanying eigenvector in the vector "ev"
'
def eigen_n (n,mat(,),ev())
dim evo(n)
count=0 ! maxcount=100 ! eps=.00001
labo=1 ! vec_rnd(n,evo,-1,1)
do
mat_vec(n,n,mat,evo,ev)
lab=vec_len(n,ev)/vec_len(n,evo)
dif=abs(abs(lab)-abs(labo)) ! vec_norm(n,ev)
if dif>eps then
labo=lab ! vec_copy(n,ev,evo) ! count=count+1
end if
until dif<eps or count=maxcount
if ev(1)*evo(1)<0 then lab=-lab
if count=maxcount then return 0 else return lab
end def
' solve 2 linear equations with two unknowns
' returns 0 if det=0 else returns 1
'
def lin_eq2(a(,),x(),b())
det=a(1,1)*a(2,2)-a(2,1)*a(1,2)
if det=0 then return 0
x(1)=(a(2,2)*b(1)-a(1,2)*b(2))/det
x(2)=(a(1,1)*b(2)-a(2,1)*b(1))/det
return 1
end def
' Solving a system of n linear equations with n variables
' nvar = number of equations (and number of variables)
' a(,) = nxn coefficient matrix
' b() = the right-hand side with known values
' x() = contains the calculated "unknowns"
'
def lin_eqn(nvar,a(,),x(),b())
for i=1 to nvar-1
for j=i+1 to nvar
fac=a(j,i)/a(i,i) ! b(j)=b(j)-fac*b(i)
for k=i+1 to nvar ! a(j,k)=a(j,k)-fac*a(i,k) ! next k
next j
next i
x(nvar)=b(nvar)/a(nvar,nvar)
for i=nvar-1 to 1 step -1 ! x(i)=b(i)
for j=i+1 to nvar ! x(i)=x(i)-a(i,j)*x(j) ! next j
x(i)=x(i)/a(i,i)
next i
return
end def
' solves the simple gonio equation: a*sin(x)+b*cos(x)=c
' returns the angle x for which the equation holds
' the answer is a complex number for values c>sqrt(a^2+b^2)
'
def gonio_eq1 (a,b,c)
return asin(c/sqrt(a*a+b*b))-atan(b/a)
end def
' this is a m-degree polynomial least squares fit of a number of
' point in 2 dimensional space
' there are n points, with x- and y-coordinates in vectors x() and y()
' m is the degree of the polynomial (take 1 for straight line fit,
' m=2 for parabola, and so on)
' the coefficients of the best fit polynomial are returned in vector c()
' f.i. for m=2 : y = c(1) + c(2)*x + c(3)*x^2
'
def ls (n,m,x(),y(),c())
m+=1
dim a1(n,m),a2(m,n),a3(m,m),rl(m)
for i=1 to n
a1(i,1)=1
for j=2 to m ! a1(i,j)=a1(i,j-1)*x(i) ! next j
next i
mat_trans (n,m,a1,a2)
mat_mul (m,n,a2,a1,a3)
mat_vec (m,n,a2,y,rl)
mat_inv (m,a3,a1)
mat_vec (m,m,a1,rl,c)
end def
' Least square aproximation of (x,y,z) points by a plane in R3
' n-number of points
' c-vector with 3 resulting coefficients: z=c(1)+c(2)*x+c(3)*y
'
def ls_lin3(n,x(),y(),z(),c())
dim xy(n,3),xyt(3,n),m(3,3),mi(3,3),b(3)
for i=1 to n
xy(i,1)=1 ! xy(i,2)=x(i) ! xy(i,3)=y(i)
next i
mat_trans(n,3,xy,xyt)
mat_mul(3,n,xyt,xy,m) ! mat_inv(3,m,mi)
mat_vec(3,n,xyt,z,b) ! mat_vec(3,3,mi,b,c)
end def
' Least square aproximation of (x,y,z) points by a quadratic
' surface in R3
' n-number of points
' c-vector with 6 resulting coefficients with the formula:
' z=c(1)+c(2)*x+c(3)*y+c(4)*x^2+c(5)*x*y+c(6)*y^2
'
def ls_lin6(n,x(),y(),z(),c())
dim xy(n,6),xyt(6,n),m(6,6),mi(6,6),b(6)
for i=1 to n
xy(i,1)=1 ! xy(i,2)=x(i) ! xy(i,3)=y(i)
xy(i,4)=x(i)^2 ! xy(i,5)=x(i)*y(i) ! xy(i,6)=y(i)^2
next i
mat_trans(n,6,xy,xyt)
mat_mul(6,n,xyt,xy,m) ! mat_inv(6,m,mi)
mat_vec(6,n,xyt,z,b) ! mat_vec(6,6,mi,b,c)
end def
' rotate a 2-dimensional vector vin() deg degrees, giving vector vout()
'
def rot2(deg,vin(),vout())
x=vin(1) ! vout(1)=x*cos(deg)-vin(2)*sin(deg)
vout(2)=x*sin(deg)+vin(2)*cos(deg)
end def
' rotate the 3-dim. vector vin() deg degrees about the x-axes -> vout()
'
def rot_x(deg,vin(),vout())
vout(1)=vin(1)
y=vin(2) ! vout(2)=y*cos(deg)-vin(3)*sin(deg)
vout(3)=y*sin(deg)+vin(3)*cos(deg)
end def
' rotate the 3-dim. vector vin() deg degrees about the y-axes -> vout()
'
def rot_y(deg,vin(),vout())
vout(2)=vin(2)
x=vin(1) ! vout(1)=x*cos(deg)-vin(3)*sin(deg)
vout(3)=x*sin(deg)+vin(3)*cos(deg)
end def
' rotate the 3-dim. vector vin() deg degrees about the z-axes -> vout()
'
def rot_z(deg,vin(),vout())
vout(3)=vin(3)
x=vin(1) ! vout(1)=x*cos(deg)-vin(2)*sin(deg)
vout(2)=x*sin(deg)+vin(2)*cos(deg)
end def
' rotate vector vin() about all 3 axes, giving vector vout()
def rot_xyz(dgx,dgy,dgz,vin(),vout())
dim temp(3)
rot_x(dgx,vin,temp)
rot_y(dgy,temp,temp)
rot_z(dgz,temp,vout)
end def
' cubic interpolation, using 4 points, p1 trough p4
' sp is the starting point (= p1)
' x is the distance from p1 in the interval p1 - p2
' x_scale is equidistant
def interp3(pnt(),sp,x)
p=(pnt(sp+2)-pnt(sp+1))-(pnt(sp-1)-pnt(sp))
q=(pnt(sp-1)-pnt(sp))-p
r=pnt(sp+1)-pnt(sp-1)
s=pnt(sp)
return p*x^3+q*x^2+r*x+s
end def
' calculate the value of a polynomial for a give value of x
' n is the degree of the polynomial, hence n+1 coefficients must
' be passed: a0, a1, a2, ..... an, in that order
'
def poly (n,coef(),x)
res=coef(n+1)
for i=n to 1 step-1 ! res=res*x+coef(i) ! next i
return res
end def
' calculates the surface of a n-polygon, n>=3
' (divides the polygon in triangles and then uses surf_3 function)
' c() has size nx2 and contains the coordinates of the vertices.
'
def surf_n(n,c(,))
dim cr(n,2),u(2),v(2)
if n<3 then return 0
x1=c(1,1) ! y1=c(1,2) ! sum=0
for i=1 to n ! cr(i,1)=c(i,1)-x1 ! cr(i,2)=c(i,2)-y1 ! next i
for i=2 to n-1
u(1)=cr(i,1) ! u(2)=cr(i,2) ! v(1)=cr(i+1,1) ! v(2)=cr(i+1,2)
sum=sum+surf_3(u,v)
next i
return sum
end def
' calculates the surface of a triangle, produced by 2 vectors
'
def surf_3(a(),b())=.5*abs(a(1)*b(2)-a(2)*b(1))
' bearing angle from xs,ys to xe,ye
' compass method for direction angles
' angle in degrees
'
def bearing(xs,ys,xe,ye)
x=xe-xs ! y=ye-ys
if y=0 then
if x>0 then k=90 else k=270
else
k=atan(x/y)
end if
if y<0 then ! k+=180 ! else ! if x<0 then k+=360 ! end if
return k
end def
' distance from a point to a line in n-dimensional space
' ndim = number of dimensions
' p() = vector of point
' s() = vector of some point on the line
' r() = direction vector of the line
'
def point_to_line(ndim,p(),s(),r())
dim ps(ndim),dis(ndim)
vec_copy(ndim,p,ps) ! vec_min(ndim,ps,s)
fac=inprod(ndim,ps,r)/inprod(ndim,r,r)
vec_copy(ndim,r,dis) ! vec_scal(ndim,dis,fac)
vec_min(ndim,ps,dis)
return vec_len(ndim,ps)
end def
' distance from a point (x,y) to a line in 2D
' the line is passed in vector form f+lambda*d
'
def point_to_line2(x,y,f(),d())
vec(1)=x-f(1) ! vec(2)=y-f(2)
lambda=inprod(2,d,vec)/inprod(2,d,d)
for i=1 to 2 ! vec(i)=lambda*d(i)-vec(i) ! next i
return vec_len(2,vec)
end def
' function that checks if two linesections intersect
' first linesection is between vectors a and b
' second linesection is between vectors p and q
' if intersect, returns the lambda value of first line
' returns -1 if no intersection
'
def intersect(a(),b(),p(),q())
dim m(2,2),lab(2),rhs(2)
m(1,1)=b(1)-a(1) ! m(1,2)=p(1)-q(1)
m(2,1)=b(2)-a(2) ! m(2,2)=p(2)-q(2)
rhs(1)=p(1)-a(1) ! rhs(2)=p(2)-a(2)
if lin_eq2(m,lab,rhs)=0 then return -1
if lab(1)<0 or lab(1)>1 or lab(2)<0 or lab(2)>1 then return -1
return lab(1)
end def
' factorial of x (by recursion)
'
def factorial(x)
if x then return x*factorial(x-1) else return 1
end def
' integer remainder of a/m
'
def mod(a,m)
d=a/m
mod=m*(d-floor(d))
end def
' value of pi
'
def pi=3.14159265
' value of 1 radian
'
def rad=180/pi
' formatting floats for graphics mode
'
def n2a$(num,lang,dec)
b$=" "
fac=10^dec
num$=floor(fac*num)/fac
tot=lang-len(num$)
if tot<1 then tot=1
a$=substr$(b$,1,tot) & num$
return a$
end def
' pre-padding strings with blancs to a given width
'
def pre_pad$(w,a$)
sp$=" "
tot=w-len(a$)
return substr$(sp$,1,tot) & a$
end def