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

js/leaflet_init.js

@@ -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

@@ -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

@@ -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

@@ -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];