Commit 8f7da472 authored by Oleg Dzhimiev's avatar Oleg Dzhimiev
Browse files

all moved to a separate function

parent 80551632

js/x3dom_deltas.js

+24 −220
Original line number Diff line number Diff line
@@ -73,9 +73,14 @@ function Av_func(mark,pars){ 


  var e2_model = W2M.multMatrixVec(e2_world);



  x3dom_draw_line("soup0","white", new x3dom.fields.SFVec3f(0,0,0), e1_model);

  x3dom_draw_line("soup1","white", new x3dom.fields.SFVec3f(0,0,0), e2_model);

  x3dom_draw_line("soup2","white", e1_model, e2_model);

  //x3dom_draw_line("soup0","white", new x3dom.fields.SFVec3f(0,0,0), e1_model);

  //x3dom_draw_line("soup1","white", new x3dom.fields.SFVec3f(0,0,0), e2_model);

  //x3dom_draw_line("soup2","white", e1_model, e2_model);



  // world axes are correct - verified

  //x3dom_draw_line("soup0","red",   e1_model, e1_model.add(W2M.multMatrixVec(new x3dom.fields.SFVec3f(10,0,0))));

  //x3dom_draw_line("soup1","green", e1_model, e1_model.add(W2M.multMatrixVec(new x3dom.fields.SFVec3f(0,10,0))));

  //x3dom_draw_line("soup2","blue",  e1_model, e1_model.add(W2M.multMatrixVec(new x3dom.fields.SFVec3f(0,0,10))));



  console.log("MARK-on-MODEL in WORLD: "+e1_world.toString());
@@ -87,8 +92,8 @@ function Av_func(mark,pars){ 
  // point in Model

  // from 0,0,0

  var e1_d = x3dom_3d_distance(e1_world.x,e1_world.y,e1_world.z,false);

  var e1_k = 20;

  //var e1_k = 1;

  //var e1_k = 20;

  var e1_k = 1;

  var e1_abc = [e1_k*0.00038*e1_d, e1_k*0.00038*e1_d, 2*e1_k*e1_d*e1_d/10000];

  //var e1_abc = [e1_k*0.00038*e1_d, e1_k*0.00038*e1_d, e1_k*e1_d*e1_d/25000];
@@ -109,6 +114,7 @@ function Av_func(mark,pars){ 
  },transparency=0.5);

  */





  var RE1 = x3dom.fields.SFMatrix4f.identity();

  RE1.setValue(xa1,ya1,za1);

  // and so RE1 is in the world coordinates
@@ -142,7 +148,7 @@ function Av_func(mark,pars){ 
  var C1 = RE1xJE1.mult(RE1xJE1.transpose());

  var C2 = RE2xJE2.mult(RE2xJE2.transpose());

  var C = C1.add(C2);

  var C = C1.add(C1);

  //var C = C1.add(C1);

  //var C = C2.add(C2);



  Cn = matrix_x3dom_to_numeric(C);
@@ -159,7 +165,7 @@ function Av_func(mark,pars){ 
  var RESmatrix = new x3dom.fields.SFMatrix4f(

               vec0.x, vec1.x, vec2.x, 0,

               vec0.y, vec1.y, vec2.y, 0,

               vec0.z, vec1.x, vec2.z, 0,

               vec0.z, vec1.z, vec2.z, 0,

                    0,      0,      0, 1

             );

  //RESmatrix = RESmatrix.transpose();
@@ -173,12 +179,21 @@ function Av_func(mark,pars){ 


  var A = RESaxes.inverse().mult(RESmatrix.transpose());



  x3dom_draw_ellipsoid_by_semiaxes_and_center("ec1","gold",{

  x3dom_draw_ellipsoid_by_semiaxes_and_center("ec1","red",{

    O:  W2M.multMatrixVec(e2_world),

    Ox: W2M.multMatrixVec(RESmatrix.transpose().e0().multiply(es_a)),

    Oy: W2M.multMatrixVec(RESmatrix.transpose().e1().multiply(es_b)),

    Oz: W2M.multMatrixVec(RESmatrix.transpose().e2().multiply(es_c))

  },transparency=0.7);

  },transparency=0.8);



  // and so, the error vector is E=Av

  var Av = A.multMatrixVec(v);

  // draw the vector

  x3dom_draw_line("err0","gold", e1_model, e1_model.add(W2M.multMatrixVec(Av)));



  // Is it correct? The direction seems right for close and far objects



  //



}
@@ -223,217 +238,6 @@ function x3dom_delta_markers(){ 


  return 0;



  var p_mark_ll = new L.LatLng(marker.latitude, marker.longitude);

  //var p_cam_ll  = Camera._latlng;

  var p_cam_ll  = new L.LatLng(Data.camera.kml.latitude,Data.camera.kml.longitude);



  var p_model = x3dom_delta_map2scene(p_cam_ll, p_mark_ll);

  //p_w = x3dom_scene_to_real(p_w.x,p_w.y,p_w.z);





  // vertical ellipse's center

  var e2_c = p_model;

  // depth ellipse's center

  var e1_c = new x3dom.fields.SFVec3f(marker.x,marker.y,marker.z);





  // E2: draw vertical ellipsoid



  // just a coefficient

  //var e2_k = 1/2;

  var e2_k = 1;

  var e2_abc = [e2_k*1,e2_k*30,e2_k*1];

  //var e2_scale = e2_abc.join(",");

  //x3dom_draw_ellipsoid("e2","green",e2_c.toString(),"",e2_scale);



  var R0  = Data.camera.Matrices.R0;

  var R0i = Data.camera.Matrices.R0.inverse();



  var xa2 = new x3dom.fields.SFVec3f(e2_abc[0],         0,         0);

  var ya2 = new x3dom.fields.SFVec3f(        0, e2_abc[1],         0);

  var za2 = new x3dom.fields.SFVec3f(        0,         0, e2_abc[2]);



  /*

  x3dom_draw_ellipsoid_by_semiaxes_and_center("e2","green",{

    O:  e2_c,

    Ox: R0i.multMatrixVec(xa2),

    Oy: R0i.multMatrixVec(ya2),

    Oz: R0i.multMatrixVec(za2)

  },transparency=0.0);

  */





  var RE2 = x3dom.fields.SFMatrix4f.identity();

  //RE2.setValue(xa,ya,za);



  // draw ellipsoid axes

  //var xa = new x3dom.fields.SFVec3f(1,0,0);

  //var ya = new x3dom.fields.SFVec3f(0,1,0);

  //var za = new x3dom.fields.SFVec3f(0,0,1);



  //x3dom_draw_line("xa","red",  e2_c, e2_c.add(xa.multiply(e2_abc[0])));

  //x3dom_draw_line("ya","green",e2_c, e2_c.add(ya.multiply(e2_abc[1])));

  //x3dom_draw_line("za","blue", e2_c, e2_c.add(za.multiply(e2_abc[2])));



  // to 0,0,0

  //x3dom_draw_line("test0","white", e2_c, new x3dom.fields.SFVec3f(0,0,0));



  //x3dom_draw_line("test1","tomato", new x3dom.fields.SFVec3f(0,0,0), new x3dom.fields.SFVec3f(100,0,0));

  //x3dom_draw_line("test2","seagreen", new x3dom.fields.SFVec3f(0,0,0), new x3dom.fields.SFVec3f(0,100,0));

  //x3dom_draw_line("test3","royalblue", new x3dom.fields.SFVec3f(0,0,0), new x3dom.fields.SFVec3f(0,0,100));

  //x3dom_draw_line("test4","royalblue", e2_c.add(za.multiply(e2_abc[2])), new x3dom.fields.SFVec3f(0,0,100));



  // e1 direction

  x3dom_draw_line("helper_line0a","white", e1_c, new x3dom.fields.SFVec3f(0,0,0));

  x3dom_draw_line("helper_line0b","white", e2_c, new x3dom.fields.SFVec3f(0,0,0));

  // e1 direction from e2_c

  //x3dom_draw_line("helper_line1","white", e2_c.add(e1_c), e2_c.subtract(e1_c));



  x3dom_draw_line("helper_line3","white", e1_c, e2_c);



  // E1: draw tilted ellipsoid



  var e1_d = x3dom_3d_distance(e1_c.x,e1_c.y,e1_c.z,false);

  //var e1_k = 20;

  var e1_k = 1;

  var e1_abc = [e1_k*0.00038*e1_d,e1_k*0.00038*e1_d,2*e1_k*e1_d*e1_d/10000];



  console.log("e1 abc: "+e1_abc.join(" "));



  // draw ellipsoid axes

  var e1_dir  = e1_c;

  e1_dir = R0.multMatrixVec(e1_dir.normalize());



  var xa1 = e1_dir.cross(new x3dom.fields.SFVec3f(0,1,0));

  var ya1 = xa1.cross(e1_dir);

  var za1 = e1_dir.negate();



  // next construct rotation matrix

  // it's in the WORLD coordinates

  var RE1 = x3dom.fields.SFMatrix4f.identity();

  RE1.setValue(xa1,ya1,za1);



  //console.log(RE1.toString());



  /*

  x3dom_draw_ellipsoid_by_semiaxes_and_center("e1b","green",{

    O:  e2_c,

    Ox: R0i.multMatrixVec(xa1).multiply(e1_abc[0]),

    Oy: R0i.multMatrixVec(ya1).multiply(e1_abc[1]),

    Oz: R0i.multMatrixVec(za1).multiply(e1_abc[2])

  },transparency=0.0);

  */



  // now let's get to covariance matrix

  var JE1 = x3dom_ellipsoid_inertia_tensor_v2(1,e1_abc[0],e1_abc[1],e1_abc[2]);

  var RE1xJE1 = RE1.mult(JE1);



  var JE2 = x3dom_ellipsoid_inertia_tensor_v2(1,e2_abc[0],e2_abc[1],e2_abc[2]);

  var RE2xJE2 = RE2.mult(JE2);



  var C1 = RE1xJE1.mult(RE1xJE1.transpose());

  var C2 = RE2xJE2.mult(RE2xJE2.transpose());



  var C = C1.add(C2);

  // testing

  //var C = C1.add(C1);



  Cn = matrix_x3dom_to_numeric(C);

  Bn = numeric.eig(Cn);



  //console.log(Bn);



  var es_a = Math.sqrt(Bn.lambda.x[0]);

  var es_b = Math.sqrt(Bn.lambda.x[1]);

  var es_c = Math.sqrt(Bn.lambda.x[2]);



  console.log("e1 restored abc: "+[es_a,es_b,es_c].join(" "));



  var RESmatrix = new x3dom.fields.SFMatrix4f(

               Bn.E.x[0][0], Bn.E.x[1][0], Bn.E.x[2][0], 0,

               Bn.E.x[0][1], Bn.E.x[1][1], Bn.E.x[2][1], 0,

               Bn.E.x[0][2], Bn.E.x[1][2], Bn.E.x[2][2], 0,

                          0,            0,            0, 1

             );



  console.log(RESmatrix.toString());



  var RESaxes = new x3dom.fields.SFMatrix4f(

               es_a,    0,    0, 0,

                  0, es_b,    0, 0,

                  0,    0, es_c, 0,

                  0,    0,    0, 1

             );



  var A = RESaxes.inverse().mult(RESmatrix.transpose());



  console.log(RESaxes.toString());



  var RES = RESmatrix.mult(RESaxes);



  var RESt = RES.transpose();

  console.log("RES");

  console.log(RES.toString());

  console.log("RESt");

  console.log(RESt.toString());



  /*

  x3dom_draw_ellipsoid_by_semiaxes_and_center("e1c","red",{

    O:  e2_c,

    Ox: R0i.multMatrixVec(RESmatrix.transpose().e0()).multiply(es_a),

    Oy: R0i.multMatrixVec(RESmatrix.transpose().e1()).multiply(es_b),

    Oz: R0i.multMatrixVec(RESmatrix.transpose().e2()).multiply(es_c)

  },transparency=0.8);

  */



  var v = R0.multMatrixVec(e1_c.subtract(e2_c));

  var Av = A.multMatrixVec(v);

  var Av_model = R0i.multMatrixVec(Av);



  x3dom_draw_line("helper_lineX","yellow", e2_c, e2_c.add(Av_model));



  // now get error:



  // we have:

  // 1. e1_c

  // 2. RES and es_a, es_b, es_c



  // RES x e1_c

  var RESi = RES.inverse();



  var E = RESi.multMatrixVec(v);



  console.log("delta vector in ellipsoid:");

  console.log(E.toString());



  E = E.multComponents(new x3dom.fields.SFVec3f(1/es_a, 1/es_b, 1/es_c));



  console.log(E.toString());

  console.log("E^2 from eigen: " +E.length()*E.length());



  // scale the result ellipsoid so it with touch another marker

  // and it did - it only tests that the scale is correct



  /*

  x3dom_draw_ellipsoid_by_semiaxes_and_center("e1d","gold",{

    O:  e2_c,

    Ox: RES.e0().multiply(E.length()*es_a),

    Oy: RES.e1().multiply(E.length()*es_b),

    Oz: RES.e2().multiply(E.length()*es_c)

  },transparency=0.7);

  */



  // alternative calculations where E^2 = delta^T x C^-1 x delta

  var delta = v;

  var Ci = C.inverse();



  var Ci_x_delta = Ci.multMatrixVec(delta);

  var E2 = delta.dot(Ci_x_delta);



  console.log("E^2 from d^T x C^-1 x d: "+E2);



  return 0;



}



/*