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
76 if (d.X()!=X()) erg = atan( (Y()-d.Y())/(d.X()-X()) );
77 else erg = (d.Y()<Y())?M_PI_2:3*M_PI_2; // Fehler behoben ???
78 if (d.X()<X()) erg+= M_PI;
79 if (erg<RealZero) erg+= 2*M_PI;
80 return erg;
81 }
82
TurnAngleRad(const Real & angle)83 Vec2 Vec2::TurnAngleRad( const Real &angle ) const
84 {
85 if (!IsZero()) {
86 Real len = Norm();
87 Real ang = Vec2Zero.AngleRadial(*this) + angle;
88
89 return Vec2( len*cos(ang), -len*sin(ang) );
90 }
91 else return *this;
92 }
93
94 //
95 // L�sung des Gleichungssystems: p1+t1*d1 = p2+t2*d2
96 // nach den beiden "Zeiten" t1 und t2
97 //
Solve(const Vec2 & p1,const Vec2 & d1,const Vec2 & p2,const Vec2 & d2,Real * t1)98 int Vec2::Solve( const Vec2 &p1, const Vec2 &d1,
99 const Vec2 &p2, const Vec2 &d2, Real *t1 )
100 {
101 if (d1.X()!=RealZero) {
102 Real div = d2.Y()-d2.X()/d1.X()*d1.Y();
103 if (div==RealZero) { *t1=RealZero; return 1; } // parallel
104 *t1 = ( p1.Y()-p2.Y()+
105 (p2.X()-p1.X())/d1.X()*d1.Y() )
106 / div;
107 }
108 else {
109 Real div = d2.X() /* -d2.Y()/d1.Y()*d1.X() */;
110 if (div==RealZero) { *t1=RealZero; return 1; } // parallel
111 *t1 = ( p1.X()-p2.X()
112 /* + (p2.Y()-p1.Y())/d1.Y()*d1.X() */ )
113 / div;
114 }
115 return 0; // Ergebnis ok.
116 }
117
118 #if (0)
Project(const Vec2 & p1,const Vec2 & d1,const Vec2 & p2,Real * t1)119 int Vec2::Project( const Vec2 &p1, const Vec2 &d1,
120 const Vec2 &p2, Real *t1 )
121 {
122 return Solve(p1,d1,p2,d1.TurnLeft(),t1);
123 }
124 #endif
125