| 1234567891011121314151617 |
- //#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
|
