//#if 0
#include "vehicle.h"

Point cartoworld(Point P, CarState car)
{
    Point ins;
    double x_vehicle, y_vehicle;
    double theta = atan2(P.x, P.y);
    double distance = sqrt(P.x * P.x + P.y * P.y);
    double angle = (car.ins_heading_angle / 180 * PI + theta);
    x_vehicle = car.x + distance * sin(angle);
    y_vehicle = car.y + distance * cos(angle);
    ins.x = x_vehicle;
    ins.y = y_vehicle;
    return ins;
}
//#endif