//Scilab functions converted from Matlab written by Nestor Cardozo for the book Structural 
//Geology Algorithms by Allmendinger, Cardozo, & Fisher, 2011. If you use
//this script, please cite this as "Cardozo in Allmendinger et al. (2011)"

function b = ZeroTwoPi(a)
// ZeroTwoPi constrains azimuth to lie between 0 and 2*pi radians 
//   b = ZeroTwoPi(a) returns azimuth b (from 0 to 2*pi)
//   for input azimuth a (which may not be between 0 to 2*pi)
//   NOTE: Azimuths a and b are input/output in radians 
b=a;
twopi = 2.0*%pi;
if b < 0.0
    b = b + twopi;
elseif b >= twopi
    b = b - twopi;
end
endfunction

function [trd1,plg1] = Pole(trd,plg,k)
// Pole returns the pole to a plane or the plane which correspond to a pole 
//   k is an integer that tells the program what to calculate. 
//
//   If k = 0, [trd1,plg1] = Pole(trd,plg,k) returns the strike 
//   (trd1) and dip (plg1) of a plane, given the trend (trd) 
//   and plunge (plg) of its pole.
//
//   If k = 1, [trd1,plg1] = Pole(trd,plg,k) returns the trend
//   (trd1) and plunge (plg1) of a pole, given the strike (trd)
//   and dip (plg) of its plane.
//
//   NOTE: Input/Output angles are in radians. Input/Output strike 
//   and dip are in right hand rule
//
//   Pole uses functions ZeroTwoPi, SphToCart and CartToSph
//Some constants
east = %pi/2.0;

if k == 0
    if plg >= 0.0
        plg1 = east - plg;
        dipaz = trd - %pi;
    else
        plg1 = east + plg;
        dipaz = trd;
    end
    //Calculate trd1 and make sure it is between 0 and 2*pi
    trd1 = ZeroTwoPi(dipaz - east);
//Else calculate pole given its plane
elseif k == 1
    [c_n,c_e,c_d] = SphToCart(trd,plg,k);
    [trd1,plg1] = CartToSph(c_n,c_e,c_d);
end
endfunction

function [c_n,c_e,c_d] = SphToCart(trd,plg,k)
//SphToCart converts from spherical to cartesian coordinates 
//
//   [cn,ce,cd] = SphToCart(trd,plg,k) returns the north (cn), 
//   east (ce), and down (cd) direction cosines of a line.
//
//   k is an integer to tell whether the trend and plunge of a line 
//   (k = 0) or strike and dip of a plane in right hand rule 
//   (k = 1) are being sent in the trd and plg slots. In this 
//   last case, the direction cosines of the pole to the plane 
//   are returned
//
//   NOTE: Angles should be entered in radians 

//If line
if k == 0
    c_d = sin(plg);
    c_e = cos(plg) * sin(trd);
    c_n = cos(plg) * cos(trd); 
//Else pole to plane
else k == 1
    c_d = cos(plg);
    c_e = -sin(plg) * cos(trd);
    c_n = sin(plg) * sin(trd);
end
endfunction

function [trd,plg] = CartToSph(c_n,c_e,c_d)
//CartToSph converts from cartesian to spherical coordinates 
//
//   [trd,plg] = CartToSph(cn,ce,cd) returns the trend (trd)
//   and plunge (plg) of a line for input north (cn), east (ce), 
//   and down (cd) direction cosines
//
//   NOTE: Trend and plunge are returned in radians
//
//   CartToSph uses function ZeroTwoPi


//Plunge (see Table 2.1)
plg = asin(c_d);

//Trend
//If north direction cosine is zero, trend is east or west
//Choose which one by the sign of the east direction cosine
if c_n == 0.0 
    if c_e < 0.0 
        trd = 3.0/2.0*%pi; // trend is west
    else
        trd = %pi/2.0; // trend is east
    end
//Else use Table 2.1
else
    trd = atan(c_e/c_n); 
    if c_n < 0.0
        //Add pi 
        trd = trd+%pi;
    end
    //Make sure trd is between 0 and 2*pi
    trd = ZeroTwoPi(trd);
