//calculate and plot 3D mirror locations for angled laser beam
//to be reflected as though from a point source at the origin
laser_offset=[12;0]; //[cm] x;y parametric line offset
lo3=[laser_offset(1);0;laser_offset(2)]; //[cm] x;y;z parametric line offset in XZ plane
laser_angle=(180-59)/180*%pi; //[rad] up from X axis CCW about Y axis
lbw=0.2; //[cm] laser beam width along Y
lbh=0.4; //[cm] laser beam height
msl=0.5; //[cm] mirror side length
mth=0.1; //[cm] mirror thickness
mil=2*sqrt(2*((msl/2)^2)+(mth^2)); //[cm] maximal interference distance
pll=0.5; //[cm] projected line render length
lll=0.2; //[cm] laser line render length
lx=[cos(laser_angle);sin(laser_angle)]; //dx;dy parametric line slope
lx3=[cos(laser_angle);0;sin(laser_angle)]; //dx;dy;dz parametric line slope
lp3=[-lx(2);0;lx(1)]; //perpendicular line to laser
projected_angles=linspace(0,90,128)/180*%pi; //[rad] horizontal from origin CCW
//projected_angles($)=[];
po=[0;0]; //x;y;z apparent point source location
na=length(projected_angles);
mplot3d=%t;
mplotlbm=%f;
coneplot=%t;
num_thetas=na*10;
mts=mirror_ttv1;//(0:(na-1)); //mirror theta slots
//mts(127)=64;
//mts(126)=32;
//mts(125)=96;
mthetas=(mts/num_thetas)*2*%pi; //mirror theta locations

function txy=intlc(lo,lx,po,px,py)
    //intersect 3D line and 3D plane - if parallel then closest match
    //column vectors
    //line given by lo+t*lx
    //plane given by po+x*px+y*py
    //return [t;x;y]
    mat=[-lx,px,py];
    res=[lo-po];
    txy=mat\res;
endfunction

function hit=intcc(vs1,vs2) //gives false hits if line is parallel to plane
    //check for intersection of two cuboids by intersecting each edge and face
    //vs1 and vs2 are 3x8 vertices such that cube is drawn 1-2-3-4 5-6-7-8 1-5 2-6 3-7 4-8
    vss=[1,1,1,2,2,3,3,4,5,5,6,7;
         2,4,5,3,6,4,7,8,6,8,7,8];//12 edges
    fss=[1,1,1,7,7,7;
         2,2,8,3,3,8;
         4,8,4,6,8,6];//6 faces
    for(ni=1:2)
        if(ni==1)
            cb1=vs1;
            cb2=vs2;
        else
            cb1=vs2;
            cb2=vs1;
        end
        for(ne=1:12)
            for(nf=1:6)
                lo=cb1(:,vss(1,ne));
                lx=cb1(:,vss(2,ne))-lo;
                po=cb2(:,fss(1,nf));
                px=cb2(:,fss(2,nf))-po;
                py=cb2(:,fss(3,nf))-po;
                txy=intlc(lo,lx,po,px,py);
                eps=1e-4;
                td=norm((lo+lx*txy(1))-(po+px*txy(2)+py*txy(3)));
                hit=(sum((txy>=-eps)&(txy<=(1+eps)))==3)&&(td<eps);
                if(hit)
                    //disp([ni,ne,nf]);
                    //disp(txy);
                    //disp(td);
                    return;
                end
            end
        end
    end
endfunction

function tu=pl_intersect(ao,ax,bo,bx)
    //intersect two 2D parametric lines with offset and slope vectors
    mat=[-ax(1),bx(1);-ax(2),bx(2)];
    val=[ao(1)-bo(1);ao(2)-bo(2)];
    tu=mat\val;
endfunction

function av=addvs(longv,shortv)
    //adds vectors using the smaller one element by element
    s2=size(longv);
    s1=size(shortv);
    if(length(s1)~=2||length(s2)~=2)
        error("Unsupported vector dimension");
    end
    if(s1(2)==1&&s2(1)==s1(1)&&s2(2)>=1)
        av=longv;
        for(i=1:s1(1))
            av(i,:)=av(i,:)+shortv(i);
        end
        return
    end
    if(s1(1)==1&&s2(2)==s1(2)&&s2(1)>=1)
        av=longv;
        for(i=1:s2(2))
            av(:,i)=av(:,i)+shortv(i);
        end
        return
    end
    error("Unsupported vector size");
endfunction

function rv=reflect(sv,mv)
    dv=[mv(1,:)-sv(1);mv(2,:)-sv(2);mv(3,:)-sv(3)];
    dp=sum(dv.*mv,1);
    a1=[mv(1,:).*dp;mv(2,:).*dp;mv(3,:).*dp];
    a2=2.*(dv-a1);
    rv=[sv(1)+a2(1,:);sv(2)+a2(2,:);sv(3)+a2(3,:)];
endfunction

mir0=[0,0,0,0;
    -msl/2,-msl/2,msl/2,msl/2;
    msl/2,-msl/2,-msl/2,msl/2];
