Commit 80551632 authored by Oleg Dzhimiev's avatar Oleg Dzhimiev
Browse files

testing new function

parent 7af5d8cb

js/x3dom_deltas.js

+281 −33
Original line number Diff line number Diff line 


function Av_func(mark,pars){



  /*



  mark = {

    map:{ lat: 0, lng: 0, alt: 0},

    model:{ x: 0, y: 0, z: 0 }

  }



  pars = {

    lat: 0,

    lng: 0,

    alt: 0,

    heading: 0,

    tilt: 0,

    roll: 0

  }



  */



  /*

   * 'WORLD' COORDINATE SYSTEM

   * X - to North

   * Y - to East

   * Z - to Earth center

   */



  var p0_ll = new L.LatLng(pars.lat, pars.lng);

  var p1_ll = new L.LatLng(mark.map.lat, mark.map.lng);



  // get coordinates of marker in real world coorinates

  // the world is

  var pi_ll = new L.LatLng(p0_ll.lat, p1_ll.lng);



  // will need derivatives from this distanceTo

  var dx = pi_ll.distanceTo(p1_ll);

  var dy = p0_ll.distanceTo(pi_ll);

  var dz = (mark.map.alt-pars.alt)*(-1);



  // sign

  if (p1_ll.lng<p0_ll.lng) dy = -dy;

  if (p1_ll.lat<p0_ll.lat) dx = -dx;



  // dx,dy,dz - coorinates map marker in 'world' coordinates

  // with the center in p0

  //

  // which is ellipse 2 e2.



  var e2_world = new x3dom.fields.SFVec3f(dx,dy,dz);



  console.log("MARK-on-MAP in WORLD:   "+e2_world.toString());



  var heading = pars.heading*Math.PI/180;

  var tilt    = (pars.tilt-90)*Math.PI/180;

  var roll    = pars.roll*Math.PI/180;



  // Heading,Tilt,Roll

  var Mh = x3dom.fields.SFMatrix4f.rotationZ(heading);

  var Mt = x3dom.fields.SFMatrix4f.rotationY(tilt);

  var Mr = x3dom.fields.SFMatrix4f.rotationX(roll);



  // I'll need R'

  var R = Mh.mult(Mt).mult(Mr);

  // T is constant

  var T = x3dom_toYawPitchRoll();



  var M2W = R.mult(T);

  var W2M = R.mult(T).inverse();



  var e1_model = new x3dom.fields.SFVec3f(mark.model.x,mark.model.y,mark.model.z);

  var e1_world = M2W.multMatrixVec(e1_model);



  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);



  console.log("MARK-on-MODEL in WORLD: "+e1_world.toString());



  // pointing from model mark to map mark

  var v = e1_world.subtract(e2_world);



  // Need to find A



  // 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_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];



  var e1_dir = e1_world.normalize();



  // -z look upwards in world coordinates

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

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

  var ya1 = xa1.cross(e1_dir);

  var za1 = e1_dir.negate();



  /*

  x3dom_draw_ellipsoid_by_semiaxes_and_center("e1","green",{

    O:  W2M.multMatrixVec(e2_world),

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

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

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

  },transparency=0.5);

  */



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

  RE1.setValue(xa1,ya1,za1);

  // and so RE1 is in the world coordinates



  var e2_k = 1;

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



  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:  W2M.multMatrixVec(e2_world),

    Ox: W2M.multMatrixVec(xa2),

    Oy: W2M.multMatrixVec(ya2),

    Oz: W2M.multMatrixVec(za2)

  },transparency=0.5);

  */



  // it's vertical in the WORLD

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



  // 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);

  var C = C1.add(C1);

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



  Cn = matrix_x3dom_to_numeric(C);

  Bn = numeric.eig(Cn);



  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]);



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

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

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



  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,

                    0,      0,      0, 1

             );

  //RESmatrix = RESmatrix.transpose();



  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());



  x3dom_draw_ellipsoid_by_semiaxes_and_center("ec1","gold",{

    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);



}



var theLastMovedMarker = null;



