Commit 64db9f96 authored by Oleg Dzhimiev's avatar Oleg Dzhimiev
Browse files

got rid of INIT_HEADING, it's all matrix operations now

parent 43da7d62

js/leaflet_init.js

+1 −1
Original line number Diff line number Diff line
@@ -80,7 +80,7 @@ LeafletObject.prototype.initialize = function(){ 
    'https://{s}.google.com/vt/lyrs=s&x={x}&y={y}&z={z}',

    {

      maxZoom: this._settings.maxzoom,

      attribution: 'Thanks to comrade Google for our happy childhood',

      attribution: 'Google imagery',

      subdomains:['mt0','mt1','mt2','mt3'],

    }

  );

js/ui_init.js

+49 −73
Original line number Diff line number Diff line
@@ -43,9 +43,6 @@ var Data = { 
var Scene;

var Map;



// this var needs to go

var INIT_HEADING = 0;



var SETTINGS = {

    'pointer':   false,

    'highlight': false,
@@ -158,8 +155,6 @@ function light_init(){ 
                fov: fov || 0,

            });

            

            INIT_HEADING = heading;

            

            var element = document.getElementById('x3d_id');

            

            Scene = new X3DOMObject(element,Data,{});
@@ -268,38 +263,31 @@ function x3d_initial_camera_placement(){ 
    

    // Altitude is relative. Do not care.



    // Roll

    var x = new x3dom.fields.SFVec3f(1,0,0);

    var qr = x3dom.fields.Quaternion.axisAngle(x,roll);

    var Mr = qr.toMatrix();



    // Tilt

    var y = new x3dom.fields.SFVec3f(0,1,0);

    var qt = x3dom.fields.Quaternion.axisAngle(y,tilt);

    var Mt = qt.toMatrix();



    // Heading

    var z = new x3dom.fields.SFVec3f(0,0,1);

    var qh = x3dom.fields.Quaternion.axisAngle(z,heading);

    var Mh = qh.toMatrix();



    var R = Mh.mult(Mt).mult(Mr);

    

    var T = x3dom_C2E();

    // Heading,Tilt,Roll

    var Mh = x3dom.fields.SFMatrix4f.rotationZ(heading);

    var Mt = x3dom.fields.SFMatrix4f.rotationY(tilt);

    var Mr = x3dom.fields.SFMatrix4f.rotationX(roll);

    

    // rw = real world with North

    // w = virtual world = x3dom frame reference

    var Rw_rw = T.inverse().mult(R).mult(T);

    

    var M_rw2w = Rw_rw.inverse();

    // proper Euler rotation

    var R = Mh.mult(Mt).mult(Mr);

    // convert to proper Euler

    var T = x3dom_toYawPitchRoll();

    

    var R0 = T.inverse().mult(R).mult(T);

    

    // _rw - real world

    //  _w - virt world

    var Rc_rw = T.inverse().mult(Mh).mult(T);

    

    var Rc_w = M_rw2w.mult(Rc_rw);

    //var Rm_w = M_rw2w.mult(Rm_rw);

    // exclude roll?

    //var RC0_rw = T.inverse().mult(Mh).mult(Mt).mult(T);



    // exclude tilt and roll

    var RC0_rw = T.inverse().mult(Mh).mult(T);

    

    var RC_w = R0.inverse().mult(RC0_rw);

    // store matrices

    Data.camera.Matrices = {

        Ah: heading,
@@ -308,25 +296,23 @@ function x3d_initial_camera_placement(){ 
        R_h_eul: Mh,

        R_t_eul: Mt,

        R_r_eul: Mr,

        Rw_rw : Rw_rw,

        M_rw2w : M_rw2w,

        V_trueUp_w: Rc_w.e1(),

        Rc_w: Rc_w,

        //Rm_w: Rm_w

        R0 : R0,

        Up0: RC_w.e1(),

        RC_w: RC_w,

    };



    // set view

    var Q = new x3dom.fields.Quaternion(0, 0, 1, 0);

    Q.setValue(Rc_w);

    Q.setValue(RC_w);

    var AA = Q.toAxisAngle();

    

    var viewpoint = $(Scene.element).find("Viewpoint");

    viewpoint.attr("orientation",AA[0].toString()+" "+AA[1]);

    viewpoint.attr("position",Rc_w.e3().toString());

    viewpoint.attr("centerOfRotation",Rc_w.e3().toString());

    viewpoint.attr("position",RC_w.e3().toString());

    viewpoint.attr("centerOfRotation",RC_w.e3().toString());

    

    // update every time

    Data.camera.Matrices.Rc_w = Rc_w;

    Data.camera.Matrices.RC_w = RC_w;

    

}
@@ -589,27 +575,18 @@ function leaf_mousemove_nohc(e){ 
    

    var Camera = Map.marker;



    //old

    var p0 = new L.LatLng(Data.camera.latitude,Data.camera.longitude);

    var p1 = new L.LatLng(Camera._latlng.lat,Camera._latlng.lng);

        

    //update Data

    Data.camera.latitude = Camera._latlng.lat;

    Data.camera.longitude = Camera._latlng.lng;

    

    var dh = Camera._heading - Math.PI/180*Data.camera.heading;

    

    //console.log(Camera._heading);

    

    Data.camera.heading   = Camera._heading*180/Math.PI;

    

    var newheading = Data.camera.heading - INIT_HEADING;

    Data.camera.latitude  = Camera._latlng.lat;

    Data.camera.longitude = Camera._latlng.lng;

    

    if ((p0.lat!=p1.lat)||(p0.lng!=p1.lng)){

        //console.log("translation");

        leaf_translation_v1(p0,p1);

    }else{

        //leaf_rotation_v1(newheading,dh);

        x3dom_rotation(dh);        

    }
@@ -669,42 +646,41 @@ function x3d_mouseMove(){ 
    

    var Camera = Map.marker;

    

    //var initial_heading = Data.camera.heading;

    //var initial_heading = INIT_HEADING;

    var mat = Scene.element.runtime.viewMatrix().inverse();

    var R0 = Data.camera.Matrices.R0;

    var T = x3dom_toYawPitchRoll();

    

    var heading = x3dom_getViewDirection(Scene.element);

    var m_rw = T.mult(R0).mult(mat).mult(T.inverse());

    var ypr = x3dom_YawPitchRoll_degs(m_rw);

    

    Map.marker.setHeading(heading+INIT_HEADING);

    var heading = ypr.yaw;

    

    var d = x3dom_getViewTranslation(Scene.element);

    Map.marker.setHeading(heading);

    

    var dx;

    var dz;

    var dp_w = mat.e3();

    

    if (Scene.old_view_translation == null){

        dx = 0;

        dz = 0;

    }else{

        dx = d.x - Scene.old_view_translation.x;

        dz = d.z - Scene.old_view_translation.z;

    if (Scene.old_view_translation != null){

        dp_w = dp_w.subtract(Scene.old_view_translation);

    }

    

    var distance = Math.sqrt(dx*dx+dz*dz);

    var angle = 180/Math.PI*Math.atan2(dx,-dz);

    // from w to rw

    dp_rw = R0.multMatrixVec(dp_w);

    

    var distance = Math.sqrt(dp_rw.x*dp_rw.x+dp_rw.z*dp_rw.z);

    var angle = 180/Math.PI*Math.atan2(dp_rw.x,-dp_rw.z);

    

    var initial_coordinates = [Data.camera.latitude,Data.camera.longitude];

    

    var p0 = new L.LatLng(initial_coordinates[0],initial_coordinates[1]);//Camera._latlng;

    

    var p1 = p0.CoordinatesOf(angle+INIT_HEADING,distance);

    var p1 = p0.CoordinatesOf(angle,distance);



    Map.marker.setBasePoint(p1);

    Map.marker._syncMeasureMarkersToBasePoint();



    Data.camera.latitude = p1.lat;

    Data.camera.longitude = p1.lng;

    Data.camera.heading = heading+INIT_HEADING;

    Data.camera.heading = heading;

    

    Scene.old_view_translation = d;

    Scene.old_view_translation = mat.e3();

    

}

js/x3dom_functions.js

+138 −78
Original line number Diff line number Diff line
@@ -79,6 +79,8 @@ function x3dom_getXYPosOr(cnvx,cnvy,round){ 


    var shootRay = elem.runtime.shootRay(cnvx,cnvy);

    

    var valid_distance = true;

    

    if (shootRay.pickPosition != null){

        

        var index = Scene.highlighted_marker_index;
@@ -124,11 +126,25 @@ function x3dom_getXYPosOr(cnvx,cnvy,round){ 
        

        dist_xz = null;

        dist_xyz = null;

        

        valid_distance = false;

    }

    

    var R0 = Data.camera.Matrices.R0;

    var p_w = new x3dom.fields.SFVec3f(x,y,z);

    var p_rw = R0.multMatrixVec(p_w);

    

    if (valid_distance){

        dist_xz = Math.sqrt(p_rw.x*p_rw.x+p_rw.z*p_rw.z);

        dist_xyz = Math.sqrt(p_rw.y*p_rw.y+dist_xz*dist_xz);   

    }else{

        dist_xz = null;

        dist_xyz = null;

    }



    az = Math.atan2(x,-z)*180/Math.PI;

    az = (az+INIT_HEADING+360)%360;

    el = Math.atan2(y,Math.sqrt(x*x+z*z))*180/Math.PI;

    az = Math.atan2(p_rw.x,-p_rw.z)*180/Math.PI;

    az = (az+360)%360;

    el = Math.atan2(p_rw.y,Math.sqrt(p_rw.x*p_rw.x+p_rw.z*p_rw.z))*180/Math.PI;

    sk = 0;

    

    var result = {
@@ -166,53 +182,40 @@ function x3dom_getCameraPosOr(round){ 
    

    var elem = Scene.element;

    

    var vm = elem.runtime.viewMatrix().inverse();

    var m = elem.runtime.viewMatrix().inverse();

    

    var tr = vm.e3();

    var tr = m.e3();

    

    var x = tr.x;

    var y = tr.y;

    var z = tr.z;

    var R0 = Data.camera.Matrices.R0;

    var T0 = x3dom_toYawPitchRoll();

    

    var R = vm;

    m = T0.mult(R0).mult(m).mult(T0.inverse());

    

    // double check below, random correct result?

    var ypr = x3dom_YawPitchRoll(m);



    //var mat = Data.camera.Matrices.Rw_rw;

    var mat = Data.camera.Matrices.M_rw2w;

    var T = x3dom_C2E();

    ypr.yaw = (180/Math.PI*ypr.yaw+360)%360;

    ypr.pitch *= 180/Math.PI;

    ypr.roll *= 180/Math.PI;



    mat = T.mult(mat).mult(vm).mult(T.inverse());

    

    var test = x3dom_YawPitchRoll(mat);



    test.yaw = (-180/Math.PI*test.yaw+360)%360;

    test.pitch *= 180/Math.PI;

    test.roll *= 180/Math.PI;

    

    //console.log(test);



    az = test.yaw;

    el = test.pitch;

    sk = test.roll;

    //x3dom_matrix_test();

    

    if (!round){

        return {

            x: x,

            y: y,

            z: z,

            a: az,

            e: el,

            s: sk

            x: tr.x,

            y: tr.y,

            z: tr.z,

            a: ypr.yaw,

            e: ypr.pitch,

            s: ypr.roll

        };

    }else{

        return {

            x: x.toFixed(2),

            y: y.toFixed(2),

            z: z.toFixed(2),

            a: az.toFixed(1),

            e: el.toFixed(1),

            s: sk.toFixed(1)

            x: tr.x.toFixed(2),

            y: tr.y.toFixed(2),

            z: tr.z.toFixed(2),

            a: ypr.yaw.toFixed(1),

            e: ypr.pitch.toFixed(1),

            s: ypr.roll.toFixed(1)

        };        

    }
@@ -228,7 +231,7 @@ function x3dom_setUpRight(){ 
    var at = from.subtract(mat.e2());



    //var up = new x3dom.fields.SFVec3f(0, 1, 0);

    var up = Data.camera.Matrices.V_trueUp_w;

    var up = Data.camera.Matrices.Up0;

    

    var s = mat.e2().cross(up).normalize();
@@ -253,7 +256,7 @@ function x3dom_setUpRight(){ 
    viewpoint.attr("centerOfRotation",mat.e3().toString());

    viewpoint.attr("orientation",AA[0].toString()+" "+AA[1]);

    

    Data.camera.Matrices.Rc_w = mat;

    Data.camera.Matrices.RC_w = mat;



}
@@ -300,7 +303,7 @@ function x3dom_rotation(delta_a){ 
    viewpoint.attr("position",from.toString());

    viewpoint.attr("centerOfRotation",from.toString());

    

    Data.camera.Matrices.Rc_w = newmat;

    Data.camera.Matrices.RC_w = newmat;



}
@@ -337,31 +340,42 @@ function x3dom_translation(dx,dy,dz){ 
    viewpoint.attr("position",newfrom.toString());

    viewpoint.attr("centerOfRotation",newfrom.toString());

    

    Data.camera.Matrices.Rc_w = newmat;

    Data.camera.Matrices.RC_w = newmat;

    

}



function x3dom_altelev(alt,elev){



    console.log(elev);

    

    //x3dom_matrix_test();



    var mat = Scene.element.runtime.viewMatrix().inverse();

    var R0 = Data.camera.Matrices.R0;

    var T = x3dom_toYawPitchRoll();



    var m_rw = T.mult(R0).mult(mat).mult(T.inverse());

    var ypr = x3dom_YawPitchRoll(m_rw);

    

    var from = mat.e3();

    from.y = alt;

    

    // convert to W2C*C2W

    var az = Math.atan2(mat._02,mat._22);

    var az = ypr.yaw;

    var el = elev;

    var sk = Math.atan2(mat._10,mat._11);

    var sk = ypr.roll;



    var Mh = x3dom.fields.SFMatrix4f.rotationZ(az);

    var Mt = x3dom.fields.SFMatrix4f.rotationY(el);

    var Mr = x3dom.fields.SFMatrix4f.rotationX(sk);

    

    var R = Mh.mult(Mt).mult(Mr);

    

    var R_rw = T.inverse().mult(R).mult(T);

    var R_w = R0.inverse().mult(R_rw);

    

    var matx = x3dom.fields.SFMatrix4f.rotationX(el);

    var maty = x3dom.fields.SFMatrix4f.rotationY(az);

    var matz = x3dom.fields.SFMatrix4f.rotationZ(sk);

    var matt  = x3dom.fields.SFMatrix4f.translation(from);



    var newmat = matt.mult(maty).mult(matx).mult(matz);

    var newmat = matt.mult(R_w);



    var Q = new x3dom.fields.Quaternion(0,0,1,0);

    Q.setValue(newmat);
@@ -372,7 +386,7 @@ function x3dom_altelev(alt,elev){ 
    viewpoint.attr("centerOfRotation",newmat.e3().toString());

    viewpoint.attr("orientation",AA[0].toString()+" "+AA[1]);

    

    Data.camera.Matrices.Rc_w = newmat;

    Data.camera.Matrices.RC_w = newmat;

}



/**
@@ -380,31 +394,54 @@ function x3dom_altelev(alt,elev){ 
 */

function x3dom_matrix_test(){

    

    console.log("begin==================================");

    

    var viewpoint = $(Scene.element).find("Viewpoint");

    

    console.log("Viewpoint DOM element");

    console.log("position: "+viewpoint.attr("position"));

    console.log("orientation: "+viewpoint.attr("orientation"));



    var mat = Scene.element.runtime.viewMatrix().inverse();

    /* 

     * 1. view matrix:

     *      - from world to camera

     *          - cols - world basis in camera coords

     * 2. view matrix inverted:

     *      - from camera to world

     *          - cols - camera basis in world coords

     *      - rotation matrix by def, order is not conv:

     *          - yx'z" vs zy'x"

     */



    var mat = Scene.element.runtime.viewMatrix();

    

    console.log("inversed viewMatrix");

    console.log("1. View Matrix from runtime");

    console.log(mat.toString());

    

    var from = mat.e3();

    var at = from.subtract(mat.e2());

    var up = mat.e1();

    var mat_i = mat.inverse();

    

    console.log("matrix from from-at-up");

    console.log("2. Inverted View Matrix");

    console.log(mat_i.toString());

    

    var newmat = x3dom.fields.SFMatrix4f.lookAt(from, at, up);

    var from = mat_i.e3();

    var at   = from.subtract(mat_i.e2());

    var up   = mat_i.e1();

    

    console.log(newmat.toString());

    console.log("3. From-At-Up");

    

    var mat_fau = x3dom.fields.SFMatrix4f.lookAt(from, at, up);

    

    console.log(mat_fau.toString());

    

    var T = x3dom_toYawPitchRoll();

    

    var mat_eul = T.mult(mat_i).mult(T.inverse());

    var eangles = x3dom_YawPitchRoll_degs(mat_eul);

    

    console.log(eangles);

    

    var R = mat;

    var az = Math.atan2(R._02,R._22)*180/Math.PI;

    //az = (az+INIT_HEADING+360)%360;

    //az = (az+360)%360;

    var el = -Math.asin(R._12)*180/Math.PI;

    var sk = Math.atan2(R._10,R._11)*180/Math.PI;
@@ -425,38 +462,62 @@ function x3dom_matrix_test(){ 
    

    console.log(newmat.toString());

    

    console.log("end==================================");

}



/**

 * Transform to calculate conventional Euler angles for z-y'-x" = z-y-z

 * unrelated: what's x3dom's native getWCtoCCMatrix()? canvas-to-world?

 */

function x3dom_C2E(){

function x3dom_YawPitchRoll(m){

    

    var yaw = Math.atan2(m._10,m._00);

    var pitch = -Math.asin(m._20);

    var roll = Math.atan2(m._21,m._22);

    

    return {

        yaw: yaw,

        pitch: pitch,

        roll: roll

    };

}



function x3dom_toYawPitchRoll(){

    return new x3dom.fields.SFMatrix4f(

        0, 0, 1, 0,

        0, 0,-1, 0,

        1, 0, 0, 0,

        0, 1, 0, 0,

        0,-1, 0, 0,

        0, 0, 0, 1,

    );

}



function x3dom_E2C(){

    return x3dom_C2E().inverse();

}



function x3dom_YawPitchRoll(m){

function x3dom_YawPitchRoll_degs(m){

    

    var yaw = Math.atan2(m._10,m._00);

    var pitch = -Math.asin(m._20);

    var roll = Math.atan2(m._21,m._22);

    

    return {

        yaw: yaw,

        pitch: pitch,

        roll: roll

        yaw: yaw*180/Math.PI,

        pitch: pitch*180/Math.PI,

        roll: roll*180/Math.PI

    };

}



/*

function x3dom_YawPitchRoll_2_degs(m){

    

    var pitch = Math.PI+Math.asin(m._20);

    var roll = Math.atan2(m._21/Math.cos(pitch),m._22/Math.cos(pitch));

    var yaw = Math.atan2(m._10/Math.cos(pitch),m._00/Math.cos(pitch));

    

    return {

        yaw: yaw*180/Math.PI,

        pitch: pitch*180/Math.PI,

        roll: roll*180/Math.PI

    };

}

*/

function x3dom_delta_map2scene(p0,p1){

    

    var pi = new L.LatLng(p0.lat,p1.lng);
@@ -470,9 +531,8 @@ function x3dom_delta_map2scene(p0,p1){ 
    if (p1.lng<p0.lng) dp_rw.x = -dp_rw.x;

    if (p1.lat>p0.lat) dp_rw.z = -dp_rw.z;

    

    // rotate, not transform - because rotated is the camera

    var mat = Data.camera.Matrices.Rw_rw;

    var dp_w = mat.multMatrixVec(dp_rw);

    var M0 = Data.camera.Matrices.R0.inverse();

    var dp_w = M0.multMatrixVec(dp_rw);

    

    return dp_w;

js/x3dom_init.js

+21 −13
Original line number Diff line number Diff line
@@ -862,23 +862,27 @@ X3DOMObject.Marker.drag = function(dx,dy){ 


X3DOMObject.Marker.slide = function(index,x,y,z){

        

    var c = Data.markers[index];

    var R0 = Data.camera.Matrices.R0;

    //var T = x3dom_toYawPitchRoll();

    

    c.x = x;

    c.y = y;

    c.z = z;

    var p_w = new x3dom.fields.SFVec3f(x,y,z);

    var p_rw = R0.multMatrixVec(p_w);

    

    var azimuth = Math.atan2(c.x,-c.z)*180/Math.PI;

    //var initial_heading = Data.camera.heading;

    var distance = Math.sqrt(c.x*c.x+c.z*c.z);

    var distance = Math.sqrt(p_rw.x*p_rw.x+p_rw.z*p_rw.z);

    var angle = Math.atan2(p_rw.x,-p_rw.z)*180/Math.PI;

    

    var p1_ll = Map.marker._latlng;

    var p2_ll = p1_ll.CoordinatesOf(azimuth+INIT_HEADING,distance);

    var p2_ll = p1_ll.CoordinatesOf(angle,distance);

    

    var c = Data.markers[index];

    

    c.x = x;

    c.y = y;

    c.z = z;    

    c.latitude = p2_ll.lat;

    c.longitude = p2_ll.lng;

    c.altitude = c.y;

    

    //d_x3d - map distance calculated from the model

    c.d_x3d = distance;

    

    X3DOMObject.displayMarkInfo(index);
@@ -971,10 +975,14 @@ X3DOMObject.PointerMarker.prototype._registerEvents = function(){ 
        new X3DOMObject.Marker(mark.x,mark.y,mark.z);



        // Create marker on the map

        var azimuth = Math.atan2(mark.x,-mark.z)*180/Math.PI;

        var distance = Math.sqrt(mark.x*mark.x+mark.z*mark.z);

        var R0 = Data.camera.Matrices.R0;

        var p_w = new x3dom.fields.SFVec3f(mark.x,mark.y,mark.z);

        var p_rw = R0.multMatrixVec(p_w);

        

        var distance = Math.sqrt(p_rw.x*p_rw.x+p_rw.z*p_rw.z);

        var angle = Math.atan2(p_rw.x,-p_rw.z)*180/Math.PI;



        Camera.createMeasureMarker(azimuth+INIT_HEADING,distance);

        Camera.createMeasureMarker(angle,distance);

        

        var map_mark = Camera._measureMarkers[Camera._measureMarkers.length-1];