diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c index c8cd52d..57a6ba7 100755 --- a/source/Concept/Framework/modules/interpreter/position_tracker.c +++ b/source/Concept/Framework/modules/interpreter/position_tracker.c @@ -41,12 +41,15 @@ void Position_Tracker::Update() float robotMovementY = movementDifferenceY / 2.0f; robotMovementX += movementLeftX; robotMovementY += movementLeftY + mouseLeft->GetPositionY(); - float robotDistance = sqrt(robotMovementX * robotMovementX + robotMovementY * robotMovementY); + //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); + 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; + + //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) {