classdef Static2DElasticity < handle
    %UNTITLED2 Summary of this class goes here
    %   Detailed explanation goes here
    
    properties
        nodes
        Area
        P
        knod
        neq
        nele
        nnod
        dim
        E
        nu
        lambda
        mu
        t
        fload
        S
        F
        mesh
        u
        presc
    end
    
    methods
        function O = Static2DElasticity(mesh, E, nu, lambda, mu, t, fload)
            %Static2DElasticity Construct an instance of this class
            %   obj = Static2DElasticity(mesh, E, nu, lambda, mu, t, fload)
            %
            %   Example:
            %   E = 210000; %MPa
            %   nu = 0.33;
            %   t = 3;%mm
            %   fload = @(xi,eta)[0;0]; %Body load
            %   %Plane Stress
            %   lambda = E*nu/(1-nu^2);
            %   mu = E/(2*(1+nu));
            %   
            %   statElast = Static2DElasticity(mesh, E, nu, lambda, mu, t, fload)
            %
            O.mesh = mesh;
            O.P = mesh.Nodes';
            O.nodes = mesh.Elements';
            O.E = E;
            O.nu = nu;
            O.lambda = lambda;
            O.mu = mu;
            O.t = t;
            O.fload = fload;
            
            P = O.P;
            nodes = O.nodes;
            
            [S,F, Area] = LinElast2D(nodes, P, t, fload, lambda, mu);
            O.S = S;
            O.F = F;
            O.Area = Area;
            
            [O.nele, O.knod] = size(nodes);
            [O.nnod, O.dim] = size(P);
            O.neq = O.nnod*O.dim;
            
        end
        
        function u = Solve(O)
            %METHOD1 Summary of this method goes here
            %   Detailed explanation goes here
            
            O.F = O.F-O.S(:,O.presc)*O.u(O.presc);
            free = setdiff(1:O.neq, O.presc);
            O.u(free) = O.S(free,free)\O.F (free);
            u = O.u;
        end
        
        function F1 = ComputeSurfaceLoad(O, varargin)
            p = inputParser;
            addParameter(p,'Region',[]);
            addParameter(p,'Magnitude',0);
            addParameter(p,'Direction',[]);
            addParameter(p,'Visualize',0);
            parse(p,varargin{:});
            region = p.Results.Region;
            magnitude = p.Results.Magnitude;
            direction = p.Results.Direction;
            viz = p.Results.Visualize;
            
            F1 = computeSurfaceLoad(O.mesh,region,magnitude,direction(:),O.t);
            
            O.F = O.F + F1;
            
            if viz
                Fx = F1(1:2:end);
                Fy = F1(2:2:end);
                
                quiver(O.P(:,1), O.P(:,2), sign(Fx).*(abs(Fx)>0), sign(Fy).*(abs(Fy)>0), 1, 'Displayname',['F=',num2str(magnitude)]);
                legend('show')
            end
        end
        
        function PrescribeDisplacement(O, varargin)
            if isempty(O.u)
               O.u = zeros(O.neq,1); 
            end
            
            p = inputParser;
            addParameter(p,'Region',[]);
            addParameter(p,'Ux',[]);
            addParameter(p,'Uy',[]);
            addParameter(p,'Visualize',0);
            parse(p,varargin{:});
            
            ux = p.Results.Ux;
            uy = p.Results.Uy;
            region = p.Results.Region;
            viz = p.Results.Visualize;
            
            nodeInds = O.mesh.findNodes('region','Edge',region);
            
            presc_x = [];
            if ~isempty(ux)
                presc_x = nodeInds*2-1;
                O.u(presc_x) = ux;
            end
            
            presc_y = [];
            if ~isempty(uy)
                presc_y = nodeInds*2;
                O.u(presc_y) = uy;
            end
            
            O.presc = [O.presc, [presc_x, presc_y]];
            
            if viz
                if ~isempty(ux)
                    hp = plot(O.P(nodeInds,1),O.P(nodeInds,2),'s','markersize',10,'Displayname',['u_x=',num2str(ux)]);
                    set(hp,'markerfaceColor',hp.Color)
                end
                if ~isempty(uy)
                    hp = plot(O.P(nodeInds,1),O.P(nodeInds,2),'d','Displayname',['u_y=',num2str(uy)]);
                    set(hp,'markerfaceColor',hp.Color)
                end
                legend('show')
            end
        end
        
        function VisualizeMesh(O, varargin)
            
            figure('Color','w'); axis equal
            patch('Faces',O.nodes(:,1:3),'Vertices', O.P,'FaceColor','none','EdgeAlpha',0.2,'HandleVisibility','off')
            hold on
        end
        
    end