mir0=[mir0,addvs(mir0,[-mth;0;0])]; //3D points of mirror front and back
    //connect points as 1-2-3-4-1 5-6-7-8-5 1-5 2-6 3-7 4-8 for a cuboid rendering

mpl=[];
mcs=[];
dtml=(2*%pi/na)/4; //[rad] +-theta of mirror for each laser to cover an equal share of full circle

npl=1;

for(i=1:20)//(na-20):na)
    pa=projected_angles(i);
    px=[cos(pa);sin(pa)]; //dx;dy of projected line
    tu=pl_intersect(po,px,laser_offset,lx);
    ipt=po+px*tu(1);//lines intersection
    ia=pa-(pa-(laser_angle-%pi))/2;//angle for mirror normal
    if(mplot3d)
        mt=mthetas(i);
        trm=[cos(mt),-sin(mt),0;sin(mt),cos(mt),0;0,0,1]; //rotation matrix for theta about Z
        prm=[cos(ia),0,-sin(ia);0,1,0;sin(ia),0,cos(ia)]; //rotation matrix for phi about Y
        mpts=prm*mir0; //phi-rotated mirror
        ipt3=[ipt(1);0;ipt(2)];
        mpts=addvs(mpts,ipt3); //moved to expected location
        lpts=[];
        if(mplotlbm)
            for(lia=0)//[-1,0,1])
                lrl=lia*dtml;
                trl=[cos(lrl),-sin(lrl),0;sin(lrl),cos(lrl),0;0,0,1];
                for(ls1=0)//[-1,1])
                    for(ls2=[-1,1])
                        lor=lo3+ls1*lbh*0.5*lp3+ls2*lbw*0.5*[0;1;0];
                        lor=trl*lor;
                        lxr=trl*lx3;
                        txy=intlc(lor,lxr,mpts(:,1),mpts(:,2)-mpts(:,1),mpts(:,4)-mpts(:,1));
                        ix=lor+txy(1)*lxr;
                        mv=prm*[1;0;0];
                        rv=reflect(-lxr,mv);
                        nlpts=addvs([-lll*lxr,[0;0;0],pll*rv],ix);
                        lpts=[lpts,nlpts];
                    end
                end
            end
        end
        //lpts=addvs([-lll*lx(1),0,pll*px(1);0,0,0;-lll*lx(2),0,pll*px(2)],ipt3); //laser incoming and outgoing beams, connect as 1-2-3
        //lpts=addvs(lpts,[0;-lbd/2;0]);
        //lpts=[lpts,addvs(lpts,[0;lbd;0])];//include beam width
        mpts=trm*mpts; //rotate to theta
        mcp=trm*ipt3;
        cst='blue';
        if(~isempty(mpl))
            drb=sqrt(sum(addvs(mcs,-mcp).^2,1));
            dri=find(drb<=mil);
            hit=%f;
            for(di=dri)
                if(intcc(mpl(:,:,di),mpts))//intersect with nearby mirror
                    hit=%t;
                end
            end
            if(hit)
                disp(sprintf("Error: mirror interference %d", i));
                cst='red';
            end
        end
        mpl(:,:,npl)=mpts;
        mcs(:,npl)=mcp;
        npl=npl+1;
        lpts=trm*lpts;
        msq=[1,2,3,4,1,5,6,7,8,5];
        param3d(mpts(1,msq),mpts(2,msq),mpts(3,msq));
        gce().foreground = color(cst);
        msq=[2,6];
        param3d(mpts(1,msq),mpts(2,msq),mpts(3,msq),cst);
        gce().foreground = color(cst);
        msq=[3,7];
        param3d(mpts(1,msq),mpts(2,msq),mpts(3,msq),cst);
        gce().foreground = color(cst);
        msq=[4,8];
        param3d(mpts(1,msq),mpts(2,msq),mpts(3,msq),cst);
        gce().foreground = color(cst);
        cst='red';
        jq=size(lpts,2)/3;
        for(j=1:jq)
            msq=[1,2,3]+(j-1)*3;
            param3d(lpts(1,msq),lpts(2,msq),lpts(3,msq),cst);
            gce().foreground = color(cst);
        end
    end
end

if(coneplot)
    co3=lo3+(lbh)*lp3;
    txy=intlc(co3,lx3,[0;0;0],[0;1;0],[0;0;1]);//tip of cone
    cot=co3+lx3*txy(1)*0.18;
    co3=co3+lx3*txy(1)*0.0;
    nt=100;
    cst='green';
    msq=[1,2];
    for(i=1:nt)
        mt=i/nt*2*%pi;
        trm=[cos(mt),-sin(mt),0;sin(mt),cos(mt),0;0,0,1]; //rotation matrix for theta about Z
        lpts=trm*[co3,cot];
        param3d(lpts(1,msq),lpts(2,msq),lpts(3,msq),cst);
        gce().foreground = color(cst);
    end
end

isoview on

