// TorwartStellen.cpp: Implementierung der Verhaltens-Klasse TorwartStellen.
////////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include 
namespace RobotOmniVerhalten {

TorwartStellen::TorwartStellen(RobotOmniEbene0 *e, SpielEbene0* s, VerhaltenManager& vm)
		:Verhalten( "TorwartStellen", vm),
		s_winkelToleranz("WinkelToleranz",sensorManager),
		s_positionsToleranz("PositionsToleranz", sensorManager),
		s_torlinienAbstand("AbstandZurTorlinie", sensorManager),
		{
	this->e = e;
	this->s = s;
	s_winkelToleranz.set((RcMath::pi*5.f)/360.f);
	s_positionsToleranz.set(1.2f*cm);
	s_torlinienAbstand.set(15.f*cm);
}

TorwartStellen::~TorwartStellen(){}

float TorwartStellen::aktivierungsfunktion(){
	if(!e->s_isTorwart.get())
		return 0.f;
	
	///////////////////////////////////////////////
	// Wenn wir _weit_ ausserhalb unseres Strafraums, fahren wir durch TorwartE1 mit
 	// "normaler" Pfadplanung und Panikausweichen in unser Tor
	// (dort werden wir dann wieder zum Torwart)

	float seite = e->s_seite.get();
	Vec2f robbiPos = e->s_position.get();
	float robbiBreite = e->s_robbiBreite.get();
	float strafraumAnfang = s->s_strafraumAnfang.get();
	float halbeStrafraumBreite = s->s_halbeStrafraumBreite.get();
	bool xAusserhalb = robbiPos.x*seitehalbeStrafraumBreite+2.f*robbiBreite;
	if(xAusserhalb||yAusserhalb) // weit außerhalb des Strafraums
		return 0.f;
	else
		return 1.f;
	}

void TorwartStellen::zielfunktion(){
	////////////////////////////////////////////////////////////////	
	// Holen von Sensorwerten und Berechnen eigener Variablen	
	////////////////////////////////////////////////////////////////	
	// Seite ist eigene (da wo Torwart steht) Seite (-1 oder 1)	
	const float seite = e->s_seite.get();	
	// x ist Länge, y ist Breite	
	const Vec2f& halbeFeldGroesse = 0.5f * s->s_feldGroesse.get();	
	// Ist global Position auf dem Feld	
	const Vec2f& ballPos = s->s_ballPos.get();	
	// Ist global Position auf dem Feld	
	const Vec2f& robbiPos = e->s_position.get();	
	// Ist globaler Orientierungsvektor auf dem Feld	
	const Vec2f& robbiOr = e->s_orientierung.get();	
	// Ist der Radius	
	const float halbeRobbiBreite = 0.5f * e->s_robbiBreite.get();	
	// Der Strafraum geht in y-Richtung von -halbeStrafraumBreite bis halbeStrafraumBreite 	
	const float halbeStrafraumBreite  = s->s_halbeStrafraumBreite.get();	
	// Strecke vom Anstoßpunkt (0,0) zum Strafraum (strafraumAnfang,0) bzw (-strafraumAnfang,0)  	
	const float strafraumAnfang = s->s_strafraumAnfang.get();	
	// y ist Torbreite, x ist Tortiefe (Torlinie bis Torwand)	
	const Vecf& torGroesse = s->s_torGroesse.get();	

	////////////////////////////////////////////////////////////////
	// Roboter soll sich zwischen die mittlere Torwandposition und
 	// den Ball stellen und dabei die Torlinie leicht berühren.
  	////////////////////////////////////////////////////////////////

	// Position zum Stellen berechnen
	vec2f mittlere_Torwandposition = new vec2f(seite*(torGroesse.x + 1), 0);

	// Der Roboter soll sich zum Ball ausrichten
	vec2f targetposdir = (ballPos - mittlere_Torwandposition).normalize;

	// Berechnung der x-Position des Roboters
	float x = torGroesse.x + s_torlinienAbstand.get();
	float scl = x/targetposdir.x;

	// Nun kann die Position berechnet werden, indem der Richtungsvector mit scl multipliziert wird.
	vec2f targetPos = mittlere_Torwandposition + targetposdir * scl;
	
	// Wenn die Position ausserhalb des Tores ist, soll der Roboter
 	// am Pfosten stehenbleiben.
	targetPos.y = rangeCut(-torGroesse.y/2+halbeRobbiBreite, targetPos.y, torGroesse.y/2-halbeRobbiBreite);
	
	
	// Umrechnen in lokales Koordinatensystem
	// Roboter guckt in +y-Richtung
	vec2f dist = targetPos- robbiPos;
	
	Vec2f localFahrGeschw = (dist.len() > s_positionsToleranz. get() ?global2lcal(robbiOr, dist):vec2f(0,0));
	// Roboter dreht bei +1 gegen UZS und bei -1 im UZS
	float a = vwinkel(robbiOr,targetposdir);
	float localDrehGeschw = (a > s_winkelToleranz.get() ? -1: (a < -s_winkelToleranz.get() ? 1: 0));
	
	// Setzen der Aktoren
 	e->a_fahrGeschw.change(this, localFahrGeschw);
	e->a_drehGeschw.change(this, localDrehGeschw);
	e->a_schuss.change(this, false);
	}
}

    Source: geocities.com/de/liwicki_uni/robotik

               ( geocities.com/de/liwicki_uni)                   ( geocities.com/de)