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

Question

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?

Answer 1

Options available:

(1) Use a flag to indicate whether you want to use cosine or sine:

double calculateObstaclePosition(double robotC, ..., bool useCos)
{
   double angle = (robotDegree + i * angleBetweenSensors) * PI / 180;
   return robotC + sensorReading * (useCos ? cos(angle) : sin(angle));
}

(2) Create some 2D vector datatype and return both coordinates in one go

struct vec2D
{
   double x, y;
};
vec2D calculateObstaclePosition(double robotX, double robotY, ...)
{
   vec2D pos;   
   double angle = (robotDegree + i * angleBetweenSensors) * PI / 180;
   pos.x = robotX + sensorReadingX * cos(angle);
   pos.y = robotY + sensorReadingY * sin(angle);
   return pos;
}

Or use a class or something. Also can convert robot class to use vec2D for coordinates.

(3) Obscure one: pass a pointer of the numerical function to want. UNSAFE!!!

typedef double (*numfuncptr)(double a);
double calculateObstaclePosition(double robotC, ..., numfuncptr trig)
{
   double angle = ...;
   return robotC + sensorReading * trig(angle);
}

(4) Not-so-obscure, but C-style and thus not OOP-esque: pass output pointers of your variables. (Again unsafe)

void calculateObstaclePosition(double robotX, double robotY, ..., double* outX, double* outY)
{
   double angle = ...;
   *outX = robotX + ...
   *outY = robotY + ...;
}

Answer 2

struct Robot {
    double x;
    double y;
    double deg; // robotDegree
    double angle; // angleBetweenSensors

    std::vector<double> sensorReadings; // sensorReading
};

std::pair<double, double> calculateObstaclePosition(const Robot &robot, int i)
{
  double obstacleX = robot.x + robot.sensorReadings.at(i) * cos((robot.deg + i * robot.angle) * PI / 180);
  double obstacleY = robot.y + robot.sensorReadings.at(i) * sin((robot.deg + i * robot.angle) * PI / 180);

  return std::make_pair(obstacleX, obstacleY);
}

How about that? You can create some classes to pass and to get the values to the function.

Answer 3

declare it like this:

std::array<double,2> calculateObstaclePosition(const Robot& robot, int angleBetweenSensors)
{
    return {
        robot.x + robot.sensorReadings.at(i) * cos((robot.deg + i * angleBetweenSensors) * PI / 180) ,
        robot.y + robot.sensorReadings.at(i) * sin((robot.deg + i * angleBetweenSensors) * PI / 180)
    };
}

and call it with

std::array<double,2> obstacle = calculateObstaclePosition(robot,angleBetweenSensors);

It won't keep you from doing the calculation twice, but given that the evaluation of the arguments x , y , sensorReadings.at(i) and robot.deg is not very costly you shouldn't worry about that too much. If it is costly, pass them as an argument as you do now instead of passing the whole robot or first evaluate them to a temporary variable and use this in your return statement.

Benefit of this declaration is, that it keeps you from declaring two different functions and ties the values for x and y together. If you like the notation with .x and .y better, use a

 struct Coords{double x, doubley};

instead of the std::array .

Answer 4

In the case of this simple function it IMHO does not really matter, but in case of more complicated functions you may use function pointers to select specific functions used in a larger function:

// enum to define which arguyment is calculated - X or Y
enum XorYEnum  { X, Y  };

double calculateObstaclePosition(double robotX, double sesnorReading, double robotDegree, int angleBetweenSensors, XorYEnum XorY)
{
    // Select the sin or cos function and assign function pointer
    double (* SinOrCosFunc)(double);
    if (XorY == X)
        SinOrCosFunc = cos;
    else
        SinOrCosFunc = sin;

    // Calculate
    return robotX + sesnorReading * SinOrCosFunc((robotDegree + i * angleBetweenSensors) * PI / 180);
}

Answer 5

If you only want to do the calculation once, you could also use a function pointer and pass in sin or cos.

For example

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

And then call it by passing in sin or cos as the first parameter

double posCur = calculateObstaclePosition(sin, param2, param3, param4, param5);

or

double posCur = calculateObstaclePosition(cos, param2, param3, param4, param5);

Answer 6

I suggest solution with template partially specialisation, that allows minimize writing. There is no condition in code and no condition in run time. Let define special functions that work as sin or cos dependently of X or Y.

Define enum for X and Y to reference:

typedef enum { X, Y } xy_enum;

Partially specialized template classes for compile time selecting:

template<xy_enum T>
struct _xcys // x cos y sin
{
    static double f( double t ) { return cos(t); }
};

template<>  // explicit specialization for T = Y
struct _xcys<Y> // x cos y sin
{
    static double f( double t ) { return sin(t); }
};

template<xy_enum T>
struct _xsyc // x sin y cos
{
    static double f( double t ) { return sin(t); }
};

template<>  // explicit specialization for T = Y
struct _xsyc<Y> // x sin y cos
{
    static double f( double t ) { return cos(t); }
};

Define functions that work as sin or cos dependently of X or Y. So xcys() work for X as cos and for Y as sin. And xsyc() work for X as sin and for Y as cos.

template<xy_enum T> // x sin y cos
double xcys ( double t ) { return _xcys<T>::f(t); }
template<xy_enum T> // x sin y cos
double xsyc ( double t ) { return _xsyc<T>::f(t); }

Simple test

std::cout << xcys<X>(0)      << " " << xcys<Y>(0) << std::endl;
std::cout << xcys<X>(M_PI/2) << " " << xcys<Y>(M_PI/2) << std::endl;
std::cout << xsyc<X>(0)      << " " << xsyc<Y>(0) << std::endl;
std::cout << xsyc<X>(M_PI/2) << " " << xsyc<Y>(M_PI/2) << std::endl;

Result output:

 1 0 ~0 1 0 1 1 ~0 

Finally your code with two functions like this:

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

Possible rewrite with one single template function:

template< xy_enum T >
double calculateObstaclePosition(double robotXY, double sesnorReading, double robotDegree, int angleBetweenSensors){
      return robotXY + sesnorReading * xcys<T>((robotDegree + i * angleBetweenSensors) * PI / 180);
}

And call single function twice for x and for y:

obstacleX = calculateObstaclePosition<X>(robot.x, robot.sensorReadings.at(i), robot.deg, angleBetweenSensors);
obstacleY = calculateObstaclePosition<Y>(robot.y, robot.sensorReadings.at(i), robot.deg, angleBetweenSensors);

Answer 7

The formulas for calculating obstacleX and obstacleY have some duplicates. You will need to simplify them first. After that you will see a very little code duplication in the implementation. Below is my code sample:

template <typename T>
std::vector<T> cal(Location<T> &robot, T angleBetweenSensors) {
    const size_t N = robot.sensorReadings.size();
    std::vector<T> results(2 * N, 0);

    // Precompute this value to improve performance.
    constexpr T angleScale = M_PI / 180;
    for (size_t i = 0; i < N; ++i) {
        // Main code
        T alpha = (robot.deg + i * angleBetweenSensors) * angleScale;
        T sensorVal = robot.sensorReadings.at(i);
        T obstacleX = robot.x + sensorVal * cos(alpha);
        T obstacleY = robot.y + sensorVal * sin(alpha);

        // Use obstacleX and obstacleY here
        results[2 * i] = obstacleX;
        results[2 * i + 1] = obstacleY;
    }
    return results;
}