1 files changed, 61 insertions, 1 deletions
diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c
index 262069a..c8cd52d 100755
--- a/source/Concept/Framework/modules/interpreter/position_tracker.c
+++ b/source/Concept/Framework/modules/interpreter/position_tracker.c
@@ -3,5 +3,65 @@
 //-----------------------------------------------------------------------------
 void Position_Tracker::Update()
 {
-	//insert code here
+	Mouse_Sensor* mouseLeft = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
+	Mouse_Sensor* mouseRight = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
+
+	int8 leftX = mouseLeft->GetXMovement();
+	int8 leftY = mouseLeft->GetYMovement();
+	float distanceLeft = sqrt(leftX * leftX + leftY * leftY);
+	float angleLeft = easyAngle(atan2(leftY, leftX) + (225.0f * PI / 180.0f));
+
+	float movementLeftX = cos(angleLeft) * distanceLeft;
+	float movementLeftY = sin(angleLeft) * distanceLeft;
+
+	if(!leftX && !leftY)
+	{
+		movementLeftX = 0;
+		movementLeftY = 0;
+	}
+
+	int8 rightX = mouseRight->GetXMovement();
+	int8 rightY = mouseRight->GetYMovement();
+	float distanceRight = sqrt(rightX * rightX + rightY * rightY);
+	float angleRight = easyAngle(atan2(rightY, rightX) - (45.0f * PI / 180.0f));
+
+	float movementRightX = cos(angleRight) * distanceRight;
+	float movementRightY = sin(angleRight) * distanceRight;
+
+	if(!rightX && !rightY)
+	{
+		movementRightX = 0;
+		movementRightY = 0;
+	}
+
+	float movementDifferenceX = movementRightX - movementLeftX;
+	float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY());
+
+	float robotMovementX = movementDifferenceX / 2.0f;
+	float robotMovementY = movementDifferenceY / 2.0f;
+	robotMovementX += movementLeftX;
+	robotMovementY += movementLeftY + mouseLeft->GetPositionY();
+	float robotDistance = sqrt(robotMovementX * robotMovementX + robotMovementY * robotMovementY);
+
+	float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f);
+
+	float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotDistance * sign(robotMovementX);
+	float absoluteDiffY	= sin(this->orientation + (orientationDiff / 2.0f)) * robotDistance * sign(robotMovementY);
+
+	if(!robotMovementX && !robotMovementY)
+	{
+		absoluteDiffX = 0;
+		absoluteDiffY = 0;
+	}
+
+	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("            ", 5, 1);
+	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(absoluteDiffX, 5, 1);
+	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(absoluteDiffY, 12, 1);
+
+	this->positionX += absoluteDiffX;
+	this->positionY += absoluteDiffY;
+
+	this->orientation += orientationDiff;
+
+	this->orientation = easyAngle(this->orientation);
 }