import  graph3;
struct reper {
    triple o,i,j,k;
    guide3 oi,oj,ok;
    string olabel,ilabel,jlabel,klabel,labelsize;

    transform3 trans;

    void update_axis() {
        this.oi = this.o -- this.o+this.i;
        this.oj = this.o -- this.o+this.j;
        this.ok = this.o -- this.o+this.k;
    }

    void rotate(real alpha,triple u) {
    this.o = three.rotate(alpha,u)*this.o;
        this.i = three.rotate(alpha,u)*this.i;
        this.j = three.rotate(alpha,u)*this.j;
        this.k = three.rotate(alpha,u)*this.k;
        trans = three.rotate(alpha,u) * trans;
        this.update_axis();
    }

    void operator init(triple o=(0,0,0),triple i=(1,0,0),triple k=(0,0,1),
                       triple j=(0,1,0),string ilabel="\vec{i}",
                       string jlabel="\vec{j}",string klabel="\vec{k}",
                       string olabel="o",string labelsize="\footnotesize"){
        this.o = o; this.i = i; this.j = j; this.k = k;
        this.update_axis();
        this.olabel = olabel; this.ilabel = ilabel;
        this.jlabel = jlabel; this.klabel = klabel;
        this.labelsize = labelsize;

        trans = identity4;

        this.rotate(90, this.i);
    }

    void shift(triple dxyz) {
        this.o += dxyz;
        trans = three.shift(dxyz)*  trans;
        this.update_axis();
    }


    void draw(triple view_point) {
        draw(this.oi,red,Arrow3);
        draw(this.oj,red,Arrow3);
        draw(this.ok,red,Arrow3);

        real dij = dot(view_point-this.o,cross(this.i,this.j));
        triple shift_o = unit(this.i+this.j);
        if (dij<0) { shift_o = - shift_o; }
        label(this.labelsize+" $"+this.olabel+"$",this.o,shift_o,blue);

        triple shift_i = cross(this.i,view_point-this.o);
        shift_i = unit(shift_i);
        label(this.labelsize+" $"+this.ilabel+"$",this.o+this.i/2,shift_i,blue);

        triple shift_j = cross(this.j,view_point-this.o);
        shift_j = unit(shift_j);
        label(this.labelsize+" $"+this.jlabel+"$",this.o+this.j/2,shift_j,blue);

        triple shift_k = cross(this.k,view_point-this.o);
        shift_k = unit(shift_k);
        label(this.labelsize+" $"+this.klabel+"$",this.o+this.k/2,shift_k,blue);

    }

    triple fromModel(triple pt) {
        real[] pp = {pt.x, pt.y, pt.z, 1};
        pp = this.trans*pp;
        return (pp[0], pp[1], pp[2]);
    }

    triple toModel(triple pt) {
        real[] pp = {pt.x, pt.y, pt.z, 1};
        pp = inverse(this.trans)*pp;
        return (pp[0], pp[1], pp[2]);
    }
}//Koniec definicji klasy reper



struct frustum {
    reper  ijk;
    real near,far;
    real left,right,down,top,aspect;
    guide3 fface,lface,rface,dface,tface; //nface

    void update_faces() {
        triple ldn =  this.left*this.ijk.i+this.down*this.ijk.j-this.near*this.ijk.k;
        triple rdn = this.right*this.ijk.i+this.down*this.ijk.j-this.near*this.ijk.k;
        triple ltn =  this.left*this.ijk.i+ this.top*this.ijk.j-this.near*this.ijk.k;
        triple rtn = this.right*this.ijk.i+ this.top*this.ijk.j-this.near*this.ijk.k;

        triple oldn = this.ijk.o+ldn;
        triple ordn = this.ijk.o+rdn;
        triple oltn = this.ijk.o+ltn;
        triple ortn = this.ijk.o+rtn;

        real beta = this.far/this.near;
        triple oldf = this.ijk.o+beta*ldn;
        triple ordf = this.ijk.o+beta*rdn;
        triple oltf = this.ijk.o+beta*ltn;
        triple ortf = this.ijk.o+beta*rtn;

        this.fface = ordf--oldf--oltf--ortf--cycle;
        this.lface = oldn--oltn--oltf--oldf--cycle;
        this.rface = ordn--ordf--ortf--ortn--cycle;
        this.dface = oldn--oldf--ordf--ordn--cycle;
        this.tface = oltn--ortn--ortf--oltf--cycle;
    }

    void operator init(real near=1,real far=10,
                       real left=-1,real right=1, real down=-1,real aspect=1,
                       reper ijk=reper(labelsize="\tiny")){
        this.near = near; this.far = far;
        this.left = left; this.right = right;
        this.down = down; this.aspect = aspect;
        this.top = down+(right-left)/aspect;
        this.ijk = ijk;
        this.update_faces();
    }

    void shift(triple dxyz) {
        this.ijk.shift(dxyz);
        this.update_faces();
    }

    void rotate(real alpha,triple u) {
        this.ijk.rotate(alpha,u);
        this.update_faces();
    }

    void draw(triple view_point) {
        this.ijk.draw(view_point);
        draw(this.fface);
        //draw(this.nface);
        draw(this.dface);
        draw(this.tface);
        draw(this.lface);
        draw(this.rface);
    }

