File: vec2.C

package info (click to toggle)
xjig 2.4-14.1
  • links: PTS
  • area: main
  • in suites: bullseye, buster, sid
  • size: 892 kB
  • sloc: cpp: 10,177; makefile: 1,142; perl: 23
file content (138 lines) | stat: -rw-r--r-- 3,079 bytes parent folder | download | duplicates (9)
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
125
126
127
128
129
130
131
132
133
134
135
136
137
138
#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;
Real	dx=d.X()-X();
Real	dy=d.Y()-Y();
Real	fdx=fabs(dx);
Real	fdy=fabs(dy);

	if (fdx>fdy) {
		if (fdx>1e-10)		erg = atan( -dy/dx );
		else					erg = (dy<0)?M_PI_2:3*M_PI_2;		// Fehler behoben ???

		if (dx<0)			erg+= M_PI;
	}
	else {
		if (fdy>1e-10)		erg = atan( dx/dy );
		else					erg = (dx>0)?M_PI_2:3*M_PI_2;		// Fehler behoben ???

		if (dy<0)			erg+= M_PI;
		erg-=M_PI_2;
	}
	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