]> rtime.felk.cvut.cz Git - hubacji1/bcar.git/blob - src/bcar.cc
Add car frame coordinates computation
[hubacji1/bcar.git] / src / bcar.cc
1 #include <cmath>
2 #include "bcar.h"
3
4 // car frame
5 double BicycleCar::lfx()
6 {
7         double lfx = this->x();
8         lfx += (this->w() / 2) * cos(this->h() + M_PI / 2);
9         lfx += this->df() * cos(this->h());
10         lfx += this->sd() * cos(this->h());
11         return lfx;
12 }
13
14 double BicycleCar::lfy()
15 {
16         double lfy = this->y();
17         lfy += (this->w() / 2) * sin(this->h() + M_PI / 2);
18         lfy += this->df() * sin(this->h());
19         lfy += this->sd() * sin(this->h());
20         return lfy;
21 }
22
23 double BicycleCar::lrx()
24 {
25         double lrx = this->x();
26         lrx += (this->w() / 2) * cos(this->h() + M_PI / 2);
27         lrx += -this->dr() * cos(this->h());
28         lrx += -this->sd() * cos(this->h());
29         return lrx;
30 }
31
32 double BicycleCar::lry()
33 {
34         double lry = this->y();
35         lry += (this->w() / 2) * sin(this->h() + M_PI / 2);
36         lry += -this->dr() * sin(this->h());
37         lry += -this->sd() * sin(this->h());
38         return lry;
39 }
40
41 double BicycleCar::rrx()
42 {
43         double rrx = this->x();
44         rrx += (this->w() / 2) * cos(this->h() - M_PI / 2);
45         rrx += -this->dr() * cos(this->h());
46         rrx += -this->sd() * cos(this->h());
47         return rrx;
48 }
49
50 double BicycleCar::rry()
51 {
52         double rry = this->y();
53         rry += (this->w() / 2) * sin(this->h() - M_PI / 2);
54         rry += -this->dr() * sin(this->h());
55         rry += -this->sd() * sin(this->h());
56         return rry;
57 }
58
59 double BicycleCar::rfx()
60 {
61         double rfx = this->x();
62         rfx += (this->w() / 2) * cos(this->h() - M_PI / 2);
63         rfx += this->df() * cos(this->h());
64         rfx += this->sd() * cos(this->h());
65         return rfx;
66 }
67
68 double BicycleCar::rfy()
69 {
70         double rfy = this->y();
71         rfy += (this->w() / 2) * sin(this->h() - M_PI / 2);
72         rfy += this->df() * sin(this->h());
73         rfy += this->sd() * sin(this->h());
74         return rfy;
75 }