    real[][] frust() {
        real n = this.near;
        real f = this.far;
        real b = this.down;
        real t = this.top;
        real l = this.left;
        real r = this.right;

        real[][] a={{2*n / (r-l),           0,  (r+l) / (r-l),              0},
                {          0, 2*n / (t-b),  (t+b) / (t-b),              0},
            {          0,           0, -(f+n) / (f-n), -2*f*n / (f-n)},
            {          0,           0,             -1,              0}};

    return a;
    }

    triple toDevice(triple p) {
        real[] pp = { p.x, p.y, p.z, 1 };
        pp = frust()*pp;
        pp /= pp[3];
        return (pp[0], pp[1], pp[2]);
    }

    triple fromDevice(triple p) {
        real[] pp = { p.x, p.y, p.z, 1 };
        pp = inverse(frust())*pp;
        pp /= pp[3];
        return (pp[0], pp[1], pp[2]);
    }

    // p - punkt we współrzędnych świata
    // zwracany również punkt we współrzędnych świata
    triple project(triple p) {
        // punkt we współrzędnych lokalnych dla związanego repera
        triple lp = this.ijk.toModel(p);
        // punkt we współrzędnych urządzenia
        triple dp = this.toDevice(lp);
        // punkt we współrzędnych urządzenia zrzutowany na near plane (z = -1)
        triple pd = (dp.x, dp.y, -1);
        // zrzutowany punkt we współrzędnych lokalnych
    triple pl = this.fromDevice(pd);
    // zrzutowany punkt we współrzędnych świata
    triple pw = this.ijk.fromModel(pl);

    return pw;
    }

}//Koniec definicji klasy frustum


triple ota(triple p) {
    return (p.x, -p.z, p.y);
}

triple ota(real x, real y, real z) {
    return (x, -z, y);
}

triple ato(triple p) {
    return (p.x, p.z, -p.y);
}

triple ato(real x, real y, real z) {
    return (x, z, -y);
}

import math;


settings.outformat="pdf";
settings . prc= false ;
defaultfilename = "reper-2" ;
size3 ( 16cm , 16cm , keepAspect=true) ;
size ( 16cm , 16cm , keepAspect=true) ;
texpreamble("\def\vec#1{\overrightarrow{#1}}");


triple view_point = (30 , -30 ,30);
currentprojection = perspective ( view_point ) ;

//reper r = reper ( labelsize="\tiny" ) ;
//write ( 'o=' , r.o ) ; write ( 'i=' , r.i ) ;
//write ( 'j=' , r.j ) ; write ( 'k=' , r.k ) ;
//r.draw ( view_point ) ;

//r.shift ((2 ,0 ,0));
//r.draw ( view_point ) ;

//r.shift ((0 ,2 ,0));
//r.rotate (60 , r.k ) ;
//r.draw ( view_point ) ;

//triple p1 = (1,0,0);
//dot (p1, blue);
//triple p2 = r.fromModel(p1);
//dot (p2, blue);
//triple p3 = r.toModel(p2);

//write('p1=', p1);
//write('p2=', p2);
//write('p3=', p3);


frustum fr1 = frustum(near=4 , far=15);
fr1.draw ( view_point ) ;

frustum fr2 = frustum(near=4 , far=15);
fr2.shift ((6 ,0 ,0));
fr2.rotate (30 , fr2.ijk.k ) ;
fr2.draw (view_point) ;


triple l1 = (-1, 0, -5);


/*triple g1 = fr1.ijk.fromModel(l1);
triple g2 = fr2.ijk.fromModel(l1);
dot(g1, blue);
dot(g2, blue);
write('g1=', g1);
write('g2=', g2);*/

/*
triple d1 = fr1.toDevice(l1);

write('l1=', l1);
write('d1=', d1);

triple dp = (d1.x, d1.y, -1);
triple p1 = fr1.fromDevice(dp);
p1 = fr1.ijk.fromModel(p1);*/

triple g = (3.3, 14, 1.5);
dot(g, blue);
triple p1 = fr1.project(g);
triple p2 = fr2.project(g);
dot(p1, red);
dot(p2, red);

// testy
triple l1 = (-1, -1, -4);
triple g1 = fr1.ijk.fromModel(l1);
dot("$g_1$", g1, green);
triple g2 = fr2.ijk.fromModel(l1);
dot("$g_2$", g2, green);

triple d1 = fr2.toDevice(l1);
triple d2 = fr1.fromDevice((1, 1, 1));
triple w2 = fr1.ijk.fromModel(d2);
dot("$d_2$", w2, green);

write('We wspolrzednych modelu: l1=', l1);
write('We wspolrzednych globalnych dla fr1: g1=', g1);
write('We wspolrzednych globalnych dla fr2: g2=', g2);
write('g1 to lewy dolny rog frustuma na near plane');
write('Przeliczenie na wspolrzedne urzadzenia');
write('d1 (powinno byc -1, -1, -1): ', d1);
write('Przeliczanie ze wspolrzednych urzadzenia');
write('d2 (powinno byc -3.75, -3.75, -15): ', d2);
write('d2 powinno znajdowac sie w prawym gornym rogu pierwszego frustuma na far plane');