Geek Answers Handbook

How to break this formula into a function in C ++?

My code uses a formula to find where my robot sonar sensors found an obstacle. The code is as follows:

 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 this a function, so as not to 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 the formula for X and Y is almost the same, only the difference is that the person uses the sine, and the second uses the cosine. Should I then create two almost identical functions, or can this be done in one?

+4
source share
7 answers

Available options:

(1) Use the 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)

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;
}

- . vec2D .

(3) : . UNSAFE!!!

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

(4) , C- , , OOP-esque: . ( )

void calculateObstaclePosition(double robotX, double robotY, ..., double* outX, double* outY)
{
   double angle = ...;
   *outX = robotX + ...
   *outY = robotY + ...;
}
+3
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);
}

? .

+3

:

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)
    };
}


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

, , x, y, sensorReadings.at(i) robot.deg , . , , , return.

, x y . .x .y ,

 struct Coords{double x, doubley};

std::array.

+1

IMHO , , :

// 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);
}
0

, sin cos.

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

, sin cos

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


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

, . . , sin cos X Y.

X Y :

typedef enum { X, Y } xy_enum;

:

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); }
};

, sin cos X Y. , xcys() X cos Y sin. xsyc() X sin Y 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); }


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;

:

1 0
~0 1
0 1
1 ~0

, :

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);
}

:

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

x 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);
0

The formulas for calculating obstacles X and obstacles Y have some duplicates. You will first need to simplify them. After that, you will see very little code duplication in the implementation. The following is an example of my code:

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;
}
0
source

More articles:


All Articles