c2-utopia/AHRSensor/AHRSensor.sc

AHRSensor {
  var <id,
  <>parent,

  // Surov kvaternion iz senzorjev
  <>quat,
  // Kalibracijski quaternion (kao "nula")
  <>refQuat,
  // Relativni kvaternion (razlika od parenta)
  <relQuat,
  // Razlika od prejsnjega do trenutnega quaterniona
  <quatDiff,

  <>euler,
  // "Prednji" vektor
  <>vecF,
  // "Zgornji" vektor
  <>vecT,
  <>accel,
  <>accelSum,

  <>battery,
  <>eps,

  // GUI elementi
  // Quaternion
  <gQw,
  <gQx,
  <gQy,
  <gQz,

  // Referencni quaternion
  <gQr,

  // Eulerjevi koti
  <gEx,
  <gEy,
  <gEz,
  <gExl,
  <gEyl,
  <gEzl,

  // Rotacijski vektor
  <gVfx,
  <gVfy,
  <gVfz,
  <gVfxl,
  <gVfyl,
  <gVfzl,
  <gVtx,
  <gVty,
  <gVtz,
  <gVtxl,
  <gVtyl,
  <gVtzl,

  // Pospeskomer
  <gAx,
  <gAy,
  <gAz,
  <gAs,

  // Baterija
  <gB,
  <gBi,

  // Dogodkov na sekundo
  <gEps;

  *new { |id, parent = nil|
    ^super.newCopyArgs(id, parent).init;
  }

  quat2euler { |quat|
    // Quaternion
    //var w = quat.a, x = quat.b, y = quat.c, z = quat.d,

    // Quaternion v supercolliderju gre po zaporedju wxyz = abcd
    // Iz foruma (BNO055): x = -y, y = x, z = -z
    var q = [quat.a, quat.b, quat.c, quat.d],
    //var q = [quat.a, quat.c.neg, quat.b, quat.d.neg],
    // Euler angles; roll, pitch and yaw
    e = [0, 0, 0];

    e[0] = atan2((2 * q[2] * q[0]) - (2 * q[1] * q[3]), 1 - (2 * q[2] * q[2]) - (2 * q[3] * q[3]));
    e[1] = asin((2 * q[1] * q[2]) + (2 * q[3] * q[0]));
    e[2] = atan2((2 * q[1] * q[0]) - (2 * q[2] * q[3]), 1 - (2 * q[1] * q[1]) - (2 * q[3] * q[3]));
    /*
    e[0] = atan2( (2 * q[1] * q[2]) - (2 * q[0] * q[3]), (2 * q[0]*q[0]) + (2 * q[1] * q[1]) - 1); // psi
    e[1] = asin( (2 * q[1] * q[3]) + (2 * q[0] * q[2])).neg; // theta
    e[2] = atan2( (2 * q[2] * q[3]) - (2 * q[0] * q[1]), (2 * q[0] * q[0]) + (2 * q[3] * q[3]) - 1); // phi
    */

    /*
    // Variables
    sinr_cosp, cosr_cosp, sinp, cosp, siny_cosp, cosy_cosp;
    // Conversion source:
    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_code_2

    sinr_cosp = 2 * (w * x * y * z);
    cosr_cosp = 1 - (2 * (x * x * y * y));
    // roll (x-axis rotation)
    e[0] = atan2(sinr_cosp, cosr_cosp);

    sinp = sqrt(1 + (2 * ((w * y) - (x * z))));
    cosp = sqrt(1 - (2 * ((w * y) - (x * z))));
    // pitch (y-axis rotation)
    e[1] = 2 * atan2(sinp, cosp) - 0.5pi;

    siny_cosp = 2 * ((w * z) + (x * y));
    cosy_cosp = 1 - (2 * ((y * y) + (z * z)));
    // yaw (z-axis rotation)
    e[2] = atan2(siny_cosp, cosy_cosp);
    */

    /*
    x2, y2, z2, xx, xy, xz, yy, yz, zz, wx, wy, wz,
    matrix, m11, m12, m13, m21, m22, m23, m31, m32, m33;

    x2 = x + x;  y2 = y + y;  z2 = z + z;
    xx = x * x2; xy = x * y2; xz = x * z2;
    yy = y * y2; yz = y * z2; zz = z * z2;
    wx = w * x2; wy = w * y2; wz = w * z2;

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

    m11 = matrix[0]; m12 = matrix[4]; m13 = matrix[8];
    m21 = matrix[1]; m22 = matrix[5]; m23 = matrix[9];
    m31 = matrix[2]; m32 = matrix[6]; m33 = matrix[10];

    e[1] = asin(m13.clip(-1, 1));

    if ((m13.abs < 0.9999999), {
        e[0] = atan2(m23.neg, m33);
        e[2] = atan2(m12.neg, m11);
    }, {
        e[0] = atan2(m32, m22);
        e[2] = 0;
    });
    */

    ^e;
  }

  init {
	quat = Quaternion.new(1, 0, 0, 0);
	refQuat = Quaternion.new(1, 0, 0, 0);
    relQuat = Quaternion.new(1, 0, 0, 0);
	euler = [0, 0, 0];
	accel = [0, 0, 0];
	battery = 0;
    eps = 0;
    vecF = Cartesian.new(1, 0, 0);
    vecT = Cartesian.new(0, 0, 1);

	this.guiInit;
  }

  guiInit {
	// Barve
	var cRed, cGreen, cBlue, cPurple, cI;
	// Intenziteta
	cI = 0.8;

	cRed = Color.new(cI, 0, 0);
	cGreen = Color.new(0, cI, 0);
	cBlue = Color.new(0, 0, cI);
	cPurple = Color.new(cI, 0, cI);

	gEx = StaticText().string_(0).stringColor_(cRed);
	gEy = StaticText().string_(0).stringColor_(cGreen);
	gEz = StaticText().string_(0).stringColor_(cBlue);
    gExl = LevelIndicator();
    gEyl = LevelIndicator();
    gEzl = LevelIndicator();

	gVfx = StaticText().string_(0).stringColor_(cRed);
	gVfy = StaticText().string_(0).stringColor_(cGreen);
	gVfz = StaticText().string_(0).stringColor_(cBlue);
    gVfxl = LevelIndicator();
    gVfyl = LevelIndicator();
    gVfzl = LevelIndicator();

	gVtx = StaticText().string_(0).stringColor_(cRed);
	gVty = StaticText().string_(0).stringColor_(cGreen);
	gVtz = StaticText().string_(0).stringColor_(cBlue);
    gVtxl = LevelIndicator();
    gVtyl = LevelIndicator();
    gVtzl = LevelIndicator();

	//gAx = StaticText().string_(0).stringColor_(cRed);
	//gAy = StaticText().string_(0).stringColor_(cGreen);
	//gAz = StaticText().string_(0).stringColor_(cBlue);
    gAx = LevelIndicator();
    gAy = LevelIndicator();
    gAz = LevelIndicator();
    gAs = LevelIndicator();

	gQw = StaticText().string_(0).stringColor_(cPurple);
	gQx = StaticText().string_(0).stringColor_(cRed);
	gQy = StaticText().string_(0).stringColor_(cGreen);
	gQz = StaticText().string_(0).stringColor_(cBlue);

	gQr = Button().string_("ref").action_({ |butt|
      this.setRefQuat;
    });

	gB = StaticText().string_(0).stringColor_(cRed);
	gBi = LevelIndicator();
	gEps = StaticText().string_(0);
  }

  getGui {
    var napis = "Sensor " ++ id, elementi;
    if (parent != nil) { napis = napis ++ " (parent " ++ parent.id ++ ")"};

	elementi = [
      [
        StaticText().font_(Font("OpenSans", 12, true)).string_(napis),
        nil,
        [StaticText().string_("bat: "), align: \right],
        [gB, columns: 2],
        gBi,
        nil,
        [StaticText().string_("events/s: "), align: \right],
        gEps,
        gQr
      ],
      /*
	  [
		StaticText().string_("quaternion: "),
		[StaticText().string_("w: "), align: \right],
		gQw,
		[StaticText().string_("x: "), align: \right],
		gQx,
		[StaticText().string_("y: "), align: \right],
		gQy,
		[StaticText().string_("z: "), align: \right],
		gQz,
	  ],
      */
      [
        [StaticText().string_("e"), align: \right],
        [gExl, columns: 2],
        [gEyl, columns: 2],
        [gEzl, columns: 2],
          gEx, gEy, gEz
      ],
      /*
      [ 
		StaticText().string_("vec F: "),
		[StaticText().string_("x: "), align: \right],
		gVfx,
		[StaticText().string_("y: "), align: \right],
		gVfy,
		[StaticText().string_("z: "), align: \right],
		gVfz
	  ],
      [
		[StaticText().string_("vec F: "), align: \right],
        [gVfxl, columns: 2],
        [gVfyl, columns: 2],
        [gVfzl, columns: 2]
      ],
      */
      /*
      [ 
		StaticText().string_("vec T: "),
		[StaticText().string_("x: "), align: \right],
		gVtx,
		[StaticText().string_("y: "), align: \right],
		gVty,
		[StaticText().string_("z: "), align: \right],
		gVtz
	  ],
      [
        [StaticText().string_("vec T:"), align: \right],
        [gVtxl, columns: 2],
        [gVtyl, columns: 2],
        [gVtzl, columns: 2]
      ],
      */
	  [ 
		[StaticText().string_("accel: "), align: \right],
		[gAx, columns: 2],
		[gAy, columns: 2],
		[gAz, columns: 2],
        gAs
	  ]
	];
    ^elementi;
  }

  setQuat { |q|
    var rq;
    // Razlika od prejsnjega quaterniona
    quatDiff = quat.conjugate * q;
    // "Surov quat"
    quat = q;
    // Referencen quat
    rq = refQuat.conjugate * q;
    // Ce imamo parenta, shrani razliko, sicer samo referencen
    relQuat = if ((parent != nil), {
      parent.relQuat.conjugate * rq;
      // TODO ali bi tudi childe poapdejtali? Mislim, da ni treba, ker dokaj pogosto pride nov q
    }, {
      rq;
    });
  }

  setRefQuat {
    refQuat = quat;
    euler = [0, 0, 0];
  }

  updateEuler { |q|
    // Normalize first ?
    euler = [q.tilt, q.tumble, q.rotate];
    //euler = euler + [quatDiff.tilt, quatDiff.tumble, quatDiff.rotate];
    //euler = this.quat2euler(newQuat);
  }

  rotateVector { |q|
    // rotiramo vektor s quaternionom
    var vF = Cartesian.new(1, 0, 0), vT = Cartesian.new(0, 0, 1),
        u, s, newVecF, newVecT;

    u = Cartesian.new(q.b, q.c, q.d);
    s = q.a;

    vecF = (
      (u * (2.0 * this.vecDotProd(u, vF)))
      + (vF * ((s * s) - this.vecDotProd(u, u)))
    ) + (
      this.vecCrossProd(u, vF) * (2.0 * s)
    );

    vecT = (
      (u * (2.0 * this.vecDotProd(u, vT)))
      + (vT * ((s * s) - this.vecDotProd(u, u)))
    ) + (
      this.vecCrossProd(u, vT) * (2.0 * s)
    );
  }

  vecDotProd { |v1, v2|
    ^((v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z));
  }

  vecCrossProd { |v1, v2|
    ^Cartesian.new(
      (v1.y * v2.z) - (v1.z * v2.y),
      (v1.z * v2.x) - (v1.x * v2.z),
      (v1.x * v2.y) - (v1.y * v2.x));
  }

  getDecimal { |num|
    // Stevilo decimalk
    var div = 0.01, rem;
    rem = num % div;
    ^(num - rem);
  }

  refreshGuiQuat {
	gQw.string_(this.getDecimal(quat.a));
	gQx.string_(this.getDecimal(quat.b));
	gQy.string_(this.getDecimal(quat.c));
	gQz.string_(this.getDecimal(quat.d));
  }

  refreshGuiEuler {
	gEx.string_(this.getDecimal(euler[0]));
	gEy.string_(this.getDecimal(euler[1]));
	gEz.string_(this.getDecimal(euler[2]));
	gExl.value_(sin(euler[0]).linlin(-1, 1, 0, 1));
	gEyl.value_(sin(euler[1]).linlin(-1, 1, 0, 1));
	gEzl.value_(sin(euler[2]).linlin(-1, 1, 0, 1));
  }

  refreshGuiVector {
	gVfx.string_(this.getDecimal(vecF.x));
	gVfy.string_(this.getDecimal(vecF.y));
	gVfz.string_(this.getDecimal(vecF.z));
	gVfxl.value_(vecF.x.linlin(-1, 1, 0, 1));
	gVfyl.value_(vecF.y.linlin(-1, 1, 0, 1));
	gVfzl.value_(vecF.z.linlin(-1, 1, 0, 1));

	gVtx.string_(this.getDecimal(vecT.x));
	gVty.string_(this.getDecimal(vecT.y));
	gVtz.string_(this.getDecimal(vecT.z));
	gVtxl.value_(vecT.x.linlin(-1, 1, 0, 1));
	gVtyl.value_(vecT.y.linlin(-1, 1, 0, 1));
	gVtzl.value_(vecT.z.linlin(-1, 1, 0, 1));
  }


  refreshGuiAccel {
    // Stevilo decimalk
    //var prec = 2;
	//gAx.string_(accel[0].asStringPrec(prec));
	//gAy.string_(accel[1].asStringPrec(prec));
	//gAz.string_(accel[2].asStringPrec(prec));
    var from = -50, to = 50;
    [gAx, gAy, gAz].do({|el, i|
      el.value_(accel[i].linlin(from, to, 0, 1));
    });
    gAs.value_(accelSum.linlin(0, 50, 0, 1));
  }

  refreshGuiBat {
	gB.string_(this.getDecimal(battery));
    gBi.value_(battery.linlin(3.6, 4.2, 0, 1));
  }

  refreshGuiEps {
    // Stevilo decimalk
    var prec = 2;
	gEps.string_(eps);
  }

  refreshGui {
    this.refreshGuiQuat;
    this.refreshGuiEuler;
    this.refreshGuiAccel;
    this.refreshGuiBat;
  }
}