How to split this formula into function in c++?

Viewed 114

My code uses formula to find where my robot sonar sensors found an obstacle. The code looks like this:

 obstacleX = robot.x + robot.sensorReadings.at(i) * cos((robot.deg + i * angleBetweenSensors) * PI / 180);
 obstacleY = robot.y + robot.sensorReadings.at(i) * sin((robot.deg + i * angleBetweenSensors) * PI / 180);

And I would like to make it a function to don't repeat this formula many times(and to make it easier to change). I would do a function like this:

double calculateObstaclePosition(double robotX, double sesnorReading, double robotDegree, int angleBetweenSensors){
      return robotX + sesnorReading * cos((robotDegree + i * angleBetweenSensors) * PI / 180);
}

And pass by

obstacleX = calculateObstaclePosition(robot.x, robot.sensorReadings.at(i), robot.deg, angleBetweenSensors);

But formula for X and Y is almost the same, only difference is that one uses sinus, and second uses cosinus. Should I then create two almost identical functions or could it be done in one?

7 Answers
Related