end

function F = computeSurfaceLoad(mesh,region,F_magnitude,F_dir,t)
    nodeInds = mesh.findNodes('region','Edge',region);
    nodes = mesh.Elements';
    P = mesh.Nodes';
    eleInds = find(sum(ismember(nodes,nodeInds),2)>1);

    knod = size(nodes,2);
    nnod = size(P,1);
    neq = nnod*2;
    F = zeros(neq,1);
    if knod == 6
        ieqs = zeros(3*2,1);
        nele = length(eleInds);
        order = 2;
        [gp,gw] = gauss(order,0,1);
        phi = @(xi)[2*(xi-1).*(xi-1/2), -4*xi.*(xi-1), 2*xi.*(xi-1/2)];
        dphi = @(xi)[4*xi-3, 4-8*xi, 4*xi-1];

        L = 0;
        for iel = 1:nele
            indTri = eleInds(iel);
            iTri = nodes(indTri,:);
            edges = iTri([1 4 2; 2 5 3; 1 6 3]);
            iEdgeNodes = edges(all(ismember(edges, nodeInds),2),:);

            %         plot(P(iEdgeNodes,1),P(iEdgeNodes,2),'c*')

            ieqs(1:2:end) = 2*iEdgeNodes'-1; %x-dofs
            ieqs(2:2:end) = 2*iEdgeNodes'-0; %y-dofs

            xc = P(iEdgeNodes,:);

            %         h = norm(xc(end,:)-xc(1,:)); %linear length, edge is never curved
            % This can handle curved edges
            h = 0;
            for ig = 1:length(gp)
                xi = gp(ig); iw = gw(ig);
                h = h + norm(dphi(xi)*xc)*iw;
            end
            L = L + h;

            fe = zeros(2*3,1);
            for ig = 1:length(gp)
                xi = gp(ig); iw = gw(ig);
                PHI = zeros(2,2*3);
                PHI(1,1:2:end) = phi(xi);
                PHI(2,2:2:end) = phi(xi);
                fe = fe + PHI'*F_dir*F_magnitude*t*h*iw;
            end
            F(ieqs) = F(ieqs) + fe;
        end
    elseif knod == 3
        ieqs = zeros(2*2,1);
        nele = length(eleInds);
        order = 1;
        [gp,gw] = gauss(order,0,1);
        phi = @(xi)[1-xi, xi];
        dphi = @(xi)[-1, 1];

        L = 0;
        for iel = 1:nele
            indTri = eleInds(iel);
            iTri = nodes(indTri,:);
            iEdgeNodes = iTri(ismember(iTri, nodeInds));

            %         try delete(hp); end
            %         hp = plot(P(iEdgeNodes,1),P(iEdgeNodes,2),'c*')

            ieqs(1:2:end) = 2*iEdgeNodes'-1; %x-dofs
            ieqs(2:2:end) = 2*iEdgeNodes'-0; %y-dofs
            xc = P(iEdgeNodes,:);
            h = norm(xc(end,:)-xc(1,:));
            L = L + h;
            fe = zeros(2*2,1);
            for ig = 1:length(gp)
                xi = gp(ig); iw = gw(ig);
                PHI = zeros(2,2*2);
                PHI(1,1:2:end) = phi(xi);
                PHI(2,2:2:end) = phi(xi);
                fe = fe + PHI'*F_dir*F_magnitude*t*h*iw;
            end
            F(ieqs) = F(ieqs) + fe;

        end
    else
        error('Wrong element type!')
    end
    F = F/(L*t);
end

