Commit 8f7da472 authored by Oleg Dzhimiev's avatar Oleg Dzhimiev

all moved to a separate function

parent 80551632
......@@ -73,9 +73,14 @@ function Av_func(mark,pars){
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);
//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);
// 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))));
console.log("MARK-on-MODEL in WORLD: "+e1_world.toString());
......@@ -87,8 +92,8 @@ function Av_func(mark,pars){
// 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_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];
......@@ -109,6 +114,7 @@ function Av_func(mark,pars){
},transparency=0.5);
*/
var RE1 = x3dom.fields.SFMatrix4f.identity();
RE1.setValue(xa1,ya1,za1);
// and so RE1 is in the world coordinates
......@@ -142,7 +148,7 @@ function Av_func(mark,pars){
var C1 = RE1xJE1.mult(RE1xJE1.transpose());
var C2 = RE2xJE2.mult(RE2xJE2.transpose());
var C = C1.add(C2);
var C = C1.add(C1);
//var C = C1.add(C1);
//var C = C2.add(C2);
Cn = matrix_x3dom_to_numeric(C);
......@@ -159,7 +165,7 @@ function Av_func(mark,pars){
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,
vec0.z, vec1.z, vec2.z, 0,
0, 0, 0, 1
);
//RESmatrix = RESmatrix.transpose();
......@@ -173,12 +179,21 @@ function Av_func(mark,pars){
var A = RESaxes.inverse().mult(RESmatrix.transpose());
x3dom_draw_ellipsoid_by_semiaxes_and_center("ec1","gold",{
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.7);
},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
//
}
......@@ -223,217 +238,6 @@ function x3dom_delta_markers(){
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_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_model;
// depth ellipse's center
var e1_c = new x3dom.fields.SFVec3f(marker.x,marker.y,marker.z);
// E2: draw vertical ellipsoid
// just a coefficient
//var e2_k = 1/2;
var e2_k = 1;
var e2_abc = [e2_k*1,e2_k*30,e2_k*1];
//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: 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);
// draw ellipsoid axes
//var xa = new x3dom.fields.SFVec3f(1,0,0);
//var ya = new x3dom.fields.SFVec3f(0,1,0);
//var za = new x3dom.fields.SFVec3f(0,0,1);
//x3dom_draw_line("xa","red", e2_c, e2_c.add(xa.multiply(e2_abc[0])));
//x3dom_draw_line("ya","green",e2_c, e2_c.add(ya.multiply(e2_abc[1])));
//x3dom_draw_line("za","blue", e2_c, e2_c.add(za.multiply(e2_abc[2])));
// to 0,0,0
//x3dom_draw_line("test0","white", e2_c, new x3dom.fields.SFVec3f(0,0,0));
//x3dom_draw_line("test1","tomato", new x3dom.fields.SFVec3f(0,0,0), new x3dom.fields.SFVec3f(100,0,0));
//x3dom_draw_line("test2","seagreen", new x3dom.fields.SFVec3f(0,0,0), new x3dom.fields.SFVec3f(0,100,0));
//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));
// e1 direction
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));
x3dom_draw_line("helper_line3","white", e1_c, e2_c);
// E1: draw tilted ellipsoid
var e1_d = x3dom_3d_distance(e1_c.x,e1_c.y,e1_c.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];
console.log("e1 abc: "+e1_abc.join(" "));
// draw ellipsoid axes
var e1_dir = e1_c;
e1_dir = R0.multMatrixVec(e1_dir.normalize());
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(xa1,ya1,za1);
//console.log(RE1.toString());
/*
x3dom_draw_ellipsoid_by_semiaxes_and_center("e1b","green",{
O: e2_c,
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
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);
// testing
//var C = C1.add(C1);
Cn = matrix_x3dom_to_numeric(C);
Bn = numeric.eig(Cn);
//console.log(Bn);
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]);
console.log("e1 restored abc: "+[es_a,es_b,es_c].join(" "));
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: 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:
// we have:
// 1. e1_c
// 2. RES and es_a, es_b, es_c
// RES x e1_c
var RESi = RES.inverse();
var E = RESi.multMatrixVec(v);
console.log("delta vector in ellipsoid:");
console.log(E.toString());
E = E.multComponents(new x3dom.fields.SFVec3f(1/es_a, 1/es_b, 1/es_c));
console.log(E.toString());
console.log("E^2 from eigen: " +E.length()*E.length());
// 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 = v;
var Ci = C.inverse();
var Ci_x_delta = Ci.multMatrixVec(delta);
var E2 = delta.dot(Ci_x_delta);
console.log("E^2 from d^T x C^-1 x d: "+E2);
return 0;
}
/*
......
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