File: vec2.C

package info (click to toggle)
flying 6.20-6
  • links: PTS
  • area: main
  • in suites: etch, etch-m68k, lenny
  • size: 752 kB
  • ctags: 1,873
  • sloc: cpp: 10,966; makefile: 189
file content (124 lines) | stat: -rw-r--r-- 2,849 bytes parent folder | download | duplicates (4)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
#ifndef _global_h
#	include "global.h"
#endif

#ifndef _vec2_h
#	include "vec2.h"
#endif

Vec2 Vec2Zero( RealZero, RealZero );

//
// Aufsplittung des Vektors in 2 Vektoren parallel und vertikal zum
// Richtungsvektor d. Dazu wird folgendes Gleichungssytem gel�st:
//
//	   vx        dx        -dy
//	  (  ) =	l *(  ) + u *(   )
// 	vy        dy         dx
//
//     V()  =   x     +   y
//
void Vec2::Split( const Vec2 &d, Vec2 *vx, Vec2 *vy ) const
{
Real	l,u;

	if (d.Y()!=RealZero) {
		l  = (   Y() + (   X() * d.X() / d.Y() ) )
			/ ( d.Y() + ( d.X() * d.X() / d.Y() ) );
		u  = ( l * d.X() - X() ) / d.Y();
		*vx = Vec2( l*  d.X() , l*d.Y() );
		*vy = Vec2( u*(-d.Y()), u*d.X() );
	}
	else {
		if (d.X()!=RealZero) {
			*vx = Vec2( X(), RealZero );		// parallel zur X-Achse
			*vy = Vec2(RealZero, Y() );
		}
		else {
			*vx = *this;					// keine Richtung -> gesamter Vektor ist x
			*vy = Vec2Zero;
		}
	}
}

//
// Analog zur kompletten Aufsplittung wird in der folgenden
// Version von split nur der parallele Anteil zur�ckgeliefert.
//
void Vec2::Split( const Vec2 &d, Vec2 *vx ) const
{
Real	l;

	if (d.Y()!=RealZero) {
		l  = (   Y() + (   X() * d.X() / d.Y() ) )
			/ ( d.Y() + ( d.X() * d.X() / d.Y() ) );
		*vx = Vec2( l*  d.X() , l*d.Y() );
	}
	else {
		if (d.X()!=RealZero) {
			*vx = Vec2( X(), RealZero );		// parallel zur X-Achse
		}
		else {
			*vx = *this;					// keine Richtung -> gesamter Vektor ist x
		}
	}
}


//
// Berechnung des Winkels, den der angegebene Punkt zur aktuellen Korrdinate
// hat. Ergebnis liegt zwischen 0 und 2*M_PI
//
Real Vec2::AngleRadial( const Vec2 &d ) const
{
Real	erg;

	if (d.X()!=X())	erg = atan( (Y()-d.Y())/(d.X()-X()) );
	else					erg = (d.Y()<Y())?M_PI_2:3*M_PI_2;		// Fehler behoben ???
	if (d.X()<X())		erg+= M_PI;
	if (erg<RealZero)	erg+= 2*M_PI;
	return erg;
}

Vec2 Vec2::TurnAngleRad( const Real &angle ) const
{
	if (!IsZero()) {
			Real	len = Norm();
			Real	ang = Vec2Zero.AngleRadial(*this) + angle;

			return Vec2( len*cos(ang), -len*sin(ang) );
	}
	else	return *this;
}

//
// L�sung des Gleichungssystems:	p1+t1*d1 = p2+t2*d2
// nach den beiden "Zeiten" t1 und t2
//
int Vec2::Solve(	const Vec2 &p1, const Vec2 &d1,
							const Vec2 &p2, const Vec2 &d2, Real *t1 )
{
		if (d1.X()!=RealZero) {
			Real div = d2.Y()-d2.X()/d1.X()*d1.Y();
			if (div==RealZero)		{ *t1=RealZero; return 1; }		// parallel
			*t1 = ( p1.Y()-p2.Y()+
					(p2.X()-p1.X())/d1.X()*d1.Y() )
					/ div;
		}
		else {
			Real div = d2.X()	/* -d2.Y()/d1.Y()*d1.X() */;
			if (div==RealZero)		{ *t1=RealZero; return 1; }		// parallel
			*t1 = ( p1.X()-p2.X()
					/* + (p2.Y()-p1.Y())/d1.Y()*d1.X() */ )
					/ div;
		}
		return 0;											// Ergebnis ok.
}

#if (0)
int Vec2::Project( const Vec2 &p1, const Vec2 &d1,
							const Vec2 &p2, Real *t1 )
{
	return Solve(p1,d1,p2,d1.TurnLeft(),t1);
}
#endif