function [S,F, Area] = LinElast2D(nodes, P, t, fload, lambda, mu)


    nnod = size(P,1);

    [nele,knod] = size(nodes);
    loceqs = 2*knod;
    neq = nnod*2;
    % S = spalloc(neq,neq,3*neq);
    F = zeros(neq,1);
    ieqs =  zeros(loceqs,1);
    Area = 0;
    Beps=zeros(3,knod*2); %B matrix for epsilon. 3xn if 2D, 9xn if 3D
    Bdiv=zeros(1,knod*2); %B matrix for divergence
    Bv=zeros(2,knod*2); %3 nodes, 2DOFs
    sq2 = sqrt(2);

    % Pre-allocate datastructures to efficient assembly
    idofs = 2*knod;
    row = zeros(nele, idofs^2); col = row; 
    valS = zeros(nele, idofs^2);


    if knod == 3
        phi = @(xi,eta)[1-xi-eta, xi, eta];
        Bh = @(xi,eta)[-1,1,0
                       -1,0,1];
    %     GP = [2/3 1/6
    %         1/6 1/6
    %         1/6 2/3];
    %     GW = [1;1;1]/3;

        [gcx,gcy,gw]=trigauc([0,1,0],[0,0,1],1);
        GP = [gcx', gcy'];
        GW = gw';

    elseif knod == 6
        phi = @(xi,eta)[eta.*-3.0-xi.*3.0+eta.^2.*2.0+xi.^2.*2.0+eta.^2.*xi.^2.*1.6e1+1.0,xi.*(xi.*2.0-1.0),eta.*(eta.*2.0-1.0),xi.*(xi+eta.^2.*xi.*4.0-1.0).*-4.0,eta.^2.*xi.^2.*1.6e1,eta.*(eta+eta.*xi.^2.*4.0-1.0).*-4.0];
        Bh = @(eta,xi)[xi.*4.0+eta.^2.*xi.*3.2e1-3.0,xi.*4.0-1.0,0.0,xi.*-4.0-xi.*(eta.^2.*4.0+1.0).*4.0-eta.^2.*xi.*1.6e1+4.0,eta.^2.*xi.*3.2e1,eta.^2.*xi.*-3.2e1;
            eta.*4.0+eta.*xi.^2.*3.2e1-3.0,0.0,eta.*4.0-1.0,eta.*xi.^2.*-3.2e1,eta.*xi.^2.*3.2e1,eta.*-4.0-eta.*(xi.^2.*4.0+1.0).*4.0-eta.*xi.^2.*1.6e1+4.0];

        [gcx,gcy,gw]=trigauc([0,1,0],[0,0,1],2);
        GP = [gcx', gcy'];
        GW = gw';
    else
       error('Wrong element type for 2D linear elasticity! Needs to be linear or quadratic triangle!') 
    end
    nGP = size(GP,1);

    for iel = 1:nele
        inod = nodes(iel,:);
        ieqs(1:2:end) = 2*inod'-1; %x-dofs
        ieqs(2:2:end) = 2*inod'-0; %y-dofs

        Pi = P(inod,:);

        Se=zeros(loceqs,loceqs); fe=zeros(loceqs,1);

        for i=1:nGP
            xi = GP(i,1); eta = GP(i,2);
            weight = GW(i);
            Bhi = Bh(xi,eta);
            phii = phi(xi,eta);

            J = Bhi*Pi;
            detJ = det(J);
            area = detJ/2;
            Area = Area + area*weight;
            B = J\Bhi;

            % Mandel notation
            Bdiv(1:2:end)=B(1,:);
            Bdiv(2:2:end)=B(2,:);

            Beps(1,1:2:end)=B(1,:);
            Beps(2,2:2:end)=B(2,:);
            Beps(3,1:2:end)=B(2,:)/sq2;
            Beps(3,2:2:end)=B(1,:)/sq2;

            Bv(1,1:2:end)=phii;
            Bv(2,2:2:end)=phii;

            fe = fe + Bv'*fload(xi,eta)*area;

            se = 2*mu*(Beps'*Beps) + lambda*(Bdiv'*Bdiv);
            Se = Se + se*area*weight;

        end


        % Assembly
        ii = repmat(ieqs,1,idofs); jj = ii';
        row(iel,:) = ii(:);
        col(iel,:) = jj(:);
        valS(iel,:) = Se(:);

    %     S(ieqs,ieqs)=S(ieqs,ieqs)+Se; %This is sloooow!
        F(ieqs)=F(ieqs)+fe;

    end
    S = t*sparse(row(:), col(:), valS(:), neq, neq);

end

function [x, w, A] = gauss(n, a, b)

