title: Sorting RHS/LHS Objects in Vehicle Path C++
body: <p>So I currently am trying to create some method which when taking in a simulation vehicles position, direction, and an objects position, Will determine whether or not the object lies on the right and side or left hand side of that vehicles direction. An image will be shown here,<a href="https://i.stack.imgur.com/42VZI.png" rel="nofollow noreferrer">Simple Diagram of Problem Situation</a></p>
  <p>So far I have tried to use the cross product and some other methods to solve the problem i will include relevant code blocks here:</p>
  <pre class="lang-cpp prettyprint-override"><code>void Class::sortCones()
  {
      // Clearing both _lhsCones and _rhsCones vectors
      _rhsCones.clear();
      _lhsCones.clear();
  
      for (int i =0; i &lt; _cones.size(); i++)
      {
  
          if (indicateSide(_x, _y, _cones[i].x(), _cones[i].y(), _yaw) &gt; 0)
          {
              _lhsCones.push_back(_cones[i]);
          }
          if (indicateSide(_x, _y, _cones[i].x(), _cones[i].y(), _yaw) == 0)
          {
              return;
          }
          else
          {
              _rhsCones.push_back(_cones[i]);
          }
      }
      
      return;
  }
  
  
  double Class::indicateSide(double xCar, double yCar, double xCone, double yCone, double yawCar)
  {
  
      // Compute the i and j compoents of the yaw measurment as a unit vector i.e Vector Mag = 1
      double iOne = cos(yawCar);
      double jOne = sin(yawCar);
  
      // Create the Car to Cone Vector
      double iTwo = xCone - xCar;
      double jTwo = yCone - yCar;
  
      //ensure to normalise the vCar to Cone Vector
      double magTwo = std::sqrt(std::pow(iTwo, 2) + std::pow(jTwo, 2));
      iTwo = iTwo / magTwo;
      jTwo = jTwo / magTwo;
  
      // - old method
      // Using the transformation Matrix with Theta = yaw (angle in radians) transform the axis to the augmented 2D space
      // Take the Cross Product of &lt; Ex, 0 &gt; x &lt; x', y' &gt; where x', y' have the same location in the simulation space.
      // double Ex = cos(yawCar)*iOne - sin(yawCar)*jOne;
      // double Ey = sin(yawCar)*iOne + cos(yawCar)*jOne;
  
   
      double result = iOne*jTwo - jOne*iTwo;
  
      return result;
  }
  </code></pre>
  <p>The car currently just seems to run off in a straight line and one of the funny elements is the sorting method of left and right any direction is GREATLY appreciated.</p>