end
endfunction

function [xp,yp] = StCoordLine(trd,plg,sttype)
//StCoordLine computes the coordinates of a line 
//in an equal angle or equal area stereonet of unit radius
//
//   USE: [xp,yp] = StCoordLine(trd,plg,sttype)
//
//   trd = trend of line
//   plg = plunge of line
//   sttype = An integer indicating the type of stereonet. 0 for equal angle
//            and 1 for equal area
//   xp and yp = Coordinates of the line in the stereonet plot
//
//   NOTE: trend and plunge should be entered in radians
//
//   StCoordLine uses function ZeroTwoPi


// Take care of negative plunges
if plg < 0.0
    trd = ZeroTwoPi(trd+%pi);
    plg = -plg;
end

// Some constants
piS4 = %pi/4.0;
s2 = sqrt(2.0);
plgS2 = plg/2.0;

// Equal angle stereonet
if sttype == 0
    xp = tan(piS4 - plgS2)*sin(trd);
    yp = tan(piS4 - plgS2)*cos(trd);
// Equal area stereonet
elseif sttype == 1
    xp = s2*sin(piS4 - plgS2)*sin(trd);
    yp = s2*sin(piS4 - plgS2)*cos(trd);
end
endfunction

function [path1,path2,np1,np2] = SmallCircle(trda,plga,coneAngle,sttype)
//SmallCircle computes the paths of a small circle defined by its axis and
//cone angle, for an equal angle or equal area stereonet of unit radius
//
//   USE: [path1,path2,np1,np2] = SmallCircle(trda,plga,coneAngle,sttype)
//
//   trda = trend of axis
//   plga = plunge of axis
//   coneAngle = cone angle
//   sttype = type of stereonet. 0 for equal angle and 1 for equal area
//   path1 and path2 = vectors with the x and y coordinates of the points
//                     in the small circle paths
//   np1 and np2 = Number of points in path1 and path2, respectively
//
//   NOTE: All angles should be in radians
//
//   SmallCircle uses functions ZeroTwoPi, StCoordLine and Rotate
//

//Find where to start the small circle
if (plga - coneAngle) >= 0.0
    trd = trda;
    plg = plga - coneAngle;
else
    if plga == %pi/2.0
        plga = plga * 0.9999;
    end
    angle = acos(cos(coneAngle)/cos(plga));
    trd = ZeroTwoPi(trda+angle);
    plg = 0.0;
end

//To make the small circle, rotate the starting line 360 degrees in
//increments of 1 degree
rot=(0:1:360)*%pi/180;
path1 = zeros(size(rot,2),2);
path2 = zeros(size(rot,2),2);
np1 = 0; np2 = 0;
for i=1:size(rot,2)
    //Rotate line: Notice that here the line is considered as a vector
    [rtrd,rplg] = Rotate(trda,plga,rot(i),trd,plg,'v');
    // Add to the right path
    // If plunge of rotated line is positive add to first path
    if rplg >= 0.0
        np1 = np1 + 1;
        //Calculate stereonet coordinates and add to path
        [path1(np1,1),path1(np1,2)] = StCoordLine(rtrd,rplg,sttype);
    //If plunge of rotated line is negative add to second path
    else
        np2 = np2 + 1;
        //Calculate stereonet coordinates and add to path
        [path2(np2,1),path2(np2,2)] = StCoordLine(rtrd,rplg,sttype);
    end
end
endfunction

function path = GreatCircle(strike,dip,sttype)
//GreatCircle computes the great circle path of a plane in an equal angle
//or equal area stereonet of unit radius
//
//   USE: path = GreatCircle(strike,dip,sttype)
//
//   strike = strike of plane
//   dip = dip of plane
//   sttype = type of stereonet. 0 for equal angle and 1 for equal area
//   path = vector with x and y coordinates of points in great circle path
//
//   NOTE: strike and dip should be entered in radians. 
//
//   GreatCircle uses functions StCoordLine, Pole and Rotate
//