%------------------------------------------------------------------------------
% gauss.m
%------------------------------------------------------------------------------
%
% Purpose:
%
% Generates abscissas and weigths on I = [ a, b ] (for Gaussian quadrature).
%
%
% Syntax:
%
% [x, w, A] = gauss(n, a, b);
%
%
% Input:
%
% n    integer    Number of quadrature points.
% a    real       Left endpoint of interval.
% b    real       Right endpoint of interval.
%
%
% Output:
%
% x    real       Quadrature points.
% w    real       Weigths.
% A    real       Area of domain.
%------------------------------------------------------------------------------


% 3-term recurrence coefficients:
n = 1:(n - 1);
beta = 1 ./ sqrt(4 - 1 ./ (n .* n));

% Jacobi matrix:
J = diag(beta, 1) + diag(beta, -1); 


% Eigenvalue decomposition:

%
% e-values are used for abscissas, whereas e-vectors determine weights.
%

[V, D] = eig(J);
x = diag(D);


% Size of domain:
A = b - a;


% Change of interval:

%
% Initally we have I0 = [ -1, 1 ]; now one gets I = [ a, b ] instead.
%
% The abscissas are Legendre points.
%

if ~(a == -1 && b == 1)
  x = 0.5 * A * x + 0.5 * (b + a);
end


% Weigths:
w = V(1, :) .* V(1, :);
end

