Commit 1594de3f authored by Oleg Dzhimiev's avatar Oleg Dzhimiev
Browse files

combined ellipse matched

parent 3ece274b

js/x3dom_deltas.js

+88 −172
Original line number Original line Diff line number Diff line
@@ -30,9 +30,19 @@ function x3dom_delta_markers(){ 
  // just a coefficient

  // just a coefficient

  var e2_k = 1/2;

  var e2_k = 1/2;

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

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

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

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

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

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





  x3dom_draw_ellipsoid_by_semiaxes_and_center("e2","green",{

    O:  e2_c,

    Ox: new x3dom.fields.SFVec3f(e2_abc[0],         0,         0),

    Oy: new x3dom.fields.SFVec3f(        0, e2_abc[1],         0),

    Oz: new x3dom.fields.SFVec3f(        0,         0, e2_abc[2])

  },0.0);



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

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



  // draw ellipsoid axes

  // draw ellipsoid axes

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

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

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

  //var ya = new x3dom.fields.SFVec3f(0,1,0);
@@ -62,8 +72,8 @@ function x3dom_delta_markers(){ 
  var e1_d = x3dom_3d_distance(e1_c.x,e1_c.y,e1_c.z,false);

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

  var e1_k = 20;

  var e1_k = 20;

  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,2*e1_k*e1_d*e1_d/10000];

  var e1_scale = e1_abc.join(",");



  console.log("e1 scale: "+e1_scale);

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





  // draw ellipsoid axes

  // draw ellipsoid axes

  var e1_dir  = e1_c;

  var e1_dir  = e1_c;
@@ -73,13 +83,15 @@ function x3dom_delta_markers(){ 
  var ya = xa.cross(e1_dir);

  var ya = xa.cross(e1_dir);

  var za = e1_dir.negate();

  var za = e1_dir.negate();





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

  // need to normalize?

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

  /*

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

  xa = xa.normalize();

  ya = ya.normalize();

  za = za.normalize();

  */





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

  var R0 = Data.camera.Matrices.R0;

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

  var R0i = R0.inverse();

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





  // next construct rotation matrix

  // next construct rotation matrix

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

  var RE1 = x3dom.fields.SFMatrix4f.identity();
@@ -87,204 +99,97 @@ function x3dom_delta_markers(){ 




  console.log(RE1.toString());

  console.log(RE1.toString());





  var translation = new x3dom.fields.SFVec3f(0,0,0);

  x3dom_draw_ellipsoid_by_semiaxes_and_center("e1b","green",{

  var scaleFactor = new x3dom.fields.SFVec3f(1,1,1);

    O:  e2_c,

  var rotation = new x3dom.fields.Quaternion(0,0,1,0);

    Ox: xa.multiply(e1_abc[0]),

  var scaleOrientation = new x3dom.fields.Quaternion(0,0,1,0);

    Oy: ya.multiply(e1_abc[1]),

  RE1.getTransform(translation, rotation, scaleFactor, scaleOrientation);

    Oz: za.multiply(e1_abc[2])

  },0.0);





  var q = rotation.toAxisAngle();

  // now let's get to covariance matrix

  var q_str = q[0].toString()+" "+q[1];

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

  var RE1xJE1 = RE1.mult(JE1);





  x3dom_draw_ellipsoid("e1b","green",e2_c,q_str,e1_scale);

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

  var RE2xJE2 = RE2.mult(JE2);





  // now let's get to covariance matrix

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

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

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

  var RE1i = RE1.inverse();

  var RE1iJE1 = RE1i.mult(JE1);





  var C = RE1iJE1.mult(RE1iJE1.transpose());

  var C = C1.add(C2);





  Cn = matrix_x3dom_to_numeric(C);

  Cn = matrix_x3dom_to_numeric(C);

  Bn = numeric.eig(Cn);

  Bn = numeric.eig(Cn);





  console.log(Bn);

  console.log(Bn);





  var m = 1/5;

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



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

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

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

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

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



  var ee_a = Math.sqrt(1/2/m*(jyy+jzz-jxx));

  var ee_b = Math.sqrt(1/2/m*(jxx+jzz-jyy));

  var ee_c = Math.sqrt(1/2/m*(jxx+jyy-jzz));





  console.log("e1 restored scale: "+[ee_a,ee_b,ee_c].join(" "));

  console.log("e1 restored abc: "+[ee_a,ee_b,ee_c].join(" "));





  var RE1n = new x3dom.fields.SFMatrix4f(

  var RE1n = new x3dom.fields.SFMatrix4f(

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

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

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

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

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

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

                          0,             0,           0, 1

                          0,             0,           0, 1

             );

             );





  console.log(RE1n.toString());

  console.log(RE1n.toString());





  return 0;

  x3dom_draw_ellipsoid_by_semiaxes_and_center("e1c","red",{



    O:  e2_c,

  //$(".my-markers").find("material").attr("transparency","0.5");

    Ox: RE1n.e0().multiply(ee_a),



    Oy: RE1n.e1().multiply(ee_b),

  //$("#deltalink").remove();

    Oz: RE1n.e2().multiply(ee_c)



  },0.5);

  var coords = [

    "0 0 0",

    p_w.x+" "+p_w.y+" "+p_w.z,

    marker.x+" "+marker.y+" "+marker.z,

    "0 0 0",

  ].join(" ");



  var html = [

    '<group id="deltalink" class="deltalink">',

    '  <switch whichChoice="0">',

    '    <transform>',

    '      <shape>',

    '        <appearance>',

    '          <material emissiveColor="white" transparency="0.0"></material>',

    '        </appearance>',

    '        <lineset vertexCount="4" solid="true" ccw="true" lit="true">',

    '          <coordinate point="'+coords+'"></coordinate>',

    '        </lineset>',

    '      </shape>',

    '    </transform>',

    '  </switch>',

    '</group>'

  ].join('\n');





  var sphere_element = $(html);

  return 0;

  $('scene',Scene.element).append(sphere_element);





  //var d = x3dom_3d_distance(p_w.x,p_w.y,p_w.z,false);

}

  var d = x3dom_3d_distance(marker.x,marker.y,marker.z,false);

  //console.log("Distance to pointe is "+d);





  // z-axis

/*

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

 e is an object

  mark_dir = mark_dir.normalize();

  {

    O:  x3dom.fields.SFVec3f()

    Ox: x3dom.fields.SFVec3f()

    Oy: x3dom.fields.SFVec3f()

    Oz: x3dom.fields.SFVec3f()

  }

*/

function x3dom_draw_ellipsoid_by_semiaxes_and_center(id,color,e,transparency=0.5){





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

  var transl = e.O.toString();

  var scale = [e.Ox.length(),e.Oy.length(),e.Oz.length()].join(" ");





  var mark_x = mark_dir.cross(world_y);

  var xa = e.Ox.normalize();

  var mark_y = mark_x.cross(mark_dir);

  var ya = e.Oy.normalize();

  var mark_z = mark_dir.negate();

  var za = e.Oz.normalize();





  var M = x3dom.fields.SFMatrix4f.lookAt(new x3dom.fields.SFVec3f(0,0,0), mark_dir, mark_y);

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

  R.setValue(xa,ya,za);





  var translation = new x3dom.fields.SFVec3f(0,0,0);

  var translation = new x3dom.fields.SFVec3f(0,0,0);

  var scaleFactor = new x3dom.fields.SFVec3f(1,1,1);

  var scaleFactor = new x3dom.fields.SFVec3f(1,1,1);

  var rotation = new x3dom.fields.Quaternion(0,0,1,0);

  var rotation = new x3dom.fields.Quaternion(0,0,1,0);

  var scaleOrientation = new x3dom.fields.Quaternion(0,0,1,0);

  var scaleOrientation = new x3dom.fields.Quaternion(0,0,1,0);

  M.getTransform(translation, rotation, scaleFactor, scaleOrientation);



  console.log("Rotation matrix");

  console.log(M.toString());



  // draw vectors?!



  var aa = rotation.toAxisAngle();

  var aa_string = aa[0].toString()+" "+aa[1];



  //

  // now draw the 1st ellipse

  //

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

  var k = 20;

  var e1_abc = [k*0.00038*d,k*0.00038*d,2*k*d*d/10000];

  var model_scale = e1_abc.join(",");



  // at marker

  //x3dom_draw_ellipsoid("e2a","gold",mark_dir.toString(),aa_string,model_scale);

  // overlayed

  //x3dom_draw_ellipsoid("e1b","green",transl,aa_string,model_scale);



  // now Covariance



  // E1 (model, tilted):

  var J1 = x3dom_ellipsoid_inertia_tensor(1,e1_abc[0],e1_abc[1],e1_abc[2]);

  var R1 = M;

  var R1i = R1.inverse();

  var R1iJ1 = R1i.mult(J1);



  // E2 (map, vertical):

  var J2 = x3dom_ellipsoid_inertia_tensor(1,e2_abc[0],e2_abc[1],e2_abc[2]);

  var R2 = new x3dom.fields.SFMatrix4f(

              1, 0, 0, 0,

              0, 1, 0, 0,

              0, 0, 1, 0,

              0, 0, 0, 1

           );

  var R2i = R2.inverse();

  var R2iJ2 = R2i.mult(J2);



  var   C = R1iJ1.mult(R1iJ1.transpose());

  //var   C = R2iJ2.mult(R2iJ2.transpose());

  //C = C.add(R2iJ2.mult(R2iJ2.transpose()));



  Cnum = matrix_x3dom_to_numeric(C);

  Bnum = numeric.eig(Cnum);



  var point = new x3dom.fields.SFVec3f(p_w.x,p_w.y,p_w.z);



  var RNEW = new x3dom.fields.SFMatrix4f(

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

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

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

                           0,              0,              0, 1

           );



  console.log("R new");

  console.log(Rnew.toString());



  var xnew = new x3dom.fields.SFVec3f(Bnum.E.x[0][0],Bnum.E.x[1][0],Bnum.E.x[2][0]);

  var ynew = new x3dom.fields.SFVec3f(Bnum.E.x[0][1],Bnum.E.x[1][1],Bnum.E.x[2][1]);

  var znew = new x3dom.fields.SFVec3f(Bnum.E.x[0][2],Bnum.E.x[1][2],Bnum.E.x[2][2]);



  var m = 1/5;



  var jxx = Math.sqrt(Bnum.lambda.x[0]);

  var jyy = Math.sqrt(Bnum.lambda.x[1]);

  var jzz = Math.sqrt(Bnum.lambda.x[2]);





  var ee_a = Math.sqrt(1/2/m*(jyy+jzz-jxx));

  R.getTransform(translation, rotation, scaleFactor, scaleOrientation);

  var ee_b = Math.sqrt(1/2/m*(jxx+jzz-jyy));

  var ee_c = Math.sqrt(1/2/m*(jxx+jyy-jzz));





  //x3dom_draw_line("yaxis","green",point, point.add(ynew.multiply(ee_b)));

  var q = rotation.toAxisAngle();



  var q_str = q[0].toString()+" "+q[1];

  x3dom_draw_line("xaxis","red",  point, point.add(xnew.multiply(ee_a)));

  x3dom_draw_line("yaxis","green",point, point.add(ynew.multiply(ee_b)));

  x3dom_draw_line("zaxis","blue", point, point.add(znew.multiply(ee_c)));



  console.log("Covariance matrix");

  console.log(C.toString());



  // now what if I get transform from this?

  var translation = new x3dom.fields.SFVec3f(0,0,0);

  var scaleFactor = new x3dom.fields.SFVec3f(1,1,1);

  var rotation = new x3dom.fields.Quaternion(0,0,1,0);

  var scaleOrientation = new x3dom.fields.Quaternion(0,0,1,0);

  C.getTransform(translation, rotation, scaleFactor, scaleOrientation);





  console.log("Covariance decomposition");

  // draw semi-axes

  console.log(translation.toString());

  x3dom_draw_line("xa","red",   e.O, e.O.add(e.Ox));

  console.log(rotation.toString());

  x3dom_draw_line("ya","green", e.O, e.O.add(e.Oy));

  console.log(scaleFactor.toString());

  x3dom_draw_line("za","blue",  e.O, e.O.add(e.Oz));

  console.log(scaleOrientation.toString());





  x3dom_draw_ellipsoid(id,color,transl,q_str,scale,transparency);





}

}





function x3dom_draw_ellipsoid(id,color,transl,rotat,scale){

function x3dom_draw_ellipsoid(id,color,transl,rotat,scale,transparency=0.5){





  $("#"+id).remove();

  $("#"+id).remove();
@@ -296,7 +201,7 @@ function x3dom_draw_ellipsoid(id,color,transl,rotat,scale){ 
    '        <transform scale="'+scale+'" >',

    '        <transform scale="'+scale+'" >',

    '          <shape>',

    '          <shape>',

    '            <appearance>',

    '            <appearance>',

    '               <material diffuseColor="'+color+'" transparency="0.5"></material>',

    '               <material diffuseColor="'+color+'" transparency="'+transparency+'"></material>',

    '            </appearance>',

    '            </appearance>',

    '            <sphere radius="1"></sphere>',

    '            <sphere radius="1"></sphere>',

    '          </shape>',

    '          </shape>',
@@ -370,6 +275,17 @@ function x3dom_ellipsoid_inertia_tensor(m,a,b,c){ 




}

}





function x3dom_ellipsoid_inertia_tensor_v2(m,a,b,c){



    return new x3dom.fields.SFMatrix4f(

          a, 0, 0, 0,

          0, b, 0, 0,

          0, 0, c, 0,

          0, 0, 0, 1

    );



}



// also from 4x4 to 3x3

// also from 4x4 to 3x3

function matrix_x3dom_to_numeric(m){

function matrix_x3dom_to_numeric(m){