split into more base blocks

js/x3dom_deltas.js

@@ -12,20 +12,21 @@ function x3dom_delta_markers(){
 
   var Camera = Map.marker;
 
-  // calling this function
-  Av_f({
+  var marker_i = {
       map: { lat: marker.latitude, lng: marker.longitude, alt: marker.altitude},
       model: {x: marker.x, y: marker.y, z: marker.z}
-    },
-    {
+    };
+
+  var parameters = {
       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 Error_vector = Av_f(marker_i,parameters);
 
   /*
   var e1_c = new x3dom.fields.SFVec3f(marker.x,marker.y,marker.z);
@@ -44,55 +45,26 @@ function x3dom_delta_markers(){
 
 function Av_f(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);
+  var v = v_f(mark,pars);
+  var A = A_f(mark,pars);
+  var Av = A.multMatrixVec(v);
 
-  // get coordinates of marker in real world coorinates
-  // the world is
-  var pi_ll = new L.LatLng(p0_ll.lat, p1_ll.lng);
+  return Av;
 
-  // 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;
+function v_f(mark,pars){
 
-  // dx,dy,dz - coorinates map marker in 'world' coordinates
-  // with the center in p0
-  //
-  // which is ellipse 2 e2.
+  var e1_world = get_e1_w(mark,pars);
+  var e2_world = get_e2_w(mark,pars);
+  // pointing from model mark to map mark
+  var v = e1_world.subtract(e2_world);
 
-  var e2_world = new x3dom.fields.SFVec3f(dx,dy,dz);
+  return v;
+}
 
-  console.log("MARK-on-MAP in WORLD:   "+e2_world.toString());
+// model to world (for debugging mainly)
+function get_e1_M2W(mark,pars){
 
   var heading = pars.heading*Math.PI/180;
   var tilt    = (pars.tilt-90)*Math.PI/180;
@@ -109,41 +81,70 @@ function Av_f(mark,pars){
   var T = x3dom_toYawPitchRoll();
 
   var M2W = R.mult(T);
-  var W2M = R.mult(T).inverse();
+
+  return M2W;
+}
+
+// get ellipse 1 in world coordinates
+function get_e1_w(mark,pars){
+
+  var M2W = get_e1_M2W(mark,pars);
+  var W2M = M2W.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);
+  return e1_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);
+// get ellipse 2 in world coordinates
+function get_e2_w(mark,pars){
 
-  // 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))));
+  var p0_ll = new L.LatLng(pars.lat, pars.lng);
+  var p1_ll = new L.LatLng(mark.map.lat, mark.map.lng);
 
-  console.log("MARK-on-MODEL in WORLD: "+e1_world.toString());
+  // get coordinates of marker in real world coorinates
+  // the world is
+  var pi_ll = new L.LatLng(p0_ll.lat, p1_ll.lng);
 
-  // pointing from model mark to map mark
-  var v = e1_world.subtract(e2_world);
+  // 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);
 
-  // Need to find A
+  // sign
+  if (p1_ll.lng<p0_ll.lng) dy = -dy;
+  if (p1_ll.lat<p0_ll.lat) dx = -dx;
 
-  // 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];
+  // dx,dy,dz - coorinates map marker in 'world' coordinates
+  // with the center in p0
+  //
+  // which is ellipse 2 e2.
 
-  var e1_dir = e1_world.normalize();
+  var e2_world = new x3dom.fields.SFVec3f(dx,dy,dz);
 
-  // -z look upwards in world coordinates
-  //var xa1 = e1_dir.cross(new x3dom.fields.SFVec3f(0,0,-1));
+  return e2_world;
+
+}
+
+function A_f(mark,pars){
+
+  var e1_world = get_e1_w(mark,pars);
+  var e2_world = get_e2_w(mark,pars);
+  var M2W = get_e1_M2W(mark,pars);
+  var W2M = M2W.inverse();
+
+  // e1 depends on distance
+  var e1_d = x3dom_3d_distance(e1_world.x,e1_world.y,e1_world.z,false);
+  var e1_k = 20;
+  //var e1_k = 1;
+  // less precise
+  //var e1_abc = [e1_k*0.00038*e1_d, e1_k*0.00038*e1_d, e1_k*e1_d*e1_d/10000];
+  // more precise
+  var e1_abc = [e1_k*0.00038*e1_d, e1_k*0.00038*e1_d, e1_k*e1_d*e1_d/25000];
+
+  // build a right basis
+  var e1_dir = e1_world.normalize();
   var xa1 = e1_dir.cross(new x3dom.fields.SFVec3f(0,1,0));
   var ya1 = xa1.cross(e1_dir);
   var za1 = e1_dir.negate();
@@ -157,11 +158,11 @@ function Av_f(mark,pars){
   },transparency=0.5);
   */
 
-
+  // e1 ref system
   var RE1 = x3dom.fields.SFMatrix4f.identity();
   RE1.setValue(xa1,ya1,za1);
-  // and so RE1 is in the world coordinates
 
+  // e2 depens on the map
   var e2_k = 1;
   var e2_abc = [e2_k*1,e2_k*1,e2_k*30];
 
@@ -178,9 +179,11 @@ function Av_f(mark,pars){
   },transparency=0.5);
   */
 
+  // e2 ref system
   // 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);
@@ -211,8 +214,6 @@ function Av_f(mark,pars){
                vec0.z, vec1.z, 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,
@@ -220,28 +221,26 @@ function Av_f(mark,pars){
                   0,    0,    0, 1
              );
 
-  var A = RESaxes.inverse().mult(RESmatrix.transpose());
-
+  /*
   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.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
+  var A = RESaxes.inverse().mult(RESmatrix.transpose());
+  return A;
 
-  // return vector
-  return Av;
 }
 
 function Av_df_dx(){
+
   console.log("Welcome to dAv/dx");
+
+  //dE =
+
 };
 
 /*