testing new function

js/x3dom_deltas.js

 
+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

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