//Compute the pole to the plane. This will be the axis of rotation to make
//the great circle
[trda,plga] = Pole(strike,dip,1);

//Now pick a line at the intersection of the great circle with the primitive
//of the stereonet
trd = strike;
plg = 0.0;

//To make the great circle, rotate the line 180 degrees in increments
//of 1 degree
rot=(0:1:180)*%pi/180;
path = zeros(size(rot,2),2);
for i=1:size(rot,2)
    //Avoid joining ends of path
    if rot(i) == %pi
        rot(i) = rot(i)*0.9999;
    end
    //Rotate line
    [rtrd,rplg] = Rotate(trda,plga,rot(i),trd,plg,'a');
    //Calculate stereonet coordinates of rotated line and add to great
    //circle path
    [path(i,1),path(i,2)] = StCoordLine(rtrd,rplg,sttype);
end
endfunction

function [rtrd,rplg] = GeogrToView(trd,plg,trdv,plgv)
//GeogrToView transforms a line from NED to View Direction
//coordinates
//
//   USE: [rtrd,rplg] = GeogrToView(trd,plg,trdv,plgv)
//
//   trd = trend of line
//   plg = plunge of line 
//   trdv = trend of view direction 
//   plgv = plunge of view direction 
//   rtrd and rplg are the new trend and plunge of the line in the view
//   direction.
//
//   NOTE: Input/Output angles are in radians
//
//   GeogrToView uses functions ZeroTwoPi, SphToCart and CartToSph

//Some constants
east = %pi/2.0;

// Make transformation matrix between NED and View Direction
a = zeros(3,3);
[a(3,1),a(3,2),a(3,3)] = SphToCart(trdv,plgv,0);
temp1 = trdv + east;
temp2 = 0.0;
[a(2,1),a(2,2),a(2,3)] = SphToCart(temp1,temp2,0);
temp1 = trdv;
temp2 = plgv - east;
[a(1,1),a(1,2),a(1,3)] = SphToCart(temp1,temp2,0);

// Direction cosines of line
dirCos = zeros(1,3);
[dirCos(1),dirCos(2),dirCos(3)] = SphToCart(trd,plg,0);

// Transform line
nDirCos = zeros(1,3);
for i=1:3
    nDirCos(i) = a(i,1)*dirCos(1) + a(i,2)*dirCos(2)+ a(i,3)*dirCos(3);
end

// Compute line from new direction cosines
[rtrd,rplg] = CartToSph(nDirCos(1),nDirCos(2),nDirCos(3));

// Take care of negative plunges
if rplg < 0.0
    rtrd = ZeroTwoPi(rtrd+%pi);
    rplg = -rplg;
end
endfunction

function [] = Stereonet(trdv,plgv,intrad,sttype)
//Stereonet plots an equal angle or equal area stereonet of unit radius
//in any view direction
//
//   USE: Stereonet(trdv,plgv,intrad,stttype)
//
//   trdv = trend of view direction
//   plgv = plunge of view direction
//   intrad = interval in radians between great or small circles 
//   sttype = An integer indicating the type of stereonet. 0 for equal angle,
//            and 1 for equal area
//
//   NOTE: All angles should be entered in radians
//
//   Example: To plot an equal area stereonet at 10 deg intervals in a
//   default view direction type:
//
//   Stereonet(0,90*pi/180,10*pi/180,1);
//
//   To plot the same stereonet but with a view direction of say: 235/42,
//   type:
//
//   Stereonet(235*pi/180,42*pi/180,10*pi/180,1);
//
//   Stereonet uses functions Pole, GeogrToView, SmallCircle and GreatCircle

// Some constants
east = %pi/2.0;
west = 3.0*east;

// Plot stereonet reference circle
///r = 1.0; // radius of stereonet
///TH = (0:1:360)*%pi/180; // polar angle, range 2 pi, 1 degree increment
///[X,Y] = pol2cart(TH,r); // cartesian coordinates of reference circle
///plot(X,Y,'k'); // plot reference circle
//axis ([-1 1 -1 1]); // size of stereonet
//axis equal; axis off; // equal axes, no axes
//hold on; // hold plot