function [gcx,gcy,gv]=trigauc(xc,yc,poldegree)
    % function [gcx,gcy,gv]=trigauc(xc,yc,poldegree)
    %
    %     calculate coordinates and weight for Gauss quadrature
    %      in the triangle with vertices in xc(*),yc(*)
    %
    %     poldegree: degree of polynomial to be integrated exactly (1,2,3,4,5,7,8 or 11)
    %
    switch(poldegree)
    case 1
        gv=1;
        gcx=(xc(1)+xc(2)+xc(3))/3;
        gcy=(yc(1)+yc(2)+yc(3))/3;
    case 2
        gv=[1/3,1/3,1/3];
        gcx=[(xc(1)+xc(2))/2,(xc(2)+xc(3))/2,(xc(3)+xc(1))/2];
        gcy=[(yc(1)+yc(2))/2,(yc(2)+yc(3))/2,(yc(3)+yc(1))/2];
    case 3
        gv=[3,3,3,8,8,8,27]/60;
        gcx=[xc(1),xc(2),xc(3),(xc(1)+xc(2))/2,(xc(2)+xc(3))/2, ...
        (xc(3)+xc(1))/2,(xc(1)+xc(2)+xc(3))/3];
        gcy=[yc(1),yc(2),yc(3),(yc(1)+yc(2))/2,(yc(2)+yc(3))/2, ...
        (yc(3)+yc(1))/2,(yc(1)+yc(2)+yc(3))/3];
    case 4
        gcx=zeros(1,6);gcy=gcx;gv=gcx;
        x1=0.8168475729804585;y1=0.09157621350977073;z1=1-x1-y1;  % multiplicity 3
        w1=0.3298552309659655/3;
        [xv,yv,vv]=multiplicity(x1,y1,w1,xc,yc,3);
        gcx(1:3)=xv;gcy(1:3)=yv;gv(1:3)=vv;	
        x1=0.1081030181680702;y1=0.4459484909159649;z2=1-x1-y1;  % multiplicity 3
        w1=0.6701447690340345/3;
        [xv,yv,vv]=multiplicity(x1,y1,w1,xc,yc,3);
        gcx(4:6)=xv;gcy(4:6)=yv;gv(4:6)=vv;	
    case 5
        x1=0.101286507323456;
        x2=0.797426985353087;
        x3=x1;
        x4=0.470142064105115;
        x5=x4;
        x6=0.059715871789770;
        x7=1/3;
        xsi=[x1,x2,x3,x4,x5,x6,x7];eta=[x1,x1,x2,x6,x4,x4,x7];
        w1=0.125939180544827;w4=0.132394152788506;w7=0.225;
        gv=[w1,w1,w1,w4,w4,w4,w7];
        gcx=[xsi*xc(1)+eta*xc(2)+(1-xsi-eta)*xc(3)];
        gcy=[xsi*yc(1)+eta*yc(2)+(1-xsi-eta)*yc(3)];
    case 6
        gcx=zeros(1,12);gcy=gcx;gv=gcx;
        x1=0.5014265096581342;y1=0.2492867451709329;z1=1-x1-y1;  % multiplicity 3
        w1=0.3503588271790222/3;
        [xv,yv,vv]=multiplicity(x1,y1,w1,xc,yc,3);
        gcx(1:3)=xv;gcy(1:3)=yv;gv(1:3)=vv;	
        x2=0.8738219710169965;y2=0.06308901449150177;z2=1-x2-y2;  % multiplicity 3
        w2=0.1525347191106164/3;
        [xv,yv,vv]=multiplicity(x2,y2,w2,xc,yc,3);
        gcx(4:6)=xv;gcy(4:6)=yv;gv(4:6)=vv;	
        x3=0.6365024991213939;y3=0.05314504984483216;z3=1-x3-y3;  % multiplicity 6
        w3=0.4971064537103575/6;
        [xv,yv,vv]=multiplicity(x3,y3,w3,xc,yc,6);
        gcx(7:12)=xv;gcy(7:12)=yv;gv(7:12)=vv;	
    case 7
        x1=0.065130102902216;
        x2=0.869739794195568;
        x3=x1;
        x4=0.312865496004874;
        x5=0.638444188569810;
        x6=0.048690315425316;
        x7=x5;
        x8=x4;
        x9=x6;
        x10=0.260345966079040;
        x11=0.479308067841920;
        x12=x10;
        x13=1/3;
        xsi=[x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13];
        eta=[x1,x1,x2,x6,x4,x5,x6,x5,x4,x10,x10,x11,x13];
        w1=0.053347235608838;w4=0.077113760890257;w10=0.175615257433208;
        w13=-0.149570044467682;
        gv=[w1,w1,w1,w4,w4,w4,w4,w4,w4,w10,w10,w10,w13];
        gcx=[xsi*xc(1)+eta*xc(2)+(1-xsi-eta)*xc(3)];
        gcy=[xsi*yc(1)+eta*yc(2)+(1-xsi-eta)*yc(3)];
    case 8
        gcx=zeros(1,16);gcy=gcx;gv=gcx;
        x1=1/3;y1=1/3;z1=1-x1-y1;  % multiplicity 1
        w1=0.1443156076777862;
        [xv,yv,vv]=multiplicity(x1,y1,w1,xc,yc,1);
        gcx(1)=xv;gcy(1)=yv;gv(1)=vv;	

        x1=0.08141482341455413;y1=0.4592925882927229;z1=1-x1-y1;  % multiplicity 3
        w1=0.2852749028018549/3;
        [xv,yv,vv]=multiplicity(x1,y1,w1,xc,yc,3);
        gcx(2:4)=xv;gcy(2:4)=yv;gv(2:4)=vv;	

        x1=0.8989055433659379;y1=0.05054722831703103;z1=1-x1-y1;  % multiplicity 3
        w1=0.09737549286959440/3;
        [xv,yv,vv]=multiplicity(x1,y1,w1,xc,yc,3);
        gcx(5:7)=xv;gcy(5:7)=yv;gv(5:7)=vv;	

        x1=0.6588613844964797;y1=0.1705693077517601;z1=1-x1-y1;  % multiplicity 3
        w1=0.3096521116041552/3;
        [xv,yv,vv]=multiplicity(x1,y1,w1,xc,yc,3);
        gcx(8:10)=xv;gcy(8:10)=yv;gv(8:10)=vv;	

        x1=0.0083947774099572110;y1=0.7284923929554041;z1=1-x1-y1;  % multiplicity 6
        w1=0.1633818850466092/6;
        [xv,yv,vv]=multiplicity(x1,y1,w1,xc,yc,6);
        gcx(11:16)=xv;gcy(11:16)=yv;gv(11:16)=vv;	
    case 11
        gcx=zeros(1,28);gcy=gcx;gv=gcx;
        x1=0.9480217181434233;y1=0.02598914092828833;z1=1-x1-y1;  % multiplicity 3
        w1=0.02623293466120857/3;
        [xv,yv,vv]=multiplicity(x1,y1,w1,xc,yc,3);
        gcx(1:3)=xv;gcy(1:3)=yv;gv(1:3)=vv;	

        x1=0.8114249947041546;y1=0.09428750264792270;z1=1-x1-y1;  % multiplicity 3
        w1=0.1142447159818060/3;
        [xv,yv,vv]=multiplicity(x1,y1,w1,xc,yc,3);
        gcx(4:6)=xv;gcy(4:6)=yv;gv(4:6)=vv;	

        x1=0.01072644996557060;y1=0.4946367750172147;z1=1-x1-y1;  % multiplicity 3
        w1=0.05656634416839376/3;
        [xv,yv,vv]=multiplicity(x1,y1,w1,xc,yc,3);
        gcx(7:9)=xv;gcy(7:9)=yv;gv(7:9)=vv;	

        x1=0.5853132347709715;y1=0.2073433826145142;z1=1-x1-y1;  % multiplicity 3
        w1=0.2164790926342230/3;
        [xv,yv,vv]=multiplicity(x1,y1,w1,xc,yc,3);
        gcx(10:12)=xv;gcy(10:12)=yv;gv(10:12)=vv;	

        x1=0.1221843885990187;y1=0.4389078057004907;z1=1-x1-y1;  % multiplicity 3
        w1=0.20798741611661160/3;
        [xv,yv,vv]=multiplicity(x1,y1,w1,xc,yc,3);
        gcx(13:15)=xv;gcy(13:15)=yv;gv(13:15)=vv;	

        x1=0;y1=0.858870281282263640;z1=1-x1-y1;  % multiplicity 6
        w1=0.04417430269980344/6;
        [xv,yv,vv]=multiplicity(x1,y1,w1,xc,yc,6);
        gcx(16:21)=xv;gcy(16:21)=yv;gv(16:21)=vv;	

        x1=0.04484167758913055;y1=0.6779376548825902;z1=1-x1-y1;  % multiplicity 6
        w1=0.2463378925757316/6;
        [xv,yv,vv]=multiplicity(x1,y1,w1,xc,yc,6);
        gcx(22:27)=xv;gcy(22:27)=yv;gv(22:27)=vv;	

        x1=1/3;y1=1/3;z1=1-x1-y1;  % multiplicity 1
        w1=0.08797730116222190;
        [xv,yv,vv]=multiplicity(x1,y1,w1,xc,yc,1);
        gcx(end)=xv;gcy(end)=yv;gv(end)=vv;	

    otherwise 
        disp('Polynomial degrees are 1,2,3,4,5,7,8,11 in trigauc')
        gcx=[];gcy=[];gv=[];
    end


    function [xv,yv,vv]=multiplicity(x,y,v,xc,yc,m)
    z=[x,y,1-x-y];
    switch(m)
    case 1
        xv=[xc(1)*z(1)+xc(2)*z(2)+xc(3)*z(3)];
        yv=[yc(1)*z(1)+yc(2)*z(2)+yc(3)*z(3)];
        vv=v;
    case 3
        xv=[xc(1)*z(1)+xc(2)*z(2)+xc(3)*z(3),xc(1)*z(2)+xc(2)*z(3)+xc(3)*z(1),...
        xc(1)*z(3)+xc(2)*z(1)+xc(3)*z(2)];
        yv=[yc(1)*z(1)+yc(2)*z(2)+yc(3)*z(3),yc(1)*z(2)+yc(2)*z(3)+yc(3)*z(1),...
        yc(1)*z(3)+yc(2)*z(1)+yc(3)*z(2)];
        vv=v*ones(1,3);
    case 6
        xv=[xc(1)*z(1)+xc(2)*z(2)+xc(3)*z(3),xc(1)*z(1)+xc(2)*z(3)+xc(3)*z(2),...
        xc(1)*z(2)+xc(2)*z(1)+xc(3)*z(3),xc(1)*z(2)+xc(2)*z(3)+xc(3)*z(1),...
        xc(1)*z(3)+xc(2)*z(1)+xc(3)*z(2),xc(1)*z(3)+xc(2)*z(2)+xc(3)*z(1)];
        yv=[yc(1)*z(1)+yc(2)*z(2)+yc(3)*z(3),yc(1)*z(1)+yc(2)*z(3)+yc(3)*z(2),...
        yc(1)*z(2)+yc(2)*z(1)+yc(3)*z(3),yc(1)*z(2)+yc(2)*z(3)+yc(3)*z(1),...
        yc(1)*z(3)+yc(2)*z(1)+yc(3)*z(2),yc(1)*z(3)+yc(2)*z(2)+yc(3)*z(1)];
        vv=v*ones(1,6);
    end
    end
end