From 22433e52a7b0f2e5c548edb915ea11a38a8077ad Mon Sep 17 00:00:00 2001
From: sicarius <devnull@localhost>
Date: Tue, 20 Feb 2007 21:01:02 +0000
Subject: +++ Codework, Navigator&Positiontracker done (buggy?)

---
 .../modules/interpreter/position_tracker.c         | 62 +++++++++++++++++++++-
 .../modules/interpreter/position_tracker.h         |  6 +++
 2 files changed, 67 insertions(+), 1 deletion(-)

(limited to 'source/Concept/Framework/modules/interpreter')

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);
 }
diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h
index b31dd8f..e93ef0c 100755
--- a/source/Concept/Framework/modules/interpreter/position_tracker.h
+++ b/source/Concept/Framework/modules/interpreter/position_tracker.h
@@ -10,12 +10,18 @@ public:
 	{
 		this->parent = NULL;
 		this->moduleId = 0;
+		this->positionX = 0;
+		this->positionY = 0;
+		this->orientation = 0;
 	}
 
 	Position_Tracker(uint32 trackerId)
 	{
 		this->parent = NULL;
 		this->moduleId = trackerId;
+		this->positionX = 0;
+		this->positionY = 0;
+		this->orientation = 0;
 	}
 
 protected:
-- 
cgit v1.2.3