// Number of small circles
nCircles = %pi/(intrad*2.0);

// Small circles
// Start at the North
trd = 0.0;
plg = 0.0;
// If view direction is not the default (trd=0,plg=90), transform line to
// view direction
//if trdv ~= 0.0 & plgv ~= east
if trdv ~= 0.0 || plgv ~= east
    [trd,plg] = GeogrToView(trd,plg,trdv,plgv);
end
// Plot small circles
for i=1:nCircles
    coneAngle = i*intrad;
    [path1,path2,np1,np2] = SmallCircle(trd,plg,coneAngle,sttype);
    plot(path1(1:np1,1),path1(1:np1,2),'b');
    if np2 > 0
        plot(path2(1:np2,1),path2(1:np2,2),'b');
    end
end

// Great circles
for i=0:nCircles*2
    //Western half
    if i <= nCircles
        // Pole of great circle
        trd = west;
        plg = i*intrad;
    //Eastern half
    else
        // Pole of great circle
        trd = east;
        plg = (i-nCircles)*intrad;
    end
    // If pole is vertical, shift it a little bit
    if plg == east
        plg = plg * 0.9999;
    end
    // If view direction is not the default (trd=0,plg=90), transform line to
    // view direction
    if trdv ~= 0.0 || plgv ~= east
        [trd,plg] = GeogrToView(trd,plg,trdv,plgv);
    end
    // Compute plane from pole
    [strike,dip] = Pole(trd,plg,0);
    // Plot great circle
    path = GreatCircle(strike,dip,sttype);
    plot(path(:,1),path(:,2),'b');
end
r = 1.0; // radius of stereonet
TH = (0:1:360)*%pi/180; // polar angle, range 2 pi, 1 degree increment
[X,Y] = pol2cart(TH,r); // cartesian coordinates of reference circle
plot(X,Y,'k'); // plot reference circle
endfunction

function [rtrd,rplg] = Rotate(raz,rdip,rot,trd,plg,ans0)
//Rotate rotates a line by performing a coordinate transformation on
//vectors. The algorithm was originally written by Randall A. Marrett
//
//   USE: [rtrd,rplg] = Rotate(raz,rdip,rot,trd,plg,ans0)
//
//   raz = trend of rotation axis
//   rdip = plunge of rotation axis
//   rot = magnitude of rotation
//   trd = trend of the vector to be rotated
//   plg = plunge of the vector to be rotated
//   ans0 = A character indicating whether the line to be rotated is an axis
//   (ans0 = 'a') or a vector (ans0 = 'v')
//
//   NOTE: All angles are in radians
//
//   Rotate uses functions SphToCart and CartToSph
//

//Allocate some arrays
a = zeros(3,3); //Transformation matrix
pole = zeros(1,3); //Direction cosines of rotation axis
plotr = zeros(1,3); //Direction cosines of rotated vector
temp = zeros(1,3);  //Direction cosines of unrotated vector

//Convert rotation axis to direction cosines. Note that the convention here
//is X1 = North, X2 = East, X3 = Down
[pole(1) pole(2) pole(3)] = SphToCart(raz,rdip,0);

// Calculate the transformation matrix
x = 1.0 - cos(rot);
sinRot = sin(rot);  //Just reduces the number of calculations
cosRot = cos(rot);
a(1,1) = cosRot + pole(1)*pole(1)*x;
a(1,2) = -pole(3)*sinRot + pole(1)*pole(2)*x;
a(1,3) = pole(2)*sinRot + pole(1)*pole(3)*x;
a(2,1) = pole(3)*sinRot + pole(2)*pole(1)*x;
a(2,2) = cosRot + pole(2)*pole(2)*x;
a(2,3) = -pole(1)*sinRot + pole(2)*pole(3)*x;
a(3,1) = -pole(2)*sinRot + pole(3)*pole(1)*x;
a(3,2) = pole(1)*sinRot + pole(3)*pole(2)*x;
a(3,3) = cosRot + pole(3)*pole(3)*x;

