diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c index 57a6ba7..63a7f5f 100755 --- a/source/Concept/Framework/modules/interpreter/position_tracker.c +++ b/source/Concept/Framework/modules/interpreter/position_tracker.c @@ -34,17 +34,17 @@ void Position_Tracker::Update() movementRightY = 0; } - float movementDifferenceX = movementRightX - movementLeftX; + float movementDifferenceX = (movementRightX + mouseRight->GetPositionX()) - (movementLeftX + mouseLeft->GetPositionX()); float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY()); + float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f); + float robotMovementX = movementDifferenceX / 2.0f; float robotMovementY = movementDifferenceY / 2.0f; - robotMovementX += movementLeftX; - robotMovementY += movementLeftY + mouseLeft->GetPositionY(); + robotMovementX += movementLeftX + mouseLeft->GetPositionX() + (-mouseLeft->GetPositionX() * cos(orientationDiff)); + robotMovementY += movementLeftY + mouseLeft->GetPositionY() + (-mouseLeft->GetPositionX() * sin(orientationDiff)); //float robotDistance = sqrt(robotMovementX * robotMovementX + robotMovementY * robotMovementY); - float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f); - float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotMovementX - sin(this->orientation + (orientationDiff / 2.0f)) * robotMovementY; float absoluteDiffY = sin(this->orientation + (orientationDiff / 2.0f) + PI / 2.0f) * robotMovementY + cos(this->orientation + (orientationDiff / 2.0f) - PI / 2.0f) * robotMovementX;