function x3dom_delta_markers(){
@@ -8,19 +191,48 @@ function x3dom_delta_markers(){ 
  var index = theLastMovedMarker;



  var marker = Data.markers[index];

  console.log(marker);



  var Camera = Map.marker;



  // calling this function

  Av_func({

      map: { lat: marker.latitude, lng: marker.longitude, alt: marker.altitude},

      model: {x: marker.x, y: marker.y, z: marker.z}

    },

    {

      lat:     Data.camera.kml.latitude,

      lng:     Data.camera.kml.longitude,

      alt:     Data.camera.kml.altitude,

      heading: Data.camera.kml.heading,

      tilt:    Data.camera.kml.tilt,

      roll:    Data.camera.kml.roll

    }

  );



  /*

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

  x3dom_draw_line("x0","red",   e1_c, e1_c.add(new x3dom.fields.SFVec3f(50, 0, 0)));

  x3dom_draw_line("y0","green", e1_c, e1_c.add(new x3dom.fields.SFVec3f( 0,50, 0)));

  x3dom_draw_line("z0","blue",  e1_c, e1_c.add(new x3dom.fields.SFVec3f( 0, 0,50)));



  x3dom_draw_line("x1","red",   new x3dom.fields.SFVec3f(0,0,0), new x3dom.fields.SFVec3f(50, 0, 0));

  x3dom_draw_line("y1","green", new x3dom.fields.SFVec3f(0,0,0), new x3dom.fields.SFVec3f( 0,50, 0));

  x3dom_draw_line("z1","blue",  new x3dom.fields.SFVec3f(0,0,0), new x3dom.fields.SFVec3f( 0, 0,50));

  */



  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_w = x3dom_delta_map2scene(p_cam_ll, p_mark_ll);

  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_w;

  var e2_c = p_model;

  // depth ellipse's center

  var e1_c = new x3dom.fields.SFVec3f(marker.x,marker.y,marker.z);
@@ -34,15 +246,23 @@ function x3dom_delta_markers(){ 
  //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: 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])

  },transparency=0.5);

    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);
@@ -63,13 +283,9 @@ function x3dom_delta_markers(){ 
  //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));





  $("#helper_line0").remove();

  $("#helper_line1").remove();

  $("#helper_line3").remove();



  // e1 direction

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

  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));
@@ -86,25 +302,26 @@ function x3dom_delta_markers(){ 


  // draw ellipsoid axes

  var e1_dir  = e1_c;

  e1_dir = e1_dir.normalize();

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



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

  var ya = xa.cross(e1_dir);

  var za = e1_dir.negate();

  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(xa,ya,za);

  RE1.setValue(xa1,ya1,za1);



  console.log(RE1.toString());

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



  /*

  x3dom_draw_ellipsoid_by_semiaxes_and_center("e1b","green",{

    O:  e2_c,

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

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

    Oz: za.multiply(e1_abc[2])

  },transparency=0.5);

    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
@@ -132,21 +349,48 @@ function x3dom_delta_markers(){ 


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



  var RES = new x3dom.fields.SFMatrix4f(

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

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

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

  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: RES.e0().multiply(es_a),

    Oy: RES.e1().multiply(es_b),

    Oz: RES.e2().multiply(es_c)

  },transparency=0.7);

    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:
@@ -155,10 +399,9 @@ function x3dom_delta_markers(){ 
  // 2. RES and es_a, es_b, es_c



  // RES x e1_c



  var RESi = RES.inverse();



  var E = RESi.multMatrixVec(e1_c.subtract(e2_c));

  var E = RESi.multMatrixVec(v);



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

  console.log(E.toString());
@@ -170,15 +413,18 @@ function x3dom_delta_markers(){ 


  // 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 = e1_c.subtract(e2_c);

  var delta = v;

  var Ci = C.inverse();



  var Ci_x_delta = Ci.multMatrixVec(delta);
@@ -264,6 +510,8 @@ function x3dom_draw_ellipsoid(id,color,transl,rotat,scale,transparency=0.5){ 


function x3dom_draw_line(id,color,p1,p2){



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



  var coords = [

    p1.x+" "+p1.y+" "+p1.z,

    p2.x+" "+p2.y+" "+p2.z

js/x3dom_init.js

+2 −0
Original line number Diff line number Diff line
@@ -1586,6 +1586,8 @@ X3DOMObject.createNewMarker = function(x,y,z){ 
  // Update marker in Data

  mark.latitude = map_mark._latlng.lat;

  mark.longitude = map_mark._latlng.lng;

  // used to be zero, but now it's not zero

  mark.altitude = Data.camera.kml.altitude+xyz_real.y;



  // register events for a new marker in Data

  X3DOMObject.MapMarker.registerEvents(map_mark);