3D_Tracking_with_IMU/iot/sketch-test2D.js

/*
 *  rootApostolos
 *  P5js example 
 *  IMU  2D
 *  
 * 
 * 
 */
var dataObjects = [];
var objectIndex = 0;
var debugFlag = true;

var bubbles = []; // Global array to hold all bubble objects
var currentBubble;

var anglesX =0;		// rotation X
var anglesY =0;		// rotation Y
var anglesZ =0;		// rotation Z

var G = 9.82; 			// g in greece https://en.wikipedia.org/wiki/Gravitational_acceleration

var dt = 0.05; 			// miliseconds -> second read in sketch Arduino

var canvasX = 900;  //px
var canvasY = 600;  //px

var AX, AY, BX, BY, CX, CY; // accelarator data after calculation

AX = canvasX/2;					// start position X (center)
AY = canvasY/2;					// start position Y (center)

var i = 0;
var start = false;
var xTranform = 0;
var yTranform = 0;
var coordinatesTranformationStep = 300;
var endFlagMessage = true;
// Put any asynchronous data loading in preload to complete before "setup" is run
function preload() {
	// data = loadJSON(file);
}

function setup() {
	dataObjects = [];
	objectIndex = 0;
	bubbles = []; 
	currentBubble = null;
	anglesX = 0;		
	anglesY = 0;		
	anglesZ = 0;
	AX = canvasX / 2;
	AY = 2*canvasY;
	xTranform = 0;
	yTranform = 0;
	i = 0;
	start = false;
	endFlagMessage = true;
	transformJsonDataToObject();
	createCanvas(canvasX, canvasY);

	// createCanvas(canvasX, canvasY, WEBGL);
	// camera(450, 300, 0);
	frameRate(10);  //This must the same as collect data frequency
}
  
function draw() {
	if(start){
		// move();
		move2();
	
		background(100, 100, 100);
	  

		if (bubbles[bubbles.length - 1].x > canvasX){
			xTranform -= coordinatesTranformationStep;
			for (var i = 0; i < bubbles.length; i++) {
				// bubbles[i].x += xTranform;
				bubbles[i].x -= coordinatesTranformationStep;
			}
		}

		if (bubbles[bubbles.length - 1].x < 0){
			xTranform += coordinatesTranformationStep;
			for (var i = 0; i < bubbles.length; i++) {
				// bubbles[i].x += xTranform;
				bubbles[i].x += coordinatesTranformationStep;
			}
		}

		if (bubbles[bubbles.length - 1].y > canvasY){
			yTranform -= coordinatesTranformationStep;
			for (var i = 0; i < bubbles.length; i++) {
				bubbles[i].y -= coordinatesTranformationStep;
			}
		}

		if (bubbles[bubbles.length - 1].y < 0){
			yTranform += coordinatesTranformationStep;
			for (var i = 0; i < bubbles.length; i++) {
				bubbles[i].y += coordinatesTranformationStep;
			}
		}

		for (var i = 0; i < bubbles.length; i++) {
			if(i == (bubbles.length - 1) ){
				bubbles[i].display(0);
			}
			else{
				bubbles[i].display(255);
			}
	
		}
	
		//============= 2 END ==================

	}
}


	

// Bubble class
class Bubble {
	constructor(x, y, diameter) {
		this.x = x;
		this.y = y;
		this.diameter = diameter;
		this.radius = diameter / 2;

		// Display the Bubble
		this.display = function (color) {
			fill(color);
			stroke(0);
			strokeWeight(0.8);
			var v = createVector(0, 1);
			v.setMag(100);
			var xx1 = canvasX/2;
			var yy1 = canvasY/2;
			var xx2 = xx1 + v.x;
			var yy2 = yy1 + v.y;
			line(xx1, yy1, xx2, yy2);
			xx1 = xx2;
			yy1 = yy2;
			//https://el.wikipedia.org/wiki/%CE%91%CE%BA%CF%84%CE%AF%CE%BD%CE%B9%CE%BF_(%CE%BC%CE%BF%CE%BD%CE%AC%CE%B4%CE%B1_%CE%BC%CE%AD%CF%84%CF%81%CE%B7%CF%83%CE%B7%CF%82)
			// https://stackoverflow.com/questions/12959237/get-point-coordinates-based-on-direction-and-distance-vector
			// ihttps://en.wikipedia.org/wiki/Euler_angles#Geometrical_definition
			//angleMode(DEGREES);
			angleMode(RADIANS);
			v.rotate(Math.PI - anglesX);
			//v.rotate(XXX); 
			stroke(255, 0, 0);
			line(xx1, yy1, xx1 + v.x, yy1 + v.y);
			ellipse(this.x, this.y, this.diameter, this.diameter);
		};
	}
}

