1 #ifndef _global_h
2 # include "global.h"
3 #endif
4
5 #ifndef _vec2_h
6 # include "vec2.h"
7 #endif
8
9 Vec2 Vec2Zero( RealZero, RealZero );
10
11 //
12 // Aufsplittung des Vektors in 2 Vektoren parallel und vertikal zum
13 // Richtungsvektor d. Dazu wird folgendes Gleichungssytem gel�st:
14 //
15 // vx dx -dy
16 // ( ) = l *( ) + u *( )
17 // vy dy dx
18 //
19 // V() = x + y
20 //
Split(const Vec2 & d,Vec2 * vx,Vec2 * vy)21 void Vec2::Split( const Vec2 &d, Vec2 *vx, Vec2 *vy ) const
22 {
23 Real l,u;
24
25 if (d.Y()!=RealZero) {
26 l = ( Y() + ( X() * d.X() / d.Y() ) )
27 / ( d.Y() + ( d.X() * d.X() / d.Y() ) );
28 u = ( l * d.X() - X() ) / d.Y();
29 *vx = Vec2( l* d.X() , l*d.Y() );
30 *vy = Vec2( u*(-d.Y()), u*d.X() );
31 }
32 else {
33 if (d.X()!=RealZero) {
34 *vx = Vec2( X(), RealZero ); // parallel zur X-Achse
35 *vy = Vec2(RealZero, Y() );
36 }
37 else {
38 *vx = *this; // keine Richtung -> gesamter Vektor ist x
39 *vy = Vec2Zero;
40 }
41 }
42 }
43
44 //
45 // Analog zur kompletten Aufsplittung wird in der folgenden
46 // Version von split nur der parallele Anteil zur�ckgeliefert.
47 //
Split(const Vec2 & d,Vec2 * vx)48 void Vec2::Split( const Vec2 &d, Vec2 *vx ) const
49 {
50 Real l;
51
52 if (d.Y()!=RealZero) {
53 l = ( Y() + ( X() * d.X() / d.Y() ) )
54 / ( d.Y() + ( d.X() * d.X() / d.Y() ) );
55 *vx = Vec2( l* d.X() , l*d.Y() );
56 }
57 else {
58 if (d.X()!=RealZero) {
59 *vx = Vec2( X(), RealZero ); // parallel zur X-Achse
60 }
61 else {
62 *vx = *this; // keine Richtung -> gesamter Vektor ist x
63 }
64 }
65 }
66
67
68 //
69 // Berechnung des Winkels, den der angegebene Punkt zur aktuellen Korrdinate
70 // hat. Ergebnis liegt zwischen 0 und 2*M_PI
71 //
AngleRadial(const Vec2 & d)72 Real Vec2::AngleRadial( const Vec2 &d ) const
73 {
74 Real erg;
75 Real dx=d.X()-X();
76 Real dy=d.Y()-Y();
77 Real fdx=fabs(dx);
78 Real fdy=fabs(dy);
79
80 if (fdx>fdy) {
81 if (fdx>1e-10) erg = atan( -dy/dx );
82 else erg = (dy<0)?M_PI_2:3*M_PI_2; // Fehler behoben ???
83
84 if (dx<0) erg+= M_PI;
85 }
86 else {
87 if (fdy>1e-10) erg = atan( dx/dy );
88 else erg = (dx>0)?M_PI_2:3*M_PI_2; // Fehler behoben ???
89
90 if (dy<0) erg+= M_PI;
91 erg-=M_PI_2;
92 }
93 if (erg<RealZero) erg+= 2*M_PI;
94 return erg;
95 }
96
TurnAngleRad(const Real & angle)97 Vec2 Vec2::TurnAngleRad( const Real &angle ) const
98 {
99 if (!IsZero()) {
100 Real len = Norm();
101 Real ang = Vec2Zero.AngleRadial(*this) + angle;
102
103 return Vec2( len*cos(ang), -len*sin(ang) );
104 }
105 else return *this;
106 }
107
108 //
109 // L�sung des Gleichungssystems: p1+t1*d1 = p2+t2*d2
110 // nach den beiden "Zeiten" t1 und t2
111 //
Solve(const Vec2 & p1,const Vec2 & d1,const Vec2 & p2,const Vec2 & d2,Real * t1)112 int Vec2::Solve( const Vec2 &p1, const Vec2 &d1,
113 const Vec2 &p2, const Vec2 &d2, Real *t1 )
114 {
115 if (d1.X()!=RealZero) {
116 Real div = d2.Y()-d2.X()/d1.X()*d1.Y();
117 if (div==RealZero) { *t1=RealZero; return 1; } // parallel
118 *t1 = ( p1.Y()-p2.Y()+
119 (p2.X()-p1.X())/d1.X()*d1.Y() )
120 / div;
121 }
122 else {
123 Real div = d2.X() /* -d2.Y()/d1.Y()*d1.X() */;
124 if (div==RealZero) { *t1=RealZero; return 1; } // parallel
125 *t1 = ( p1.X()-p2.X()
126 /* + (p2.Y()-p1.Y())/d1.Y()*d1.X() */ )
127 / div;
128 }
129 return 0; // Ergebnis ok.
130 }
131
132 #if (0)
Project(const Vec2 & p1,const Vec2 & d1,const Vec2 & p2,Real * t1)133 int Vec2::Project( const Vec2 &p1, const Vec2 &d1,
134 const Vec2 &p2, Real *t1 )
135 {
136 return Solve(p1,d1,p2,d1.TurnLeft(),t1);
137 }
138 #endif
139