Using The Dot Product to determine whether an object is on the left hand side or right hand side of the direction of the object

Question

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. This is what i have implemented so far (Note I am in a 2D co-ord system):

This is the code block that uses the method

void Class::leftOrRight()
{
    // Clearing both _lhsCones and _rhsCones vectors
    _rhsCones.clear();
    _lhsCones.clear();

    for (int i =0; i < _cones.size(); i++)
    {
        if (dotAngleFromYaw(_x, _y, _cones[i].x(), _cones[i].y(), _yaw) > 0)
        {
            _lhsCones.push_back(_cones[i]);
        }
        else
        {
            _rhsCones.push_back(_cones[i]);
        }
    }

    return;
}

This is the code block which computes the angle

double Class::dotAngleFromYaw(double xCar, double yCar, double xCone, double yCone, double yawCar)
{


    double iOne = cos(yawCar);
    double jOne = sin(yawCar);

    double iTwo = xCone - xCar;
    double jTwo = yCone - yCar;

    //ensure to normalise the vector two
    double magTwo = std::sqrt(std::pow(iTwo, 2) + std::pow(jTwo, 2));
    iTwo = iTwo / magTwo;
    jTwo = jTwo / magTwo;

    double theta = acos((iOne * iTwo) + (jOne * jTwo));  // in radians

    return theta;
}

My issue with this is that dotAngleFromYaw(0,0,0,1,0) = +pi/2 and dotAngleFromYaw(0,0,0,-1,0) = +pi/2 hence the if statements fail to sort the cones.

Any help would be great

*Adjustments made from comment suggestions

I have change the sort method as follows

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;

    // // Using the transformation Matrix with Theta = yaw (angle in radians) transform the axis to the augmented 2D space
    // double Ex = cos(yawCar)*iOne - sin(yawCar)*jOne;
    // double Ey = sin(yawCar)*iOne + cos(yawCar)*jOne;

    // Take the Cross Product of < Ex, 0 > x < x', y' > where x', y' have the same location in the simulation space.
    double result = iOne*jTwo - jOne*iTwo;

    return result;
}

However I still am having issues defining the left and right, note that I have also become aware that objects behind the vehicle are still passed to every instance of the array of objects to be evaluated and hence I have implemented a dot product check elsewhere that seems to work fine for now, which is why I have not included it here I can make another adjustment to the post to include said code. I did try to implement the co-ordinate system transformation however i did not see improvements compared to when the added lines are not commented out and implemented.

Any further feedback is greatly appreciated

Answer 1

If the angle does not matter and you only want to know whether "left or right" I'd go for another approach. Set up a plane that has xCar and yCar on its surface. When setting it up it's up to you how to define the plane's normal i.e. the side its facing to.

After that you can apply the dot-product to determine the 'sign' indicating which side it's on.

Answer 2

Note that dot product does not provide information about left/right position.

Sign of dot product says whether position is ahead or backward.

To get left/right side, you need to check sign of cross product

cross = iOne * jTwo - jOne * iTwo

(note subtraction and i/j alternation)

To see the difference between dot and cross product info:

Quick test. Mathematical coordinate system (CCW) is used (left/right depends on CW/CCW)

BTW, in kinematics simulations it is worth to store components of direction vector rather than angle.

#define _USE_MATH_DEFINES // для C++
#include <cmath>
#include <iostream>

void check_target(float carx, float cary, float dirx, float diry, float tx, float ty) {
    float cross = (tx - carx) * diry - (ty - cary) * dirx;
    float dot = (tx - carx) * dirx + (ty - cary) * diry;

    if (cross >= 0) {
        if (dot >= 0)
            std::cout << "ahead right\n";
        else
            std::cout << "behind right\n";
    }
    else {
        if (dot >= 0)
            std::cout << "ahead left\n";
        else
            std::cout << "behind left\n";
    }
}

int main()
{
    float carx, cary, car_dir_angle, dirx, diry;
    float tx, ty;

    carx = 1;
    cary = 1;
    car_dir_angle = M_PI / 4;
    dirx = cos(car_dir_angle);
    diry = sin(car_dir_angle);

    check_target(carx, cary, dirx, diry, 2, 3);
    check_target(carx, cary, dirx, diry, 2, 1);
    check_target(carx, cary, dirx, diry, 1, 0);
    check_target(carx, cary, dirx, diry, 0, 1);
}