function move2(){

	if(objectIndex < dataObjects.length){

		var diameter = 5;
	
		//calculate DX, DY and DZ
		  let aX = dataObjects[objectIndex].accelerator.X * G;
		  let aY = dataObjects[objectIndex].accelerator.Y * G;
		  let aZ = dataObjects[objectIndex].accelerator.Z * G;
	  
		  AX=AX+aX*dt; 			// new position X
		  AY=AY+aY*dt;			// new position Y
	  
		  let gX = dataObjects[objectIndex].gyroscope.X * dt;
		  let gY = dataObjects[objectIndex].gyroscope.Y * dt;
		  let gZ =  dataObjects[objectIndex].gyroscope.Z * dt;
	  
		  anglesZ  = Math.acos((-gY)/(Math.pow(1-Math.pow(gZ,2),0.5)));
		  //peristrofi ston x axona mikos
		  anglesX=acos(gZ);
		  //peristrofi ston y axona
		  anglesY=acos(gX/(Math.pow(1-pow(gZ,2),0.5)));
	
	//========================== WHAT IS THAT??? ======================
	//  // {"sensor":"magnometer","X":-0.000647,"Y":-0.000583,"Z":0.000652}
	//   var iii=i+2
	//   mX = dataObjects[objectIndex].magnometer.X;
	//   mY = dataObjects[objectIndex].magnometer.Y;
	//   mZ = dataObjects[objectIndex].magnometer.Z;
	//   sensor = data[iii].sensor;
	
	
	//   var anglesXX = anglesX * 180 / Math.PI;
	
	
	//   BX=BX+aX*3;
	//   BY=BY+aY*3;
	
	//   CX=CX+aX*3;
	//   CY=CY+aY*3;
	//=================================================================
	  
	
		currentBubble = new Bubble(AX + xTranform, AY + yTranform, diameter);
		bubbles.push(currentBubble);
		objectIndex++;
		// debug('ADD POINT: '+objectIndex);
	}
	else{
		if (endFlagMessage){
			debug('END!!!');
			endFlagMessage = false;
		}
	}
}

function transformJsonDataToObject(){

	let index = 0;

	while (index < Helper.fixedData.length){
		dataObjects.push(new DataObj(
			Helper.fixedData[index].A.X, Helper.fixedData[index].A.Y, Helper.fixedData[index].A.Z,
			Helper.fixedData[index].G.X, Helper.fixedData[index].G.Y, Helper.fixedData[index].G.Z,
			Helper.fixedData[index].M.X, Helper.fixedData[index].M.Y, Helper.fixedData[index].M.Z
			));

		index += 1;
	}
}

class DataObj {
	constructor(ax, ay, az, gx, gy, gz, mx, my, mz) {
		this.accelerator = new SensorData(ax, ay, az);
		this.gyroscope = new SensorData(gx, gy, gz);
		this.magnometer = new SensorData(mx, my, mz);
	}
}

class SensorData{
	constructor(X, Y, Z){
		this.X=X;
		this.Y=Y;
		this.Z=Z;
	}
}

function moveWindowRight(xMove){
	for (var i = 0; i < bubbles.length; i++) {
		bubbles[i].x = bubbles[i].x - xMove;
	}
}
function debug(msg){
	console.log("============ DEBUG ===============");
	console.log(msg);
}