//Convert trend and plunge of vector to be rotated into direction cosines
[temp(1) temp(2) temp(3)] = SphToCart(trd,plg,0);

//The following nested loops perform the coordinate transformation
for i=1:3
    plotr(i) = 0.0;
    for j=1:3
        plotr(i) = a(i,j)*temp(j) + plotr(i);
    end
end

//Convert to lower hemisphere projection if data are axes (ans0 = 'a')
if plotr(3) < 0.0 && ans0 == 'a'
    plotr(1) = -plotr(1);
    plotr(2) = -plotr(2);
    plotr(3) = -plotr(3);
end

//Convert from direction cosines back to trend and plunge
[rtrd,rplg]=CartToSph(plotr(1),plotr(2),plotr(3));

endfunction

function [x,y] = pol2cart(theta, radius)
    x = radius .* cos (theta);
    y = radius .* sin (theta);
endfunction

function [ans1,ans2] = Angles(trd1,plg1,trd2,plg2,ans0)
//Angles calculates the angles between two lines, between two planes,
//the line which is the intersection of two planes, or the plane 
//containing two apparent dips
//
//   [ans1,ans2] = Angles(trd1,plg1,trd2,plg2,ans0) operates on
//   two lines or planes with trend/plunge or strike/dip equal to 
//   trd1/plg1 and trd2/plg2
//
//   ans0 is a character that tells the function what to calculate:
//
//       ans0 = 'a' -> the orientation of a plane given two apparent dips
//       ans0 = 'l' -> the angle between two lines
//
//       In the above two cases, the user sends the trend and plunge of two
//       lines
//
//       ans0 = 'i' -> the intersection of two planes
//       ans0 = 'p' -> the angle between two planes
//
//       In the above two cases the user sends the strike and dip of two
//       planes following the right hand rule
//
//   NOTE: Input/Output angles are in radians
//
//   Angles uses functions SphToCart, CartToSph and Pole
//

//If planes have been entered
if ans0 == 'i' || ans0 == 'p'
    k = 1;
//Else if lines have been entered
elseif ans0 == 'a' || ans0 == 'l'
    k = 0;
end

//Calculate the direction cosines of the lines or poles to planes
[c_n1,c_e1,c_d1]=SphToCart(trd1,plg1,k);
[c_n2,c_e2,c_d2]=SphToCart(trd2,plg2,k);

//If angle between 2 lines or between the poles to 2 planes
if ans0 == 'l' || ans0 == 'p'
    // Use dot product = Sum of the products of the direction cosines
    ans1 = acos(c_n1*c_n2 + c_e1*c_e2 + c_d1*c_d2);
    ans2 = %pi - ans1;
end

//If intersection of two planes or pole to a plane containing two
//apparent dips
if ans0 == 'a' || ans0 == 'i'
    //If the 2 planes or apparent dips are parallel return an error
    if trd1 == trd2 && plg1 == plg2
        error('lines or planes are parallel');
    //Else use cross product
    else
        c_n = c_e1*c_d2 - c_d1*c_e2;
        c_e = c_d1*c_n2 - c_n1*c_d2;
        c_d = c_n1*c_e2 - c_e1*c_n2;
        //Make sure the vector points down into the lower hemisphere
        if c_d < 0.0
            c_n = -c_n;
            c_e = -c_e;
            c_d = -c_d;
        end
        //Convert vector to unit vector by dividing it by its length
        r = sqrt(c_n*c_n+c_e*c_e+c_d*c_d);
        // Calculate line of intersection or pole to plane
        [trd,plg]=CartToSph(c_n/r,c_e/r,c_d/r);
        //If intersection of two planes
        if ans0 == 'i'
            ans1 = trd;
            ans2 = plg;
        //Else if plane containing two dips, calculate plane from its pole
        elseif ans0 == 'a'
            [ans1,ans2]= Pole(trd,plg,0);
        end 
    end
end

end
