From b23dbe6cfd8f5cfe20e388826ac3467be7ba42c8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jurij=20Podgor=C5=A1ek?= Date: Sat, 5 Mar 2022 12:44:07 +0100 Subject: [PATCH] Move euler conversion to OSC relay --- anim.js | 126 +++++------------------------------------------------- server.js | 45 ++++++++++++++++++- test.js | 16 +++---- 3 files changed, 59 insertions(+), 128 deletions(-) diff --git a/anim.js b/anim.js index a6cdc6f..aff0010 100644 --- a/anim.js +++ b/anim.js @@ -132,9 +132,9 @@ function modulirajParametre() { drotacijaZ += qWWd.z / 10; } if (keysPressed[0]) { - //width *= 1 + dqX; + width *= 1 + dqX; barva_mod += (dqZ / 100); - obj_limit *= 1 + (dqY / 3); + obj_limit *= 1 + (dqY); } } @@ -252,106 +252,8 @@ window.addEventListener('keyup', (e) => { } }) -function eulerFromQuaternion( quaternion, order ) { - // Quaternion to matrix. - const w = quaternion[0], x = quaternion[1], y = quaternion[2], z = quaternion[3]; - const x2 = x + x, y2 = y + y, z2 = z + z; - const xx = x * x2, xy = x * y2, xz = x * z2; - const yy = y * y2, yz = y * z2, zz = z * z2; - const wx = w * x2, wy = w * y2, wz = w * z2; - const matrix = [ - 1 - ( yy + zz ), - xy + wz, - xz - wy, - 0, - xy - wz, - 1 - ( xx + zz ), - yz + wx, - 0, - xz + wy, - yz - wx, - 1 - ( xx + yy ), - 0, - 0, - 0, - 0, - 1 - ]; - // Matrix to euler - function clamp( value, min, max ) { - return Math.max( min, Math.min( max, value ) ); - } - const m11 = matrix[ 0 ], m12 = matrix[ 4 ], m13 = matrix[ 8 ]; - const m21 = matrix[ 1 ], m22 = matrix[ 5 ], m23 = matrix[ 9 ]; - const m31 = matrix[ 2 ], m32 = matrix[ 6 ], m33 = matrix[ 10 ]; - var euler = [ 0, 0, 0 ]; - switch ( order ) { - case "XYZ": - euler[1] = Math.asin( clamp( m13, - 1, 1 ) ); - if ( Math.abs( m13 ) < 0.9999999 ) { - euler[0] = Math.atan2( - m23, m33 ); - euler[2] = Math.atan2( - m12, m11 ); - } else { - euler[0] = Math.atan2( m32, m22 ); - euler[2] = 0; - } - break; - case "YXZ": - euler[0] = Math.asin( - clamp( m23, - 1, 1 ) ); - if ( Math.abs( m23 ) < 0.9999999 ) { - euler[1] = Math.atan2( m13, m33 ); - euler[2] = Math.atan2( m21, m22 ); - } else { - euler[1] = Math.atan2( - m31, m11 ); - euler[2] = 0; - } - break; - case "ZXY": - euler[0] = Math.asin( clamp( m32, - 1, 1 ) ); - if ( Math.abs( m32 ) < 0.9999999 ) { - euler[1] = Math.atan2( - m31, m33 ); - euler[2] = Math.atan2( - m12, m22 ); - } else { - euler[1] = 0; - euler[2] = Math.atan2( m21, m11 ); - } - break; - case "ZYX": - euler[1] = Math.asin( - clamp( m31, - 1, 1 ) ); - if ( Math.abs( m31 ) < 0.9999999 ) { - euler[0] = Math.atan2( m32, m33 ); - euler[2] = Math.atan2( m21, m11 ); - } else { - euler[0] = 0; - euler[2] = Math.atan2( - m12, m22 ); - } - break; - case "YZX": - euler[2] = Math.asin( clamp( m21, - 1, 1 ) ); - if ( Math.abs( m21 ) < 0.9999999 ) { - euler[0] = Math.atan2( - m23, m22 ); - euler[1] = Math.atan2( - m31, m11 ); - } else { - euler[0] = 0; - euler[1] = Math.atan2( m13, m33 ); - } - break; - case "XZY": - euler[2] = Math.asin( - clamp( m12, - 1, 1 ) ); - if ( Math.abs( m12 ) < 0.9999999 ) { - euler[0] = Math.atan2( m32, m22 ); - euler[1] = Math.atan2( m13, m11 ); - } else { - euler[0] = Math.atan2( - m23, m33 ); - euler[1] = 0; - } - break; - } - return euler; -} - var oscCallbacks = { - '/keys/': [ + '/keys': [ function(args) { keysPressed = args.map(getVal); keysPressed[0] |= kbdPressed['a']; @@ -361,26 +263,20 @@ var oscCallbacks = { //console.log(keysPressed, kbdPressed); } ], - '/quaternion/': [ + '/quaternion': [ function (args) { // Popravimo osi (w x y z po defaultu HMM) [qWW.w, qWW.z, qWW.x, qWW.y] = args.map(getVal); - // Dve sta obratno (polozaj senzorja) :) - qWW.x *= -1; - qWW.z *= -1; } ], - '/quaternionDiff/': [ + '/quaternionDiff': [ function (args) { - [qWWd.w, qWWd.z, qWWd.x, qWWd.y] = args.map(getVal); - // Dve sta obratno (polozaj senzorja) :) - qWWd.x *= -1; - qWWd.z *= -1; - - var euler = eulerFromQuaternion([qWWd.w, qWWd.z, qWWd.x, qWWd.y], 'XYZ'); - dqX = euler[0]; - dqY = euler[1]; - dqZ = euler[2]; + [qWWd.w, qWWd.x, qWWd.y, qWWd.z] = args.map(getVal); + } + ], + '/eulerDiff': [ + function (args) { + [dqX, dqY, dqZ] = args.map(getVal); } ], /* Ne uporabljamo vec diff --git a/server.js b/server.js index 6211b7f..678703e 100644 --- a/server.js +++ b/server.js @@ -137,12 +137,53 @@ scudp.on('error', (e) => { }) scudp.open() +function eulerFromQuaternion(quaternion) { + // Quaternion to matrix. + const w = quaternion[0], x = quaternion[1], y = quaternion[2], z = quaternion[3]; + const x2 = x + x, y2 = y + y, z2 = z + z; + const xx = x * x2, xy = x * y2, xz = x * z2; + const yy = y * y2, yz = y * z2, zz = z * z2; + const wx = w * x2, wy = w * y2, wz = w * z2; + const matrix = [ + 1 - ( yy + zz ), xy + wz, xz - wy, 0, + xy - wz, 1 - ( xx + zz ), yz + wx, 0, + xz + wy, yz - wx, 1 - ( xx + yy ), 0, + 0, 0, 0, 1 + ]; + // Matrix to euler + function clamp( value, min, max ) { + return Math.max( min, Math.min( max, value ) ); + } + const m11 = matrix[ 0 ], m12 = matrix[ 4 ], m13 = matrix[ 8 ]; + const m21 = matrix[ 1 ], m22 = matrix[ 5 ], m23 = matrix[ 9 ]; + const m31 = matrix[ 2 ], m32 = matrix[ 6 ], m33 = matrix[ 10 ]; + var euler = [ 0, 0, 0 ]; + euler[1] = Math.asin( clamp( m13, - 1, 1 ) ); + if ( Math.abs( m13 ) < 0.9999999 ) { + euler[0] = Math.atan2( - m23, m33 ); + euler[2] = Math.atan2( - m12, m11 ); + } else { + euler[0] = Math.atan2( m32, m22 ); + euler[2] = 0; + } + return euler; +} const sendAll = (msg, info, oscWS, osclients) => { + + // Convert quaternion diff to euler angle diff + if (msg.address == '/quaternionDiff') { + const euler = eulerFromQuaternion(msg.args, 'XYZ'); + sendAll({ + address: '/eulerDiff', + args: euler + }, info, oscWS, osclients) + } + osclients.forEach( client => { if (client && oscWS != client) { - //console.log("sending", msg, info) + // console.log("sending", msg, info) client.send(msg) } }) @@ -155,7 +196,7 @@ const sendAll = (msg, info, oscWS, osclients) => { const osclients = [] wss.on('connection', function (ws) { - console.log('client connection', ws) + console.log('new client connection') const oscWS = new osc.WebSocketPort({ socket: ws, metadata: false diff --git a/test.js b/test.js index fe36e9c..5911761 100644 --- a/test.js +++ b/test.js @@ -91,7 +91,6 @@ function objAnim() { obj.setRotationFromQuaternion(qObj); AX.scale.x = accX / 1000; - console.log(accX); }); // Drzimo vse stiri gumbe (reset)? - kalibracija! @@ -137,28 +136,23 @@ const getVal = function (msg) { } oscCallbacks = { - '/keys/': [ + '/keys': [ function(args) { keysPressed = args.map(getVal); } ], - '/quaternion/': [ + '/quaternion': [ function (args) { // Popravimo osi (w x y z po defaultu HMM) - [qWW.w, qWW.z, qWW.x, qWW.y] = args.map(getVal); - // Dve sta obratno :) - // Ali pac? Vcasih da, drugic ne. Zanimivo. - qWW.x *= -1; - qWW.z *= -1; + [qWW.w, qWW.x, qWW.y, qWW.z] = args.map(getVal); } ], - '/accel/': [ + '/accel': [ function (args) { - console.log('accel', args.map(getVal)); [accX, accY, accZ] = args.map(getVal); } ], - '/gyro/': [ + '/gyro': [ function (args) { [rotacijaX, rotacijaY, rotacijaZ] = args.map(getVal); }