Commit 7647bcd0 authored by Oleg Dzhimiev's avatar Oleg Dzhimiev

split into more base blocks

parent a82eda19
...@@ -12,20 +12,21 @@ function x3dom_delta_markers(){ ...@@ -12,20 +12,21 @@ function x3dom_delta_markers(){
var Camera = Map.marker; var Camera = Map.marker;
// calling this function var marker_i = {
Av_f({
map: { lat: marker.latitude, lng: marker.longitude, alt: marker.altitude}, map: { lat: marker.latitude, lng: marker.longitude, alt: marker.altitude},
model: {x: marker.x, y: marker.y, z: marker.z} model: {x: marker.x, y: marker.y, z: marker.z}
}, };
{
var parameters = {
lat: Data.camera.kml.latitude, lat: Data.camera.kml.latitude,
lng: Data.camera.kml.longitude, lng: Data.camera.kml.longitude,
alt: Data.camera.kml.altitude, alt: Data.camera.kml.altitude,
heading: Data.camera.kml.heading, heading: Data.camera.kml.heading,
tilt: Data.camera.kml.tilt, tilt: Data.camera.kml.tilt,
roll: Data.camera.kml.roll 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); var e1_c = new x3dom.fields.SFVec3f(marker.x,marker.y,marker.z);
...@@ -44,55 +45,26 @@ function x3dom_delta_markers(){ ...@@ -44,55 +45,26 @@ function x3dom_delta_markers(){
function Av_f(mark,pars){ function Av_f(mark,pars){
/* var v = v_f(mark,pars);
var A = A_f(mark,pars);
mark = { var Av = A.multMatrixVec(v);
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 return Av;
// 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 function v_f(mark,pars){
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 var e1_world = get_e1_w(mark,pars);
// with the center in p0 var e2_world = get_e2_w(mark,pars);
// // pointing from model mark to map mark
// which is ellipse 2 e2. 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 heading = pars.heading*Math.PI/180;
var tilt = (pars.tilt-90)*Math.PI/180; var tilt = (pars.tilt-90)*Math.PI/180;
...@@ -109,41 +81,70 @@ function Av_f(mark,pars){ ...@@ -109,41 +81,70 @@ function Av_f(mark,pars){
var T = x3dom_toYawPitchRoll(); var T = x3dom_toYawPitchRoll();
var M2W = R.mult(T); 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_model = new x3dom.fields.SFVec3f(mark.model.x,mark.model.y,mark.model.z);
var e1_world = M2W.multMatrixVec(e1_model); var e1_world = M2W.multMatrixVec(e1_model);
var e2_model = W2M.multMatrixVec(e2_world); return e1_world;
}
// get ellipse 2 in world coordinates
function get_e2_w(mark,pars){
//x3dom_draw_line("soup0","white", new x3dom.fields.SFVec3f(0,0,0), e1_model); var p0_ll = new L.LatLng(pars.lat, pars.lng);
//x3dom_draw_line("soup1","white", new x3dom.fields.SFVec3f(0,0,0), e2_model); var p1_ll = new L.LatLng(mark.map.lat, mark.map.lng);
//x3dom_draw_line("soup2","white", e1_model, e2_model);
// world axes are correct - verified // get coordinates of marker in real world coorinates
//x3dom_draw_line("soup0","red", e1_model, e1_model.add(W2M.multMatrixVec(new x3dom.fields.SFVec3f(10,0,0)))); // the world is
//x3dom_draw_line("soup1","green", e1_model, e1_model.add(W2M.multMatrixVec(new x3dom.fields.SFVec3f(0,10,0)))); var pi_ll = new L.LatLng(p0_ll.lat, p1_ll.lng);
//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()); // 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);
// pointing from model mark to map mark // sign
var v = e1_world.subtract(e2_world); if (p1_ll.lng<p0_ll.lng) dy = -dy;
if (p1_ll.lat<p0_ll.lat) dx = -dx;
// Need to find A // dx,dy,dz - coorinates map marker in 'world' coordinates
// with the center in p0
//
// which is ellipse 2 e2.
// point in Model var e2_world = new x3dom.fields.SFVec3f(dx,dy,dz);
// 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(); return e2_world;
// -z look upwards in world coordinates }
//var xa1 = e1_dir.cross(new x3dom.fields.SFVec3f(0,0,-1));
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 xa1 = e1_dir.cross(new x3dom.fields.SFVec3f(0,1,0));
var ya1 = xa1.cross(e1_dir); var ya1 = xa1.cross(e1_dir);
var za1 = e1_dir.negate(); var za1 = e1_dir.negate();
...@@ -157,11 +158,11 @@ function Av_f(mark,pars){ ...@@ -157,11 +158,11 @@ function Av_f(mark,pars){
},transparency=0.5); },transparency=0.5);
*/ */
// e1 ref system
var RE1 = x3dom.fields.SFMatrix4f.identity(); var RE1 = x3dom.fields.SFMatrix4f.identity();
RE1.setValue(xa1,ya1,za1); RE1.setValue(xa1,ya1,za1);
// and so RE1 is in the world coordinates
// e2 depens on the map
var e2_k = 1; var e2_k = 1;
var e2_abc = [e2_k*1,e2_k*1,e2_k*30]; var e2_abc = [e2_k*1,e2_k*1,e2_k*30];
...@@ -178,9 +179,11 @@ function Av_f(mark,pars){ ...@@ -178,9 +179,11 @@ function Av_f(mark,pars){
},transparency=0.5); },transparency=0.5);
*/ */
// e2 ref system
// it's vertical in the WORLD // it's vertical in the WORLD
var RE2 = x3dom.fields.SFMatrix4f.identity(); var RE2 = x3dom.fields.SFMatrix4f.identity();
// now let's get to covariance matrix // 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 JE1 = x3dom_ellipsoid_inertia_tensor_v2(1,e1_abc[0],e1_abc[1],e1_abc[2]);
var RE1xJE1 = RE1.mult(JE1); var RE1xJE1 = RE1.mult(JE1);
...@@ -211,8 +214,6 @@ function Av_f(mark,pars){ ...@@ -211,8 +214,6 @@ function Av_f(mark,pars){
vec0.z, vec1.z, vec2.z, 0, vec0.z, vec1.z, vec2.z, 0,
0, 0, 0, 1 0, 0, 0, 1
); );
//RESmatrix = RESmatrix.transpose();
var RESaxes = new x3dom.fields.SFMatrix4f( var RESaxes = new x3dom.fields.SFMatrix4f(
es_a, 0, 0, 0, es_a, 0, 0, 0,
0, es_b, 0, 0, 0, es_b, 0, 0,
...@@ -220,28 +221,26 @@ function Av_f(mark,pars){ ...@@ -220,28 +221,26 @@ function Av_f(mark,pars){
0, 0, 0, 1 0, 0, 0, 1
); );
var A = RESaxes.inverse().mult(RESmatrix.transpose()); /*
x3dom_draw_ellipsoid_by_semiaxes_and_center("ec1","red",{ x3dom_draw_ellipsoid_by_semiaxes_and_center("ec1","red",{
O: W2M.multMatrixVec(e2_world), O: W2M.multMatrixVec(e2_world),
Ox: W2M.multMatrixVec(RESmatrix.transpose().e0().multiply(es_a)), Ox: W2M.multMatrixVec(RESmatrix.transpose().e0().multiply(es_a)),
Oy: W2M.multMatrixVec(RESmatrix.transpose().e1().multiply(es_b)), Oy: W2M.multMatrixVec(RESmatrix.transpose().e1().multiply(es_b)),
Oz: W2M.multMatrixVec(RESmatrix.transpose().e2().multiply(es_c)) Oz: W2M.multMatrixVec(RESmatrix.transpose().e2().multiply(es_c))
},transparency=0.8); },transparency=0.8);
*/
// and so, the error vector is E=Av var A = RESaxes.inverse().mult(RESmatrix.transpose());
var Av = A.multMatrixVec(v); return A;
// 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
// return vector
return Av;
} }
function Av_df_dx(){ function Av_df_dx(){
console.log("Welcome to dAv/dx"); console.log("Welcome to dAv/dx");
//dE =
}; };
/* /*
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment