Commit 80551632 authored by Oleg Dzhimiev's avatar Oleg Dzhimiev

testing new function

parent 7af5d8cb
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; var theLastMovedMarker = null;
function x3dom_delta_markers(){ function x3dom_delta_markers(){
...@@ -8,19 +191,48 @@ function x3dom_delta_markers(){ ...@@ -8,19 +191,48 @@ function x3dom_delta_markers(){
var index = theLastMovedMarker; var index = theLastMovedMarker;
var marker = Data.markers[index]; var marker = Data.markers[index];
console.log(marker);
var Camera = Map.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_mark_ll = new L.LatLng(marker.latitude, marker.longitude);
//var p_cam_ll = Camera._latlng; //var p_cam_ll = Camera._latlng;
var p_cam_ll = new L.LatLng(Data.camera.kml.latitude,Data.camera.kml.longitude); 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); //p_w = x3dom_scene_to_real(p_w.x,p_w.y,p_w.z);
// vertical ellipse's center // vertical ellipse's center
var e2_c = p_w; var e2_c = p_model;
// depth ellipse's center // depth ellipse's center
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);
...@@ -34,15 +246,23 @@ function x3dom_delta_markers(){ ...@@ -34,15 +246,23 @@ function x3dom_delta_markers(){
//var e2_scale = e2_abc.join(","); //var e2_scale = e2_abc.join(",");
//x3dom_draw_ellipsoid("e2","green",e2_c.toString(),"",e2_scale); //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",{ x3dom_draw_ellipsoid_by_semiaxes_and_center("e2","green",{
O: e2_c, O: e2_c,
Ox: new x3dom.fields.SFVec3f(e2_abc[0], 0, 0), Ox: R0i.multMatrixVec(xa2),
Oy: new x3dom.fields.SFVec3f( 0, e2_abc[1], 0), Oy: R0i.multMatrixVec(ya2),
Oz: new x3dom.fields.SFVec3f( 0, 0, e2_abc[2]) Oz: R0i.multMatrixVec(za2)
},transparency=0.5); },transparency=0.0);
*/ */
var RE2 = x3dom.fields.SFMatrix4f.identity(); var RE2 = x3dom.fields.SFMatrix4f.identity();
//RE2.setValue(xa,ya,za); //RE2.setValue(xa,ya,za);
...@@ -63,13 +283,9 @@ function x3dom_delta_markers(){ ...@@ -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("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)); //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 // 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 // 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_line1","white", e2_c.add(e1_c), e2_c.subtract(e1_c));
...@@ -86,25 +302,26 @@ function x3dom_delta_markers(){ ...@@ -86,25 +302,26 @@ function x3dom_delta_markers(){
// draw ellipsoid axes // draw ellipsoid axes
var e1_dir = e1_c; 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 xa1 = e1_dir.cross(new x3dom.fields.SFVec3f(0,1,0));
var ya = xa.cross(e1_dir); var ya1 = xa1.cross(e1_dir);
var za = e1_dir.negate(); var za1 = e1_dir.negate();
// next construct rotation matrix // next construct rotation matrix
// it's in the WORLD coordinates
var RE1 = x3dom.fields.SFMatrix4f.identity(); 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",{ x3dom_draw_ellipsoid_by_semiaxes_and_center("e1b","green",{
O: e2_c, O: e2_c,
Ox: xa.multiply(e1_abc[0]), Ox: R0i.multMatrixVec(xa1).multiply(e1_abc[0]),
Oy: ya.multiply(e1_abc[1]), Oy: R0i.multMatrixVec(ya1).multiply(e1_abc[1]),
Oz: za.multiply(e1_abc[2]) Oz: R0i.multMatrixVec(za1).multiply(e1_abc[2])
},transparency=0.5); },transparency=0.0);
*/ */
// now let's get to covariance matrix // now let's get to covariance matrix
...@@ -132,21 +349,48 @@ function x3dom_delta_markers(){ ...@@ -132,21 +349,48 @@ function x3dom_delta_markers(){
console.log("e1 restored abc: "+[es_a,es_b,es_c].join(" ")); console.log("e1 restored abc: "+[es_a,es_b,es_c].join(" "));
var RES = new x3dom.fields.SFMatrix4f( var RESmatrix = new x3dom.fields.SFMatrix4f(
Bn.E.x[0][0], Bn.E.x[0][1], Bn.E.x[0][2], 0, Bn.E.x[0][0], Bn.E.x[1][0], Bn.E.x[2][0], 0,
Bn.E.x[1][0], Bn.E.x[1][1], Bn.E.x[1][2], 0, Bn.E.x[0][1], Bn.E.x[1][1], Bn.E.x[2][1], 0,
Bn.E.x[2][0], Bn.E.x[2][1], Bn.E.x[2][2], 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 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(RES.toString());
console.log("RESt");
console.log(RESt.toString());
/*
x3dom_draw_ellipsoid_by_semiaxes_and_center("e1c","red",{ x3dom_draw_ellipsoid_by_semiaxes_and_center("e1c","red",{
O: e2_c, O: e2_c,
Ox: RES.e0().multiply(es_a), Ox: R0i.multMatrixVec(RESmatrix.transpose().e0()).multiply(es_a),
Oy: RES.e1().multiply(es_b), Oy: R0i.multMatrixVec(RESmatrix.transpose().e1()).multiply(es_b),
Oz: RES.e2().multiply(es_c) Oz: R0i.multMatrixVec(RESmatrix.transpose().e2()).multiply(es_c)
},transparency=0.7); },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: // now get error:
...@@ -155,10 +399,9 @@ function x3dom_delta_markers(){ ...@@ -155,10 +399,9 @@ function x3dom_delta_markers(){
// 2. RES and es_a, es_b, es_c // 2. RES and es_a, es_b, es_c
// RES x e1_c // RES x e1_c
var RESi = RES.inverse(); 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("delta vector in ellipsoid:");
console.log(E.toString()); console.log(E.toString());
...@@ -170,15 +413,18 @@ function x3dom_delta_markers(){ ...@@ -170,15 +413,18 @@ function x3dom_delta_markers(){
// scale the result ellipsoid so it with touch another marker // scale the result ellipsoid so it with touch another marker
// and it did - it only tests that the scale is correct // and it did - it only tests that the scale is correct
/*
x3dom_draw_ellipsoid_by_semiaxes_and_center("e1d","gold",{ x3dom_draw_ellipsoid_by_semiaxes_and_center("e1d","gold",{
O: e2_c, O: e2_c,
Ox: RES.e0().multiply(E.length()*es_a), Ox: RES.e0().multiply(E.length()*es_a),
Oy: RES.e1().multiply(E.length()*es_b), Oy: RES.e1().multiply(E.length()*es_b),
Oz: RES.e2().multiply(E.length()*es_c) Oz: RES.e2().multiply(E.length()*es_c)
},transparency=0.7); },transparency=0.7);
*/
// alternative calculations where E^2 = delta^T x C^-1 x delta // 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 = C.inverse();
var Ci_x_delta = Ci.multMatrixVec(delta); var Ci_x_delta = Ci.multMatrixVec(delta);
...@@ -264,6 +510,8 @@ function x3dom_draw_ellipsoid(id,color,transl,rotat,scale,transparency=0.5){ ...@@ -264,6 +510,8 @@ function x3dom_draw_ellipsoid(id,color,transl,rotat,scale,transparency=0.5){
function x3dom_draw_line(id,color,p1,p2){ function x3dom_draw_line(id,color,p1,p2){
$("#"+id).remove();
var coords = [ var coords = [
p1.x+" "+p1.y+" "+p1.z, p1.x+" "+p1.y+" "+p1.z,
p2.x+" "+p2.y+" "+p2.z p2.x+" "+p2.y+" "+p2.z
......
...@@ -1586,6 +1586,8 @@ X3DOMObject.createNewMarker = function(x,y,z){ ...@@ -1586,6 +1586,8 @@ X3DOMObject.createNewMarker = function(x,y,z){
// Update marker in Data // Update marker in Data
mark.latitude = map_mark._latlng.lat; mark.latitude = map_mark._latlng.lat;
mark.longitude = map_mark._latlng.lng; 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 // register events for a new marker in Data
X3DOMObject.MapMarker.registerEvents(map_mark); X3DOMObject.MapMarker.registerEvents(map_mark);
......
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