Support Forum / Форум поддержки

Code: Select all

' Library "matlib" version 16-06-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)
' vec_out (n$,n,v(),x,y,l,d)
' vec_zero (n,v())
' vec_unit (n,v())
' vec_rnd (n,v(),mini,maxi)
' vec_copy (n,from(),into(()
' vec_scal (n,v(),s)
' vec_len (n,v())
' vec_norm (n,v())
' vec_plus (n,v1(),v2())
' vec_min (n,v1(),v2())
' inprod (n,v1(),v2())
' mat_in (n,m,mat(,),x,y)
' mat_out (n$,n,m,mat(,),x,y,l,d)
' mat_zero (n,m,mat(,))
' mat_unit (n,mat(,))
' mat_det(n,mat(,))
' det_sub(n,r,a(,))
' mat_rnd (n,m,mat(,),mini,maxi)
' mat_scal (n,m,mat(,),s)
' mat_copy (n,m,from(,),into(,))
' mat_trans (n,m,mat(,),matt(,))
' mat_plus (n,m,mata(,),matb(,))
' mat_min (n,m,mata(,),matb(,))
' mat_mul (n,m,mata(,),matb(,),matc(,))
' mat_inv (n,mata(,),ainv(,))
' mat_vec (n,m,mat(,),vin(),vout())
' eigen_2 (mat(,),ev(,),lab)
' eigen_n (n,mat(,),ev())
' lin_eq2(a(,),x(),b())
' lin_eqn(nvar,a(,),x(),b())
' ls (n,m,x(),y(),c())
' rot2(deg,vin(),vout())
' rot_x(deg,vin(),vout())
' rot_y(deg,vin(),vout())
' rot_z(deg,vin(),vout())
' rot_xyz(dgx,dgy,dgz,vin(),vout())
' poly (n,coef(),x)
' surf_n(n,c(,))
' surf_3(a(),b())
' bearing(xs,ys,xe,ye)
' point_to_line(ndim,p(),s(),r())
' point_to_line2(x,y,f(),d())
' interp3(pnt(),sp,x)
' mod (a,m)
' def pi
' def 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 zero's
'
def vec_zero (n,v())
for i=1 to n ! v(i)=0 ! 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

' 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

' 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 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

' 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

' 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

' 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 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

' 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

' 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

' 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, p0 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

' 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