Added Compass Helper Functions

BoneCentral/Brain/Tests/test_compassTools.js

@@ -0,0 +1,38 @@
+var ctools  = require('../Utilities/compassTools.js');
+var vector  = require('../Utilities/vector.js');
+
+const p = "PASS", f = "FAIL";
+const TOL = 0.0001;
+var state, res, a, r, c, theta;
+
+function within(target, precision, test) {
+    return (test < target+precision) && (test > target-precision);
+}
+
+console.log("Test Flat:");
+c = vector.zero;
+for(i = -4; i < 5; i++){
+    var test = (45*i);
+    theta = test/180.0*Math.PI;
+    c.x = Math.cos(theta);
+    c.y = Math.sin(theta);
+    res = ctools.degrees(c);
+    state = within(test,TOL,res)?p:f;
+    console.log("degrees(c,a): "+test+":"+res.toFixed(0)+"\t"+state);
+}
+
+console.log("Test 45-x Tilt:");
+var theta = 45/180.0*Math.PI;
+a = vector.rotx(vector.v3(0,0,-1), theta);
+r = vector.rotx(vector.v3(1,0, 0), theta);
+for(i = -4; i < 5; i++){
+    var test = (45*i);
+    var theta2 = test/180.0*Math.PI;
+    c.x = Math.cos(theta2);
+    c.y = Math.sin(theta2);
+    c.z = 0;
+    c = vector.rotx(c, theta);
+    res = ctools.degrees(c, a, r);
+    state = within(test,TOL,res)?p:f;
+    console.log("degrees(c,a): "+test+":"+res.toFixed(0)+"\t"+state);
+}

BoneCentral/Brain/Utilities/compassTools.js

@@ -0,0 +1,51 @@
+var vector = require('./vector.js');
+
+var getTrueCompass = function(compv, accelv) {
+    var av      = unitScale(accelv);
+    var right   = unitScale(vector.cross(av, compv));
+    var res     = unitScale(vector.cross(right, av));
+    return res;
+};
+
+var cdeg = function(compv, va) {
+    var cv = compv;
+
+    if(va) {
+        var az   = vector.v3(0,0,-1);
+        var ax   = vector.v3(1,0, 0);
+        var axis = vector.unit(vector.cross(az,va));
+        var angy = vector.angle(axis, ax);
+        var angx = vector.angle(va, az);
+        var c    = vector.roty(cv, -angy);
+        cv       = vector.rotx(c, -angx);
+    }
+
+    return deg(cv);
+};
+
+function unitScale(v) {
+    const scale = 5;
+    return vector.scale(vector.unit(v), scale);
+}
+
+function deg(v) {
+    var res = 0;
+
+         if(v.x===0&&v.y > 0){res = 90;}
+    else if(v.x < 0&&v.y===0){res =180;}
+    else if(v.x===0&&v.y < 0){res =-90;}
+    else {
+        d=Math.atan(v.y/v.x)*180/Math.PI;
+
+             if(v.x<0&&v.y>0){res = 180+d;}// 2nd quadrant
+        else if(v.x<0&&v.y<0){res =-180+d;}// 3rd quadrant
+        else{return d;}                    // 1st & 4th quadrant
+    }
+
+    return res;
+}
+
+module.exports = {
+    getTrueCompass: getTrueCompass,
+    degrees: cdeg
+};