//Fonction TurnOnRight-------------------------------------------------------------faux
void TurnOnRight(int delay)
{
OCR1A = RIGHT; //on change la fréquence de découpage du timer pour que l'interprétation faite par lesecond ATMEGA fasse que le gouvernail tourne à droite
Rotate(delay);
}
void RotatedComponent::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
Inherited::changed(whichField, origin, details);
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if(whichField & InternalComponentFieldMask)
{
clearChildren();
if(getInternalComponent() != NULL)
{
pushToChildren(getInternalComponent());
}
}
if((whichField & InternalComponentFieldMask) ||
(whichField & AngleFieldMask) ||
(whichField & ResizePolicyFieldMask))
{
if(getInternalComponent() != NULL)
{
Vec2f Size;
switch (static_cast<ResizePolicy>(getResizePolicy()))
{
case RESIZE_TO_MIN:
{
//Treat TopLeft as 0,0
//Get the Rotated Bounds of the Internal Component
Pnt2f p1,p2,p3,p4;
Pnt2f ComponentCenter(static_cast<Real32>(getInternalComponent()->getSize().x())/2.0, static_cast<Real32>(getInternalComponent()->getSize().y())/2.0);
p1 = -ComponentCenter;
p2.setValues(ComponentCenter.x(),-ComponentCenter.y());
p3 = ComponentCenter;
p4.setValues(-ComponentCenter.x(),ComponentCenter.y());
//Rotate them
p1 = Rotate(p1, -getAngle());
p2 = Rotate(p2, -getAngle());
p3 = Rotate(p3, -getAngle());
p4 = Rotate(p4, -getAngle());
//Get their min and max
Pnt2f Min(osgMin(osgMin(osgMin(p1.x(),p2.x()),p3.x()),p4.x()),
osgMin(osgMin(osgMin(p1.y(),p2.y()),p3.y()),p4.y()));
Pnt2f Max(osgMax(osgMax(osgMax(p1.x(),p2.x()),p3.x()),p4.x()),
osgMax(osgMax(osgMax(p1.y(),p2.y()),p3.y()),p4.y()));
Pnt2f BorderTopLeft, BorderBottomRight;
getInsideInsetsBounds(BorderTopLeft, BorderBottomRight);
Pnt2f TopLeft, BottomRight;
getBounds(TopLeft, BottomRight);
setPreferredSize(Vec2f(Max.x() - Min.x() + (BorderTopLeft.x() - TopLeft.x()) + (BottomRight.x() - BorderBottomRight.x()),
Max.y() - Min.y() + (BorderTopLeft.y() - TopLeft.y()) + (BottomRight.y() - BorderBottomRight.y())));
break;
}
case RESIZE_TO_MAX:
{
//Get the Internal Components Center
Pnt2f ComponentCenter(static_cast<Real32>(getInternalComponent()->getSize().x())/2.0, static_cast<Real32>(getInternalComponent()->getSize().y())/2.0);
//Get the distance from the Center to one of the TopLeft Corner
Real32 Length = 2*ComponentCenter.dist(Pnt2f(0,0));
setPreferredSize(Vec2f(Length,Length));
break;
}
case NO_RESIZING:
default:
break;
}
getInternalComponent()->updateClipBounds();
//Check the Mouse
if( getParentWindow() != NULL &&
getParentWindow()->getParentDrawingSurface() != NULL &&
getParentWindow()->getParentDrawingSurface()->getEventProducer() != NULL)
{
Pnt2f MouseLoc(getParentWindow()->getParentDrawingSurface()->getEventProducer()->getMousePosition());
MouseEventDetailsUnrecPtr e = MouseEventDetails::create(getParentWindow()->getParentDrawingSurface()->getEventProducer(),getSystemTime(),MouseEventDetails::NO_BUTTON,0,MouseLoc, NULL);
checkMouseEnterExit(e,e->getLocation(),getInternalComponent(),getInternalComponent()->isContained(MouseLoc, true),e->getViewport());
}
}
}
}
void Asteroid::tick(double DT)
{
Translate((motionProperties.speed * motionProperties.direction) * static_cast<float>(DT));
Rotate(motionProperties.angularSpeed * static_cast<float>(DT) * motionProperties.rotation);
}
void Matrix::Rotate(double angle, int Axis)
{ // Rotation (Axis 1 = x , 2 = y , 3 = z
Rotate(sin(angle), cos(angle), Axis);
}
SceneGraphManager *createGraph()
{
int lightIndex = SINGLETONINSTANCE(LightingManager)->getAvailableLightIndex();
SINGLETONINSTANCE(LightingManager)->lights[lightIndex].enable(true);
SINGLETONINSTANCE(LightingManager)->lights[lightIndex].setDiffuse(0.5f,0.5f,0.5f, 1.0f);
SINGLETONINSTANCE(LightingManager)->lights[lightIndex].setSpecular(0.7f,0.2f,0.1f, 1.0f);
SINGLETONINSTANCE(LightingManager)->lights[lightIndex].setAmbient(0.2f,0.2f,0.2f, 1.0f);
SINGLETONINSTANCE(LightingManager)->lights[lightIndex].setPos(10.0f,10.0f,10.0f);
auto root = new Object();
root->Name = "Root";
root->transformation->Reset();
float mapWidth = 40.0f;
auto ground = new Object();
ground->Name = "Ground";
ground->model = SINGLETONINSTANCE( MediaManager )->ground;
ground->transformation->Reset();
ground->SetPos2D(20.0f,20.0f);
ground->SetScale(mapWidth,mapWidth,1.0f);
SINGLETONINSTANCE(PathPlanner)->StartUp(mapWidth);
auto underground = new Object();
underground->Name = "Underground";
underground->model = SINGLETONINSTANCE( MediaManager )->ground;
underground->transformation->Reset();
underground->getPos()->SetZ(-10.0f);
underground->SetScale(mapWidth*10,mapWidth*10,1.0f);
Rectangle physicsBox(Point(-0.5f, -0.5f), 1.0f, 1.0f);
auto box = new Object();
box->Name = "Box";
box->model = SINGLETONINSTANCE( MediaManager )->boxModel;
StaticObjectModel* tempStaticObject = new StaticObjectModel(RECTANGULARSHAPE);
box->physicsModel = tempStaticObject;
box->physicsModel->InitializeAsRectangle(physicsBox);
SINGLETONINSTANCE(PhysicsSystem)->AddStaticObject(tempStaticObject);
box->SetPos2D(20.0f, 0.0f);
box->SetScale(40.0f, 1.0f, 3.0f);
//box->physicsModel->debug();
auto box1 = new Object();
box1->Name = "Box1";
tempStaticObject = new StaticObjectModel(RECTANGULARSHAPE);
box1->model = SINGLETONINSTANCE( MediaManager )->boxModel;
box1->physicsModel = tempStaticObject;
box1->physicsModel->InitializeAsRectangle(physicsBox);
SINGLETONINSTANCE(PhysicsSystem)->AddStaticObject(tempStaticObject);
box1->SetPos2D(20.0f, 40.0f);
box1->SetScale(40.0f, 1.0f, 3.0f);
auto box2 = new Object();
box2->Name = "Box2";
box2->model = SINGLETONINSTANCE( MediaManager )->boxModel;
tempStaticObject = new StaticObjectModel(RECTANGULARSHAPE);
box2->physicsModel = tempStaticObject;
box2->physicsModel->InitializeAsRectangle(physicsBox);
SINGLETONINSTANCE(PhysicsSystem)->AddStaticObject(tempStaticObject);
box2->SetPos2D(0.0f, 20.0f);
box2->SetScale(1.0f, 40.0f, 3.0f);
auto box3 = new Object();
box3->Name = "Box3";
box3->model = SINGLETONINSTANCE( MediaManager )->boxModel;
tempStaticObject = new StaticObjectModel(RECTANGULARSHAPE);
box3->physicsModel = tempStaticObject;
box3->physicsModel->InitializeAsRectangle(physicsBox);
SINGLETONINSTANCE(PhysicsSystem)->AddStaticObject(tempStaticObject);
box3->SetPos2D(40.0f, 20.0f);
box3->SetScale(1.0f, 40.0f, 3.0f);
auto box4 = new Object();
box4->Name = "Box4";
box4->model = SINGLETONINSTANCE( MediaManager )->boxModel;
tempStaticObject = new StaticObjectModel(RECTANGULARSHAPE);
box4->physicsModel = tempStaticObject;
box4->physicsModel->InitializeAsRectangle(physicsBox);
SINGLETONINSTANCE(PhysicsSystem)->AddStaticObject(tempStaticObject);
box4->SetPos2D(27.0f, 15.0f);
box4->SetScale(1.0f, 30.0f, 3.0f);
auto box5 = new Object();
box5->Name = "Box5";
box5->model = SINGLETONINSTANCE( MediaManager )->boxModel;
tempStaticObject = new StaticObjectModel(RECTANGULARSHAPE);
box5->physicsModel = tempStaticObject;
box5->physicsModel->InitializeAsRectangle(physicsBox);
SINGLETONINSTANCE(PhysicsSystem)->AddStaticObject(tempStaticObject);
box5->SetPos2D(13.0f, 25.0f);
box5->SetScale(1.0f, 30.0f, 3.0f);
root->children->push_back(*underground);
root->children->push_back(*ground);
root->children->push_back(*box);
root->children->push_back(*box1);
root->children->push_back(*box2);
root->children->push_back(*box3);
root->children->push_back(*box4);
root->children->push_back(*box5);
SINGLETONINSTANCE(PhysicsSystem)->SetStaticPathMap();
Point forward;
forward.X = 0;
forward.Y = -1;
forward = forward.GetNormalizedPoint();
auto player = new Object();
SINGLETONINSTANCE(PlayerInteraction)->StartUp(player);
player->Name = "Player";
player->model = SINGLETONINSTANCE( MediaManager )->crazyModel;
MovingObjectModel* tempMovingObject = new MovingObjectModel(CIRCULARSHAPE, PLAYERTYPE, forward, player);
player->physicsModel = tempMovingObject;
Circle circle(Point(0.0f,0.0f),0.5f);
player->physicsModel->InitializeAsCircle(circle);
SINGLETONINSTANCE(PhysicsSystem)->AddMovingObject(tempMovingObject);
player->SetPos2D(15,20);
player->Rotate(90.0f, 1.0f, 0.0f, 0.0f);
player->SetForward(0.0f, 1.0f);
player->setLookAt2D(forward.X,forward.Y);
root->children->push_back(*player);
player->physicsModel->SetTargetPosition(new Point(50,5));
auto camera = new Camera();
camera->Position.SetX(20);
camera->Position.SetY(5);
camera->Position.SetZ(50);
camera->LookAt.SetX(20);
camera->LookAt.SetY(20);
camera->LookAt.SetZ(-1);
camera->Up.SetX(0);
camera->Up.SetY(1);
camera->Up.SetZ(0);
SceneGraphManager *sceneGraph = new SceneGraphManager(camera, root);
return sceneGraph;
}
void HawkTransform::Rotate(HawkRect2D& oRect,const HawkVector2D& oRefPt,const HawkRadian& oRad)
{
Rotate(oRect.Point,oRefPt,oRad);
Rotate(oRect.Edge[0],HawkVector2D(0,0),oRad);
Rotate(oRect.Edge[1],HawkVector2D(0,0),oRad);
}
inline void RotateRel(const float RAngle, const float NormalX, const float NormalY, const float NormalZ)
{
Rotate(rangle+RAngle, NormalX, NormalY, NormalZ);
}
void Safety_Override(void)
{
/*
This function is intended to keep the lander from
crashing. It checks the sonar distance array,
if the distance to nearby solid surfaces and
uses thrusters to maintain a safe distance from
the ground unless the ground happens to be the
landing platform.
Additionally, it enforces a maximum speed limit
which when breached triggers an emergency brake
operation.
*/
/**************************************************
TO DO: Modify this function so that it can do its
work even if components or sensors
fail
**************************************************/
/**************************************************
How this works:
Check the sonar readings, for each sonar
reading that is below a minimum safety threshold
AND in the general direction of motion AND
not corresponding to the landing platform,
carry out speed corrections using the thrusters
**************************************************/
double DistLimit;
double Vmag;
double dmin;
// Establish distance threshold based on lander
// speed (we need more time to rectify direction
// at high speed)
Vmag=Velocity_X()*Velocity_X();
Vmag+=Velocity_Y()*Velocity_Y();
DistLimit=fmax(75,Vmag);
// If we're close to the landing platform, disable
// safety override (close to the landing platform
// the Control_Policy() should be trusted to
// safely land the craft)
if (fabs(PLAT_X-Position_X())<150&&fabs(PLAT_Y-Position_Y())<150) return;
// Determine the closest surfaces in the direction
// of motion. This is done by checking the sonar
// array in the quadrant corresponding to the
// ship's motion direction to find the entry
// with the smallest registered distance
// Horizontal direction.
dmin=1000000;
if (Velocity_X()>0)
{
for (int i=5;i<14;i++)
if (SONAR_DIST[i]>-1&&SONAR_DIST[i]<dmin) dmin=SONAR_DIST[i];
}
else
{
for (int i=22;i<32;i++)
if (SONAR_DIST[i]>-1&&SONAR_DIST[i]<dmin) dmin=SONAR_DIST[i];
}
// Determine whether we're too close for comfort. There is a reason
// to have this distance limit modulated by horizontal speed...
// what is it?
if (dmin<DistLimit*fmax(.25,fmin(fabs(Velocity_X())/5.0,1)))
{ // Too close to a surface in the horizontal direction
if (Angle()>1&&Angle()<359)
{
if (Angle()>=180) Rotate(360-Angle());
else Rotate(-Angle());
return;
}
if (Velocity_X()>0){
Right_Thruster(1.0);
Left_Thruster(0.0);
}
else
{
Left_Thruster(1.0);
Right_Thruster(0.0);
}
}
// Vertical direction
dmin=1000000;
if (Velocity_Y()>5) // Mind this! there is a reason for it...
{
for (int i=0; i<5; i++)
if (SONAR_DIST[i]>-1&&SONAR_DIST[i]<dmin) dmin=SONAR_DIST[i];
for (int i=32; i<36; i++)
if (SONAR_DIST[i]>-1&&SONAR_DIST[i]<dmin) dmin=SONAR_DIST[i];
}
else
{
for (int i=14; i<22; i++)
if (SONAR_DIST[i]>-1&&SONAR_DIST[i]<dmin) dmin=SONAR_DIST[i];
}
if (dmin<DistLimit) // Too close to a surface in the horizontal direction
{
if (Angle()>1||Angle()>359)
{
if (Angle()>=180) Rotate(360-Angle());
else Rotate(-Angle());
return;
}
if (Velocity_Y()>2.0){
Main_Thruster(0.0);
}
else
{
Main_Thruster(1.0);
}
}
}
int _stdcall Logistics::beginMission(void*, int, void*[])
{
if (MPlayer)
MPlayer->setMode(MULTIPLAYER_MODE_LOADING);
char commandersToLoad[MAX_MC_PLAYERS][3] = {{0, 0, 0}, {1, 1, 1}, {2, 2, 2}, {3, 3, 3}, {4, 4, 4}, {5, 5, 5}, {6, 6, 6}, {7, 7, 7}};
long missionLoadType = LogisticsData::instance->skipLogistics() ?
MISSION_LOAD_SP_QUICKSTART : MISSION_LOAD_SP_LOGISTICS;
if (MPlayer) {
//---------------------------
// Calc valid commanderIDs...
long curCommanderID = 0;
for (long CID = 0; CID < MAX_MC_PLAYERS; CID++) {
MPlayer->availableCIDs[CID] = true;
if (MPlayer->playerInfo[CID].player && (MPlayer->playerInfo[CID].commanderID > -1)) {
if (CID != curCommanderID) {
long oldCommanderID = CID;
MPlayer->playerInfo[CID].commanderID = curCommanderID;
memcpy(&MPlayer->playerInfo[curCommanderID], &MPlayer->playerInfo[CID], sizeof(MC2Player));
MPlayer->playerInfo[CID].player = NULL;
MPlayer->playerInfo[CID].commanderID = -1;
for (long j = 0; j < MAX_MC_PLAYERS; j++)
if (MPlayer->playerList[j].player == MPlayer->playerInfo[curCommanderID].player)
MPlayer->playerList[j].commanderID = curCommanderID;
if (oldCommanderID == MPlayer->commanderID)
MPlayer->commanderID = curCommanderID;
}
MPlayer->availableCIDs[curCommanderID] = false;
curCommanderID++;;
}
}
//----------------------
// Calc valid teamIDs...
long curTeamID = 0;
for (long teamID = 0; teamID < MAX_MC_PLAYERS; teamID++) {
bool teamFound = false;
for (long i = 0; i < MAX_MC_PLAYERS; i++)
if (MPlayer->playerInfo[i].player && (MPlayer->playerInfo[i].team == teamID)) {
MPlayer->playerInfo[i].team = curTeamID;
teamFound = true;
}
if (teamFound)
curTeamID++;
}
if (MPlayer->isHost()) {
// Determine drop zone order here...
char dropZoneList[8];
char hqs[MAX_TEAMS];
if (MPlayer->missionSettings.missionType == MISSION_TYPE_OTHER) {
bool goodToLoad = mission->calcComplexDropZones((char*)(const char*)LogisticsData::instance->getCurrentMission(), dropZoneList);
if (!goodToLoad)
STOP(("Logisitics.beginMission: teams do not match up for complex mission"));
for (long i = 0; i < MAX_TEAMS; i++)
hqs[i] = i;
}
else
MPlayer->calcDropZones(dropZoneList, hqs);
if (MPlayer->missionSettings.quickStart)
for (long i = 0; i < MAX_MC_PLAYERS; i++) {
MPlayer->commandersToLoad[i][0] = (long)dropZoneList[i]; //MPlayer->playerInfo[i].commanderID;
MPlayer->commandersToLoad[i][1] = (long)(dropZoneList[i] > -1) ? MPlayer->playerInfo[dropZoneList[i]].team : 0;
MPlayer->commandersToLoad[i][2] = hqs[i];
}
else
for (long i = 0; i < MAX_MC_PLAYERS; i++) {
MPlayer->commandersToLoad[i][0] = dropZoneList[i]; //-1;
MPlayer->commandersToLoad[i][1] = (dropZoneList[i] > -1) ? MPlayer->playerInfo[dropZoneList[i]].team : 0; //-1;
MPlayer->commandersToLoad[i][2] = hqs[i];
}
MPlayer->sendMissionSetup(0, 0, NULL);
}
if (!MPlayer->waitTillStartLoading()) {
// SERVER DROPPED
return(0);
}
if (MPlayer->commandersToLoad[0][0] < -1)
PAUSE(("Logistics.beginMission: bad commandersToLoad"));
for (long i = 0; i < MAX_MC_PLAYERS; i++) {
commandersToLoad[i][0] = MPlayer->commandersToLoad[i][0];
commandersToLoad[i][1] = MPlayer->commandersToLoad[i][1];
commandersToLoad[i][2] = MPlayer->commandersToLoad[i][2];
}
if (MPlayer->missionSettings.quickStart) {
MultiPlayTeamId = MPlayer->playerInfo[MPlayer->commanderID].team;
if (MultiPlayTeamId < 0)
STOP(("Bad commanderID"));
MultiPlayCommanderId = MPlayer->commanderID;
if (MultiPlayCommanderId < 0)
STOP(("Bad commanderID"));
missionLoadType = MISSION_LOAD_MP_QUICKSTART;
}
else {
MultiPlayTeamId = MPlayer->playerInfo[MPlayer->commanderID].team;
MultiPlayCommanderId = MPlayer->commanderID;
missionLoadType = MISSION_LOAD_MP_LOGISTICS;
}
long maxTeam = -1;
for (i = 0; i < MAX_MC_PLAYERS; i++)
if (MPlayer->playerInfo[i].team > maxTeam)
maxTeam = MPlayer->playerInfo[i].team;
MPlayer->numTeams = maxTeam + 1;
}
else if (missionLoadType == MISSION_LOAD_SP_LOGISTICS) {
commandersToLoad[0][0] = -1;
commandersToLoad[0][1] = -1;
commandersToLoad[0][2] = -1;
}
else {
commandersToLoad[0][0] = 0;
commandersToLoad[0][1] = 0;
commandersToLoad[0][2] = -1;
}
if (mission)
mission->destroy();
long numPlayers = 1;
if ( MPlayer )
MPlayer->getPlayers(numPlayers);
long numMoversPerCommander[MAX_MC_PLAYERS] = {12, 12, 12, 9, 7, 6, 5, 4};
Stuff::Vector3D dropZoneList[255]; // ubsurdly large, but sometimes we overrun this.
long dropZoneID = 0;
if (MPlayer) {
//dropZoneID = MPlayer->commanderID;
for (long i = 0; i < MAX_MC_PLAYERS; i++)
if (commandersToLoad[i][0] == MPlayer->commanderID) {
dropZoneID = i;
break;
}
useUnlimitedAmmo = MPlayer->missionSettings.unlimitedAmmo;
}
mission->init((char*)(const char*)LogisticsData::instance->getCurrentMission(), missionLoadType, dropZoneID, dropZoneList, commandersToLoad, numMoversPerCommander[numPlayers - 1]);
LogisticsData::instance->rpJustAdded = 0;
if (MPlayer) {
if (missionLoadType == MISSION_LOAD_MP_LOGISTICS) {
EList< LogisticsMech*, LogisticsMech* > list;
LogisticsData::instance->getForceGroup(list);
long dropZoneIndex = 0;
long numMechs = 0;
for (EList< LogisticsMech*, LogisticsMech* >::EIterator iter = list.Begin(); !iter.IsDone(); iter++) {
numMechs++;
if ( !(*iter)->getPilot() )
continue;
CompressedMech mechData;
mechData.lastMech = (list.Count() == numMechs);
mechData.objNumber = (*iter)->getFitID();
mechData.commanderID = MPlayer->commanderID;
mechData.baseColor = MPlayer->colors[MPlayer->playerInfo[MPlayer->commanderID].baseColor[BASECOLOR_TEAM]];
mechData.highlightColor1 = MPlayer->colors[MPlayer->playerInfo[MPlayer->commanderID].stripeColor];
mechData.highlightColor2 = MPlayer->colors[MPlayer->playerInfo[MPlayer->commanderID].stripeColor];
strcpy(mechData.pilotFile, (*iter)->getPilot()->getFileName());
strcpy(mechData.mechFile, (*iter)->getFileName());
strcpy(mechData.variantName, (*iter)->getName());
mechData.variantNum = (*iter)->getVariant()->getFileID();
mechData.cBills = (*iter)->getVariant()->getCost();
mechData.pos[0] = dropZoneList[dropZoneIndex].x;
mechData.pos[1] = dropZoneList[dropZoneIndex++].y;
mechData.designerMech = (*iter)->getVariant()->isDesignerMech();
mechData.numComponents = (*iter)->getComponentCount();
if (mechData.numComponents) {
long* componentList = (long*)systemHeap->Malloc(sizeof(long) * mechData.numComponents);
long otherCount = mechData.numComponents;
(*iter)->getComponents(otherCount, componentList);
if (otherCount != mechData.numComponents)
STOP(("Heidi's getComponentCount does not agree with count returned from getComponents"));
for (long i = 0; i < mechData.numComponents; i++)
mechData.components[i] = (unsigned char)componentList[i];
}
MPlayer->sendMissionSetup(0, 1, &mechData);
}
if (!MPlayer->waitTillMechDataReceived()) {
// SERVER DROPPED
mission->destroy();
return(0);
}
ObjectManager->numMechs = 0;
ObjectManager->numVehicles = 0;
for (long i = 0; i < MAX_MC_PLAYERS; i++) {
if (MPlayer->mechDataReceived[i]) {
for (long j = 0; j < 12; j++) {
if (MPlayer->mechData[i][j].objNumber > -1) {
MoverInitData data;
memset(&data, 0, sizeof(MoverInitData));
data.objNumber = MPlayer->mechData[i][j].objNumber;
data.rosterIndex = 255;
data.controlType = 2;
data.controlDataType = 1;
data.position.x = MPlayer->mechData[i][j].pos[0];
data.position.y = MPlayer->mechData[i][j].pos[1];
data.position.z = 0.0;
data.rotation = 0;
data.teamID = MPlayer->playerInfo[MPlayer->mechData[i][j].commanderID].team;
data.commanderID = MPlayer->mechData[i][j].commanderID;
data.baseColor = MPlayer->mechData[i][j].baseColor;
data.highlightColor1 = MPlayer->mechData[i][j].highlightColor1;
data.highlightColor2 = MPlayer->mechData[i][j].highlightColor2;
data.gestureID = 2;
data.active = 1;
data.exists = 1;
data.capturable = 0;
data.icon = 0;
strcpy(data.pilotFileName, MPlayer->mechData[i][j].pilotFile);
strcpy(data.brainFileName, "pbrain");
strcpy(data.csvFileName, MPlayer->mechData[i][j].mechFile);
data.numComponents = MPlayer->mechData[i][j].numComponents;
for (long k = 0; k < MPlayer->mechData[i][j].numComponents; k++)
data.components[k] = MPlayer->mechData[i][j].components[k];
long moverHandle = mission->addMover(&data);
if (moverHandle < 1)
STOP(("Logistics.beginMission: unable to addMover"));
MoverPtr mover = (MoverPtr)ObjectManager->get(moverHandle);
if (!mover)
STOP(("Logistics.beginMission: NULL mover"));
if (mover->getObjectClass() != BATTLEMECH)
STOP(("Logistics.beginMission: not a mech"));
((BattleMech*)mover)->cBills = MPlayer->mechData[i][j].cBills;
strcpy(((BattleMech*)mover)->variantName, MPlayer->mechData[i][j].variantName);
data.variant = MPlayer->mechData[i][j].variantNum;
}
}
}
}
}
else // gotta update pilot availability
{
long count = 256;
LogisticsPilot* pilots[256];
LogisticsData::instance->getPilots( pilots, count );
for ( int i = 0; i < count; i++ )
{
pilots[i]->setUsed(0);
}
Team* pTeam = Team::home;
if ( pTeam )
{
for ( i = pTeam->getRosterSize() - 1; i > -1; i-- )
{
Mover* pMover = (Mover*)pTeam->getMover( i );
if ( pMover && pMover->getCommander()->getId() == Commander::home->getId() )
{
LogisticsPilot* pPilot = LogisticsData::instance->getPilot( pMover->getPilot()->getName() );
if ( pPilot )
pPilot->setUsed( true );
}
}
}
}
CompressedMech mechData;
mechData.commanderID = MPlayer->commanderID;
MPlayer->sendMissionSetup(0, 2, &mechData);
if (!MPlayer->waitTillMissionLoaded()) {
// SERVER DROPPED
mission->destroy();
return(0);
}
}
else if (missionLoadType == MISSION_LOAD_SP_LOGISTICS) {
EList< LogisticsMech*, LogisticsMech* > list;
LogisticsData::instance->getForceGroup( list );
float rotation = list.Count() ? 360.f/list.Count() : 0.f;
MoverInitData data;
memset( &data, 0, sizeof( data ) );
data.rosterIndex = 255;
data.controlType = 2;
data.controlDataType = 1;
data.position.Zero(); // need to get the drop zone location
data.rotation = 0;
data.teamID = Team::home->getId();
data.commanderID = Team::home->getId();
data.active = 1;
data.exists = 1;
data.capturable = 0;
data.baseColor = prefs.baseColor;
data.highlightColor1 = prefs.highlightColor;
data.highlightColor2 = prefs.highlightColor;
strcpy( data.pilotFileName, "pmw00031" );
strcpy( data.brainFileName, "pbrain" );
Stuff::Vector3D vector;
vector.x = 0.f;
vector.y = 1.f;
vector.z = 0.f;
Stuff::Vector3D scaled;
for ( EList< LogisticsMech*, LogisticsMech* >::EIterator iter = list.Begin(); !iter.IsDone(); iter++) {
scaled = vector;
scaled *= 128.;
data.position.Add(dropZoneList[0], scaled);
//data.position = dropZoneList[dropZoneIndex++];
data.objNumber = (*iter)->getFitID();
strcpy( data.csvFileName, (*iter)->getFileName() );
if ( !(*iter)->getPilot() )
continue;
strcpy( data.pilotFileName, (*iter)->getPilot()->getFileName() );
data.overrideLoadedPilot = true;
data.gunnerySkill = (*iter)->getPilot()->getGunnery();
data.pilotingSkill = (*iter)->getPilot()->getPiloting();
memcpy(data.specialtySkills,(*iter)->getPilot()->getSpecialtySkills(),sizeof(bool) * NUM_SPECIALTY_SKILLS);
if ( !(*iter)->getVariant()->isDesignerMech())
{
data.variant = 0;
mission->addMover( &data, (*iter));
}
else
{
data.variant = (*iter)->getVariant()->getFileID();
mission->addMover( &data );
}
Rotate( vector, rotation );
}
}
/* if (MPlayer) {
if (MPlayer->isHost())
MPlayer->serverCID = MPlayer->commanderID;
else {
for (long i = 0; i < MAX_MC_PLAYERS; i++)
if (MPlayer->serverPlayer && (MPlayer->playerList[i].player == MPlayer->serverPlayer))
MPlayer->serverCID = MPlayer->playerList[i].commanderID;
if (MPlayer->serverCID == -1)
STOP(("Logistics.beginMission: bad serverCID"));
}
}
*/
mission->missionInterface->initMechs();
eye->activate();
eye->update();
mission->start();
mission->update(); // force this, so we don't render first
if (MPlayer) {
CompressedMech mechData;
mechData.commanderID = MPlayer->commanderID;
MPlayer->sendMissionSetup(0, 4, &mechData);
if (!MPlayer->waitTillMissionSetup()) {
// SERVER DROPPED
mission->destroy();
return(0);
}
MPlayer->setMode(MULTIPLAYER_MODE_MISSION);
}
if (MPlayer && MPlayer->hostLeft) {
mission->destroy();
return(0);
}
return(1);
}
void Jacobi(const SymmetricMatrix& X, DiagonalMatrix& D, SymmetricMatrix& A,
Matrix& V, bool eivec)
{
Real epsilon = FloatingPointPrecision::Epsilon();
Tracer et("Jacobi");
REPORT
int n = X.Nrows(); DiagonalMatrix B(n), Z(n); D.resize(n); A = X;
if (eivec) { REPORT V.resize(n,n); D = 1.0; V = D; }
B << A; D = B; Z = 0.0; A.Inject(Z);
bool converged = false;
for (int i=1; i<=50; i++)
{
Real sm=0.0; Real* a = A.Store(); int p = A.Storage();
while (p--) sm += fabs(*a++); // have previously zeroed diags
if (sm==0.0) { REPORT converged = true; break; }
Real tresh = (i<4) ? 0.2 * sm / square(n) : 0.0; a = A.Store();
for (p = 0; p < n; p++)
{
Real* ap1 = a + (p*(p+1))/2;
Real& zp = Z.element(p); Real& dp = D.element(p);
for (int q = p+1; q < n; q++)
{
Real* ap = ap1; Real* aq = a + (q*(q+1))/2;
Real& zq = Z.element(q); Real& dq = D.element(q);
Real& apq = A.element(q,p);
Real g = 100 * fabs(apq); Real adp = fabs(dp); Real adq = fabs(dq);
if (i>4 && g < epsilon*adp && g < epsilon*adq) { REPORT apq = 0.0; }
else if (fabs(apq) > tresh)
{
REPORT
Real t; Real h = dq - dp; Real ah = fabs(h);
if (g < epsilon*ah) { REPORT t = apq / h; }
else
{
REPORT
Real theta = 0.5 * h / apq;
t = 1.0 / ( fabs(theta) + sqrt(1.0 + square(theta)) );
if (theta<0.0) { REPORT t = -t; }
}
Real c = 1.0 / sqrt(1.0 + square(t)); Real s = t * c;
Real tau = s / (1.0 + c); h = t * apq;
zp -= h; zq += h; dp -= h; dq += h; apq = 0.0;
int j = p;
while (j--)
{
g = *ap; h = *aq;
*ap++ = g-s*(h+g*tau); *aq++ = h+s*(g-h*tau);
}
int ip = p+1; j = q-ip; ap += ip++; aq++;
while (j--)
{
g = *ap; h = *aq;
*ap = g-s*(h+g*tau); *aq++ = h+s*(g-h*tau);
ap += ip++;
}
if (q < n-1) // last loop is non-empty
{
int iq = q+1; j = n-iq; ap += ip++; aq += iq++;
for (;;)
{
g = *ap; h = *aq;
*ap = g-s*(h+g*tau); *aq = h+s*(g-h*tau);
if (!(--j)) break;
ap += ip++; aq += iq++;
}
}
if (eivec)
{
REPORT
RectMatrixCol VP(V,p); RectMatrixCol VQ(V,q);
Rotate(VP, VQ, tau, s);
}
}
}
}
B = B + Z; D = B; Z = 0.0;
}
if (!converged) Throw(ConvergenceException(X));
if (eivec) SortSV(D, V, true);
else SortAscending(D);
}
void Lander_Control(void)
{
/*
This is the main control function for the lander. It attempts
to bring the ship to the location of the landing platform
keeping landing parameters within the acceptable limits.
How it works:
- First, if the lander is rotated away from zero-degree angle,
rotate lander back onto zero degrees.
- Determine the horizontal distance between the lander and
the platform, fire horizontal thrusters appropriately
to change the horizontal velocity so as to decrease this
distance
- Determine the vertical distance to landing platform, and
allow the lander to descend while keeping the vertical
speed within acceptable bounds. Make sure that the lander
will not hit the ground before it is over the platform!
As noted above, this function assumes everything is working
fine.
*/
/*************************************************
TO DO: Modify this function so that the ship safely
reaches the platform even if components and
sensors fail!
Note that sensors are noisy, even when
working properly.
Finally, YOU SHOULD provide your own
functions to provide sensor readings,
these functions should work even when the
sensors are faulty.
For example: Write a function Velocity_X_robust()
which returns the module's horizontal velocity.
It should determine whether the velocity
sensor readings are accurate, and if not,
use some alternate method to determine the
horizontal velocity of the lander.
NOTE: Your robust sensor functions can only
use the available sensor functions and control
functions!
DO NOT WRITE SENSOR FUNCTIONS THAT DIRECTLY
ACCESS THE SIMULATION STATE. That's cheating,
I'll give you zero.
**************************************************/
double VXlim;
double VYlim;
// Set velocity limits depending on distance to platform.
// If the module is far from the platform allow it to
// move faster, decrease speed limits as the module
// approaches landing. You may need to be more conservative
// with velocity limits when things fail.
if (fabs(Position_X()-PLAT_X)>200) VXlim=25;
else if (fabs(Position_X()-PLAT_X)>100) VXlim=15;
else VXlim=5;
if (PLAT_Y-Position_Y()>200) VYlim=-20;
else if (PLAT_Y-Position_Y()>100) VYlim=-10; // These are negative because they
else VYlim=-4; // limit descent velocity
// Ensure we will be OVER the platform when we land
if (fabs(PLAT_X-Position_X())/fabs(Velocity_X())>1.25*fabs(PLAT_Y-Position_Y())/fabs(Velocity_Y())) VYlim=0;
// IMPORTANT NOTE: The code below assumes all components working
// properly. IT MAY OR MAY NOT BE USEFUL TO YOU when components
// fail. More likely, you will need a set of case-based code
// chunks, each of which works under particular failure conditions.
// Check for rotation away from zero degrees - Rotate first,
// use thrusters only when not rotating to avoid adding
// velocity components along the rotation directions
// Note that only the latest Rotate() command has any
// effect, i.e. the rotation angle does not accumulate
// for successive calls.
if (Angle()>1&&Angle()<359)
{
if (Angle()>=180) Rotate(360-Angle());
else Rotate(-Angle());
return;
}
// Module is oriented properly, check for horizontal position
// and set thrusters appropriately.
if (Position_X()>PLAT_X)
{
// Lander is to the LEFT of the landing platform, use Right thrusters to move
// lander to the left.
Left_Thruster(0); // Make sure we're not fighting ourselves here!
if (Velocity_X()>(-VXlim)) Right_Thruster((VXlim+fmin(0,Velocity_X()))/VXlim);
else
{
// Exceeded velocity limit, brake
Right_Thruster(0);
Left_Thruster(fabs(VXlim-Velocity_X()));
}
}
else
{
// Lander is to the RIGHT of the landing platform, opposite from above
Right_Thruster(0);
if (Velocity_X()<VXlim) Left_Thruster((VXlim-fmax(0,Velocity_X()))/VXlim);
else
{
Left_Thruster(0);
Right_Thruster(fabs(VXlim-Velocity_X()));
}
}
// Vertical adjustments. Basically, keep the module below the limit for
// vertical velocity and allow for continuous descent. We trust
// Safety_Override() to save us from crashing with the ground.
if (Velocity_Y()<VYlim) Main_Thruster(1.0);
else Main_Thruster(0);
}
int EDIT_TOOL::Main( const TOOL_EVENT& aEvent )
{
KIGFX::VIEW_CONTROLS* controls = getViewControls();
PCB_BASE_EDIT_FRAME* editFrame = getEditFrame<PCB_BASE_EDIT_FRAME>();
VECTOR2I originalCursorPos = controls->GetCursorPosition();
const SELECTION& selection = m_selectionTool->GetSelection();
// Shall the selection be cleared at the end?
bool unselect = selection.Empty();
// Be sure that there is at least one item that we can modify. If nothing was selected before,
// try looking for the stuff under mouse cursor (i.e. Kicad old-style hover selection)
if( !hoverSelection( selection ) )
return 0;
Activate();
m_dragging = false; // Are selected items being dragged?
bool restore = false; // Should items' state be restored when finishing the tool?
bool lockOverride = false;
// By default, modified items need to update their geometry
m_updateFlag = KIGFX::VIEW_ITEM::GEOMETRY;
controls->ShowCursor( true );
// cumulative translation
wxPoint totalMovement( 0, 0 );
GRID_HELPER grid( editFrame );
OPT_TOOL_EVENT evt = aEvent;
// Main loop: keep receiving events
do
{
if( evt->IsCancel() )
{
restore = true; // Cancelling the tool means that items have to be restored
break; // Finish
}
else if( evt->Action() == TA_UNDO_REDO )
{
unselect = true;
break;
}
else if( evt->IsAction( &COMMON_ACTIONS::editActivate )
|| evt->IsMotion() || evt->IsDrag( BUT_LEFT ) )
{
BOARD_ITEM* item = selection.Item<BOARD_ITEM>( 0 );
if( m_dragging && evt->Category() == TC_MOUSE )
{
m_cursor = grid.BestSnapAnchor( evt->Position(), item );
controls->ForceCursorPosition( true, m_cursor );
wxPoint movement = wxPoint( m_cursor.x, m_cursor.y ) - item->GetPosition();
totalMovement += movement;
// Drag items to the current cursor position
for( unsigned int i = 0; i < selection.items.GetCount(); ++i )
selection.Item<BOARD_ITEM>( i )->Move( movement + m_offset );
updateRatsnest( true );
}
else if( !m_dragging ) // Prepare to start dragging
{
if( !invokeInlineRouter() )
{
m_selectionTool->SanitizeSelection();
if( selection.Empty() )
break;
// deal with locked items (override lock or abort the operation)
SELECTION_LOCK_FLAGS lockFlags = m_selectionTool->CheckLock();
if( lockFlags == SELECTION_LOCKED )
break;
else if( lockFlags == SELECTION_LOCK_OVERRIDE )
lockOverride = true;
// Save items, so changes can be undone
if( !isUndoInhibited() )
{
editFrame->OnModify();
editFrame->SaveCopyInUndoList( selection.items, UR_CHANGED );
}
m_cursor = controls->GetCursorPosition();
if( selection.Size() == 1 )
{
// Set the current cursor position to the first dragged item origin, so the
// movement vector could be computed later
m_cursor = grid.BestDragOrigin( originalCursorPos, item );
grid.SetAuxAxes( true, m_cursor );
}
else
{
m_cursor = grid.Align( m_cursor );
}
controls->ForceCursorPosition( true, m_cursor );
controls->WarpCursor( m_cursor, true );
VECTOR2I o = VECTOR2I( item->GetPosition() );
m_offset.x = o.x - m_cursor.x;
m_offset.y = o.y - m_cursor.y;
controls->SetAutoPan( true );
m_dragging = true;
incUndoInhibit();
}
}
selection.group->ViewUpdate( KIGFX::VIEW_ITEM::GEOMETRY );
m_toolMgr->RunAction( COMMON_ACTIONS::pointEditorUpdate, true );
}
// Dispatch TOOL_ACTIONs
else if( evt->Category() == TC_COMMAND )
{
if( evt->IsAction( &COMMON_ACTIONS::rotate ) )
{
Rotate( aEvent );
}
else if( evt->IsAction( &COMMON_ACTIONS::flip ) )
{
Flip( aEvent );
// Flip causes change of layers
enableUpdateFlag( KIGFX::VIEW_ITEM::LAYERS );
}
else if( evt->IsAction( &COMMON_ACTIONS::remove ) )
{
Remove( aEvent );
break; // exit the loop, as there is no further processing for removed items
}
else if( evt->IsAction( &COMMON_ACTIONS::duplicate ) )
{
// On duplicate, stop moving this item
// The duplicate tool should then select the new item and start
// a new move procedure
break;
}
else if( evt->IsAction( &COMMON_ACTIONS::moveExact ) )
{
// Can't do this, because the selection will then contain
// stale pointers and it will all go horribly wrong...
//editFrame->RestoreCopyFromUndoList( dummy );
//
// So, instead, reset the position manually
for( unsigned int i = 0; i < selection.items.GetCount(); ++i )
{
BOARD_ITEM* item = selection.Item<BOARD_ITEM>( i );
item->SetPosition( item->GetPosition() - totalMovement );
// And what about flipping and rotation?
// for now, they won't be undone, but maybe that is how
// it should be, so you can flip and move exact in the
// same action?
}
// This causes a double event, so we will get the dialogue
// correctly, somehow - why does Rotate not?
//MoveExact( aEvent );
break; // exit the loop - we move exactly, so we have finished moving
}
}
else if( evt->IsMouseUp( BUT_LEFT ) || evt->IsClick( BUT_LEFT ) )
{
if( !lockOverride )
break; // Finish
lockOverride = false;
}
} while( evt = Wait() );
if( m_dragging )
decUndoInhibit();
m_dragging = false;
m_offset.x = 0;
m_offset.y = 0;
if( restore )
{
// Modifications have to be rollbacked, so restore the previous state of items
wxCommandEvent dummy;
editFrame->RestoreCopyFromUndoList( dummy );
}
else
{
// Changes are applied, so update the items
selection.group->ItemsViewUpdate( m_updateFlag );
}
if( unselect )
m_toolMgr->RunAction( COMMON_ACTIONS::selectionClear, true );
RN_DATA* ratsnest = getModel<BOARD>()->GetRatsnest();
ratsnest->ClearSimple();
ratsnest->Recalculate();
controls->ShowCursor( false );
controls->SetAutoPan( false );
return 0;
}
inline static void Meigen( SWIFT_Real vout[3][3], SWIFT_Real dout[3],
SWIFT_Real a[3][3] )
{
int n = 3;
int j,iq,ip,i;
SWIFT_Real tresh,theta,tau,t,sm,s,h,g,c;
int nrot;
SWIFT_Real b[3];
SWIFT_Real z[3];
SWIFT_Real v[3][3];
SWIFT_Real d[3];
v[0][0] = v[1][1] = v[2][2] = 1.0;
v[0][1] = v[1][2] = v[2][0] = 0.0;
v[0][2] = v[1][0] = v[2][1] = 0.0;
for( ip = 0; ip < n; ip++ ) {
b[ip] = a[ip][ip];
d[ip] = a[ip][ip];
z[ip] = 0.0;
}
nrot = 0;
for( i = 0; i < 50; i++ ) {
sm=0.0;
for( ip = 0;ip < n; ip++ ) {
for( iq = ip+1; iq < n; iq++ ) {
sm += fabs(a[ip][iq]);
}
}
if( sm == 0.0 ) {
vout[0][0] = v[0][0]; vout[0][1] = v[0][1]; vout[0][2] = v[0][2];
vout[1][0] = v[1][0]; vout[1][1] = v[1][1]; vout[1][2] = v[1][2];
vout[2][0] = v[2][0]; vout[2][1] = v[2][1]; vout[2][2] = v[2][2];
dout[0] = d[0]; dout[1] = d[1]; dout[2] = d[2];
return;
}
if( i < 3 ) {
tresh=(SWIFT_Real)0.2*sm/(n*n);
} else {
tresh=0.0;
}
for(ip=0; ip<n; ip++) {
for(iq=ip+1; iq<n; iq++) {
g = (SWIFT_Real)100.0*fabs(a[ip][iq]);
if( i>3 && fabs(d[ip])+g==fabs(d[ip]) &&
fabs(d[iq])+g==fabs(d[iq])
) {
a[ip][iq]=0.0;
} else if( fabs(a[ip][iq])>tresh ) {
h = d[iq]-d[ip];
if( fabs(h)+g == fabs(h) ) {
t=(a[ip][iq])/h;
} else {
theta=(SWIFT_Real)0.5*h/(a[ip][iq]);
t=(SWIFT_Real)(1.0/(fabs(theta)+sqrt(1.0+theta*theta)));
if (theta < 0.0) t = -t;
}
c=(SWIFT_Real)1.0/sqrt(1+t*t);
s=t*c;
tau=s/((SWIFT_Real)1.0+c);
h=t*a[ip][iq];
z[ip] -= h;
z[iq] += h;
d[ip] -= h;
d[iq] += h;
a[ip][iq]=0.0;
for( j = 0; j < ip; j++ ) {
Rotate( a, j, ip, j, iq, tau, s, g, h );
}
for( j = ip+1; j < iq; j++ ) {
Rotate( a, ip, j, j, iq, tau, s, g, h );
}
for( j = iq+1; j < n; j++ ) {
Rotate( a, ip, j, iq, j, tau, s, g, h );
}
for( j = 0; j < n; j++ ) {
Rotate( v, j, ip, j, iq, tau, s, g, h );
}
nrot++;
}
}
}
for( ip = 0 ; ip < n; ip++ ) {
b[ip] += z[ip];
d[ip] = b[ip];
z[ip] = 0.0;
}
}
cerr << "eigen: too many iterations in Jacobi transform." << endl;
}
VOID DrawSphere( HDC hDC, INT Xc, INT Yc )
{
INT i, j, x, y;
DOUBLE theta, phi, phase = clock() / 100;
static VEC G[N][M];
for (i = 0; i < N; i++)
{
theta = i * PI / (N - 1);
for (j = 0; j < M; j++)
{
phi = j * 2 * PI / (M - 1) + phase;
G[i][j].X = R * sin(theta) * cos(phi);
G[i][j].Y = R * sin(theta) * sin(phi);
G[i][j].Z = R * cos(theta);
G[i][j] = Rotate(G[i][j], VecSet(1, 1, 1), 45);
}
}
for (i = 0; i < N; i++)
for (j = 0; j < M; j++)
{
Ps[i][j].x = Xc + G[i][j].X;
Ps[i][j].y = Yc - G[i][j].Z;
}
SelectObject(hDC, GetStockObject(DC_PEN));
SelectObject(hDC, GetStockObject(DC_BRUSH));
SetDCPenColor(hDC, RGB(255, 0, 255));
SetDCBrushColor(hDC, RGB(255, 255, 255));
for (i = 0; i < N - 1; i++)
for (j = 0; j < M - 1; j++)
{
INT img_x, img_y;
COLORREF c, r, g, b;
img_x = j * (Globe.W - 1) / (M - 1);
img_y = i * (Globe.H - 1) / (N - 1);
c = Globe.Bits[img_x + img_y * Globe.W];
r = GetRValue(c);
g = GetGValue(c);
b = GetBValue(c);
c = RGB(b, g, r);
SetDCPenColor(hDC, c);
SetDCBrushColor(hDC, c);
p0 = Ps[i][j];
p1 = Ps[i][j + 1];
p2 = Ps[i + 1][j + 1];
p3 = Ps[i + 1][j];
DrawQuad(hDC, p0, p1, p2, p3, c);
}
/*
for (i = 0; i < N; i++)
{
x = Xc + G[i][0].X;
y = Yc - G[i][0].Z;
MoveToEx(hDC, x , y, NULL);
for (j = 1; j < M; j++)
{
x = Xc + G[i][j].X;
y = Yc - G[i][j].Z;
LineTo(hDC, x, y);
}
}
for (j = 0; j < M; j++)
{
x = Xc + G[0][j].X;
y = Yc - G[0][j].Z;
MoveToEx(hDC, x , y, NULL);
for (i = 1; i < N; i++)
{
x = Xc + G[i][j].X;
y = Yc - G[i][j].Z;
LineTo(hDC, x, y);
}
} */
}
void HawkTransform::Rotate(HawkSegment2D& oSegmen,const HawkVector2D& oRefPt,const HawkRadian& oRad)
{
Rotate(oSegmen.Start,oRefPt,oRad);
Rotate(oSegmen.End,oRefPt,oRad);
}
void CRenderTools::RenderQuads(CQuad *pQuads, int NumQuads, int RenderFlags, ENVELOPE_EVAL pfnEval, void *pUser)
{
Graphics()->QuadsBegin();
float Conv = 1/255.0f;
for(int i = 0; i < NumQuads; i++)
{
CQuad *q = &pQuads[i];
float r=1, g=1, b=1, a=1;
if(q->m_ColorEnv >= 0)
{
float aChannels[4];
pfnEval(q->m_ColorEnvOffset/1000.0f, q->m_ColorEnv, aChannels, pUser);
r = aChannels[0];
g = aChannels[1];
b = aChannels[2];
a = aChannels[3];
}
bool Opaque = false;
/* TODO: Analyze quadtexture
if(a < 0.01f || (q->m_aColors[0].a < 0.01f && q->m_aColors[1].a < 0.01f && q->m_aColors[2].a < 0.01f && q->m_aColors[3].a < 0.01f))
Opaque = true;
*/
if(Opaque && !(RenderFlags&LAYERRENDERFLAG_OPAQUE))
continue;
if(!Opaque && !(RenderFlags&LAYERRENDERFLAG_TRANSPARENT))
continue;
Graphics()->QuadsSetSubsetFree(
fx2f(q->m_aTexcoords[0].x), fx2f(q->m_aTexcoords[0].y),
fx2f(q->m_aTexcoords[1].x), fx2f(q->m_aTexcoords[1].y),
fx2f(q->m_aTexcoords[2].x), fx2f(q->m_aTexcoords[2].y),
fx2f(q->m_aTexcoords[3].x), fx2f(q->m_aTexcoords[3].y)
);
float OffsetX = 0;
float OffsetY = 0;
float Rot = 0;
// TODO: fix this
if(q->m_PosEnv >= 0)
{
float aChannels[4];
pfnEval(q->m_PosEnvOffset/1000.0f, q->m_PosEnv, aChannels, pUser);
OffsetX = aChannels[0];
OffsetY = aChannels[1];
Rot = aChannels[2]/360.0f*pi*2;
}
IGraphics::CColorVertex Array[4] = {
IGraphics::CColorVertex(0, q->m_aColors[0].r*Conv*r, q->m_aColors[0].g*Conv*g, q->m_aColors[0].b*Conv*b, q->m_aColors[0].a*Conv*a),
IGraphics::CColorVertex(1, q->m_aColors[1].r*Conv*r, q->m_aColors[1].g*Conv*g, q->m_aColors[1].b*Conv*b, q->m_aColors[1].a*Conv*a),
IGraphics::CColorVertex(2, q->m_aColors[2].r*Conv*r, q->m_aColors[2].g*Conv*g, q->m_aColors[2].b*Conv*b, q->m_aColors[2].a*Conv*a),
IGraphics::CColorVertex(3, q->m_aColors[3].r*Conv*r, q->m_aColors[3].g*Conv*g, q->m_aColors[3].b*Conv*b, q->m_aColors[3].a*Conv*a)};
Graphics()->SetColorVertex(Array, 4);
CPoint *pPoints = q->m_aPoints;
if(Rot != 0)
{
static CPoint aRotated[4];
aRotated[0] = q->m_aPoints[0];
aRotated[1] = q->m_aPoints[1];
aRotated[2] = q->m_aPoints[2];
aRotated[3] = q->m_aPoints[3];
pPoints = aRotated;
Rotate(&q->m_aPoints[4], &aRotated[0], Rot);
Rotate(&q->m_aPoints[4], &aRotated[1], Rot);
Rotate(&q->m_aPoints[4], &aRotated[2], Rot);
Rotate(&q->m_aPoints[4], &aRotated[3], Rot);
}
IGraphics::CFreeformItem Freeform(
fx2f(pPoints[0].x)+OffsetX, fx2f(pPoints[0].y)+OffsetY,
fx2f(pPoints[1].x)+OffsetX, fx2f(pPoints[1].y)+OffsetY,
fx2f(pPoints[2].x)+OffsetX, fx2f(pPoints[2].y)+OffsetY,
fx2f(pPoints[3].x)+OffsetX, fx2f(pPoints[3].y)+OffsetY);
Graphics()->QuadsDrawFreeform(&Freeform, 1);
}
Graphics()->QuadsEnd();
}
void HawkTransform::Rotate(HawkTriangle2D& oTrig,const HawkVector2D& oRefPt,const HawkRadian& oRad)
{
Rotate(oTrig.Point,oRefPt,oRad);
Rotate(oTrig.Edge[0],HawkVector2D(0,0),oRad);
Rotate(oTrig.Edge[1],HawkVector2D(0,0),oRad);
}
void HawkTransform::Rotate(HawkLine3D& oLine,const HawkVector3D& oRefAxis,const HawkRadian& oRad)
{
Rotate(oLine.Point,oRefAxis,oRad);
Rotate(oLine.Direction,oRefAxis,oRad);
}
void SetBasix(const float MoveX, const float MoveY, const float MoveZ, float RotateAngle, float RotateX, float RotateY, float RotateZ, float ScaleX, float ScaleY, float ScaleZ)
{
Move(MoveX, MoveY, MoveZ);
Rotate(RotateAngle, RotateX, RotateY, RotateZ);
Scale(ScaleX, ScaleY, ScaleZ);
}
void HawkTransform::Rotate(HawkRay3D& oRay,const HawkVector3D& oRefAxis,const HawkRadian& oRad)
{
Rotate(oRay.Point,oRefAxis,oRad);
Rotate(oRay.Direction,oRefAxis,oRad);
}
void Matrix::Rotate(double angle, Vector3d *rotAxis) {
/// Rotation about rotAxis with angle
Rotate(sin(angle), cos(angle), rotAxis);
}
void HawkTransform::Rotate(HawkTriangle3D& oTrig,const HawkVector3D& oRefAxis,const HawkRadian& oRad)
{
Rotate(oTrig.Point,oRefAxis,oRad);
Rotate(oTrig.Edge[0],oRefAxis,oRad);
Rotate(oTrig.Edge[1],oRefAxis,oRad);
}
void pbrtRotate(Float angle, Float dx, Float dy, Float dz) {
VERIFY_INITIALIZED("Rotate");
FOR_ACTIVE_TRANSFORMS(curTransform[i] =
curTransform[i] *
Rotate(angle, Vector3f(dx, dy, dz));)
}
void HawkTransform::Rotate(HawkRect3D& oRect,const HawkVector3D& oRefAxis,const HawkRadian& oRad)
{
Rotate(oRect.Point,oRefAxis,oRad);
Rotate(oRect.Edge[0],oRefAxis,oRad);
Rotate(oRect.Edge[1],oRefAxis,oRad);
}
int GMoveTransformHandler::OnMouseMove(UINT nFlags, Point &point)
{
if (State()==1) { // mouse pressed
// map new point to same depth plane
point.z = lastS.z;
Point pw,pl;
MapPoint(point,pw,pl);
/*
view->viewInfo.View2World(point,pw);
World2Local(pw,pl);
*/
Point delta = pl - lastO;
switch(transformMode) {
case MODE_TRANSLATE :
AxisMap (axes,delta,0.0f);
Translate(delta);
break;
case MODE_SCALE :
Delta2Scale(scaleUniform,delta);
AxisMap (axes,delta,1.0f);
Scale(delta);
break;
case MODE_ROTATE:
{
Quaternation q;
if (rotateTrackball) {
// q.SetTrackball(lastS.x-centerS.x,lastS.y-centerS.y,point.x-centerS.x,point.y-centerS.y);
q.SetTrackball(point.x-centerS.x,point.y-centerS.y,lastS.x-centerS.x,lastS.y-centerS.y,0.4);
} else {
delta = point - lastS; // in screen units -1 .. 1
Point a(1,1,1);
AxisMap (axes,a,0.0f);
double phi = 0.2 * delta.x * TWOPI;
q.SetAxisRotation(a.x,a.y, a.z,phi);
}
Rotate(q);
}
break;
default :
break;
}
UpdateDialogValue();
view->Redraw();
//TRACE("%s:OnMouseMove (%f %f %f) %x \n",this->ClassName(),delta.x,delta.y,delta.z,nFlags);
lastS = point;
lastO = pl;
}
return(EV_OK);
}
void HawkTransform::Rotate(HawkLine2D& oLine,const HawkVector2D& oRefPt,const HawkRadian& oRad)
{
Rotate(oLine.Point,oRefPt,oRad);
Rotate(oLine.Direction,HawkVector2D(0,0),oRad);
}
void DG8Graphics::Rotate(float *rotate)
{
Rotate(rotate[0],rotate[1],rotate[2],rotate[3]);
}
void HawkTransform::Rotate(HawkRay2D& oRay,const HawkVector2D& oRefPt,const HawkRadian& oRad)
{
Rotate(oRay.Point,oRefPt,oRad);
Rotate(oRay.Direction,HawkVector2D(0,0),oRad);
}
void PathPlanner(void)
{
if(QueryTankSM()!=DISABLED)
{
static float CurrentPosition[3] = {0,0,0};
static float CurrentWallLine[2];
static int CurrentWallAngle;
static ActivityState_t CurrentGoal;
ES_Event ThinkTankEvent;
ES_Event GameMonitorEvent;
srand(ES_Timer_GetTime());
if( CurrentGoal != QueryActivityControlSM())
{
//printf("clearing checkpoints\n\r");
wigglecounter = 0;
BinCheckPoints = 0;
WallCheckPoints = 0;
// GOOD?
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
CurrentGoal = QueryActivityControlSM();
} else
{
//ES_Event TankEvent;
//CurrentGoal = QueryActivityControlSM();
CurrentWallLine[0]= GetWallSlope();
CurrentWallLine[1]= GetWallIntercept();
CurrentWallAngle = GetWallAngle();
UpdateCurrentPosition();
CurrentPosition[X_COORDINATE] = GetX();
CurrentPosition[Y_COORDINATE] = GetY();
CurrentPosition[THETA] = GetTheta();
//UpdateWallPoints(); // before getting staging points for wall
switch (CurrentGoal)
{
//ES_Event ActionEvent;
case RASTER :
{
static char i;
//ThinkTankEvent.EventType = GO_COMMAND;
//ThinkTankEvent.EventParam = 100;
//Calibration test code
/*
if (i < 5)
{
ThinkTankEvent.EventType = GO_COMMAND;
ThinkTankEvent.EventParam = STEP_DISTANCE;
i++;
} else if (i<6)
{
ThinkTankEvent.EventType = TURN_COMMAND;
ThinkTankEvent.EventParam = 1080;
i++;
} else if (i<7)
{
ThinkTankEvent.EventType = TURN_COMMAND;
ThinkTankEvent.EventParam = -1080;
i++;
} else
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
i=0;
}
*/
// go forward thrice, turn randomly once, unless threatened by wall, then flee for safety.
if (CurrentLocationIsSafe( CurrentPosition[0], CurrentPosition[1], CurrentPosition[2], CurrentWallAngle) == True) //our current location is a safe one
{
if (i < 3)
{
if (SafeToTravel((float)STEP_DISTANCE/100.,CurrentPosition[0],CurrentPosition[1],CurrentPosition[2], CurrentWallAngle) == True)
{
ThinkTankEvent.EventType = GO_COMMAND;
ThinkTankEvent.EventParam = STEP_DISTANCE;
//SetMotorTimer(STEP_DISTANCE);
} else
{
ThinkTankEvent.EventType = TURN_COMMAND;
ThinkTankEvent.EventParam = (rand()%360) - 180;
//SetMotorTimer(angle);
}
i++;
} else if (i<4)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
i++;
} else
{
ThinkTankEvent.EventType = TURN_COMMAND;
ThinkTankEvent.EventParam = (rand()%360) - 180;
//SetMotorTimer(angle);
i = 0;
}
} else
{
//go to safe point
ThinkTankEvent = GenerateNextStep_EscapeWallPath(CurrentPosition,CurrentWallAngle);
}
}
break;
case CONTROL_LEFT_WALL :
//printf("Wall Third Point is x: %d y: %d \n\r", PushPointX(),PushPointY());
/*
if(GetSeparationDistance( -- parameters for push point -- ) < 6)
{
WallCheckPoints = 2;
}
*/
if (WallCheckPoints < 1)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = WallFirstPointX();
GoalPoint[1] = WallFirstPointY();
//NextGoalPoint[0] = WallStagingPointX(LEFT);
//NextGoalPoint[1] = WallStagingPointY(LEFT);
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else if (WallCheckPoints < 2)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = WallStagingPointX(LEFT);
GoalPoint[1] = WallStagingPointY(LEFT);
//NextGoalPoint[0] = PushPointX();
//NextGoalPoint[1] = PushPointY();
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else if (WallCheckPoints < 3)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = PushPointX();
GoalPoint[1] = PushPointY();
// change this!!
//NextGoalPoint[0] = PushPointX();
//NextGoalPoint[1] = PushPointY();
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else
{
// THINK THROUGH THIS
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
//CurrentGoal = STAY_PUT (will start process over again)
//WallCheckPointThree = False;
}
break;
case CONTROL_RIGHT_WALL :
//printf("Wall Third Point is x: %d y: %d \n\r", PushPointX(),PushPointY());
/*
if(GetSeparationDistance( -- parameters for staging point -- ) < 8)
{
WallCheckPoints = 2;
}
*/
if (WallCheckPoints < 1)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = WallFirstPointX();
GoalPoint[1] = WallFirstPointY();
//NextGoalPoint[0] = WallStagingPointX(LEFT);
//NextGoalPoint[1] = WallStagingPointY(LEFT);
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else if (WallCheckPoints < 2)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = WallStagingPointX(RIGHT);
GoalPoint[1] = WallStagingPointY(RIGHT);
//NextGoalPoint[0] = PushPointX();
//NextGoalPoint[1] = PushPointY();
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else if (WallCheckPoints < 3)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = PushPointX();
GoalPoint[1] = PushPointY();
// change this!!
//NextGoalPoint[0] = PushPointX();
//NextGoalPoint[1] = PushPointY();
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else
{
// THINK THROUGH THIS
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
//CurrentGoal = STAY_PUT (will start process over again)
//WallCheckPointThree = False;
}
break;
case DEPOSIT_IN_BIN_ONE :
if (BinCheckPoints < 1)
{
ThinkTankEvent = GenerateNextStep_RadialPath(CurrentPosition,RADIUS_OF_SAFETY-2*HALF_BOT_LENGTH,WAIT_FOR_POSITION_FOUND);
} else if (BinCheckPoints < 2)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = StagingPointForBinOneX;
GoalPoint[1] = StagingPointForBinOneY;
//NextGoalPoint[0] = BinOneDepositX;
//NextGoalPoint[1] = BinOneDepositY;
GameMonitorEvent.EventType = START_AVERAGING;
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else if (BinCheckPoints < 3)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
BinCheckPoints++;
} else if (BinCheckPoints < 4)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
BinCheckPoints++;
} else if (BinCheckPoints < 5)
{
GameMonitorEvent.EventType = STOP_AVERAGING;
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
BinCheckPoints++;
} else if (BinCheckPoints < 6)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = DepositPointForBinOneX;
GoalPoint[1] = DepositPointForBinOneY;
//UpdateCurrentPosition();
//CurrentPosition[X_COORDINATE] = GetX();
//CurrentPosition[Y_COORDINATE] = GetY();
//CurrentPosition[THETA] = GetTheta();
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_A_TENTH_SECOND); /* checkpoint */
} else
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
Drive(GENTLE_FORWARD);
}
break;
case DEPOSIT_IN_BIN_TWO :
if (BinCheckPoints < 1)
{
ThinkTankEvent = GenerateNextStep_RadialPath(CurrentPosition,RADIUS_OF_SAFETY-2*HALF_BOT_LENGTH,WAIT_FOR_POSITION_FOUND);
} else if (BinCheckPoints < 2)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = StagingPointForBinTwoX;
GoalPoint[1] = StagingPointForBinTwoY;
//NextGoalPoint[0] = BinOneDepositX;
//NextGoalPoint[1] = BinOneDepositY;
GameMonitorEvent.EventType = START_AVERAGING;
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else if (BinCheckPoints < 3)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
BinCheckPoints++;
} else if (BinCheckPoints < 4)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
BinCheckPoints++;
} else if (BinCheckPoints < 5)
{
GameMonitorEvent.EventType = STOP_AVERAGING;
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
BinCheckPoints++;
} else if (BinCheckPoints < 6)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = DepositPointForBinTwoX;
GoalPoint[1] = DepositPointForBinTwoY;
//UpdateCurrentPosition();
//CurrentPosition[X_COORDINATE] = GetX();
//CurrentPosition[Y_COORDINATE] = GetY();
//CurrentPosition[THETA] = GetTheta();
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_A_TENTH_SECOND); /* checkpoint */
} else
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
Drive(GENTLE_FORWARD);
}
break;
case DEPOSIT_IN_BIN_THREE :
if (BinCheckPoints < 1)
{
ThinkTankEvent = GenerateNextStep_RadialPath(CurrentPosition,RADIUS_OF_SAFETY-2*HALF_BOT_LENGTH,WAIT_FOR_POSITION_FOUND);
} else if (BinCheckPoints < 2)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = StagingPointForBinThreeX;
GoalPoint[1] = StagingPointForBinThreeY;
//NextGoalPoint[0] = BinOneDepositX;
//NextGoalPoint[1] = BinOneDepositY;
GameMonitorEvent.EventType = START_AVERAGING;
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else if (BinCheckPoints < 3)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
BinCheckPoints++;
} else if (BinCheckPoints < 4)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
BinCheckPoints++;
} else if (BinCheckPoints < 5)
{
GameMonitorEvent.EventType = STOP_AVERAGING;
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
BinCheckPoints++;
} else if (BinCheckPoints < 6)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = DepositPointForBinThreeX;
GoalPoint[1] = DepositPointForBinThreeY;
//UpdateCurrentPosition();
//CurrentPosition[X_COORDINATE] = GetX();
//CurrentPosition[Y_COORDINATE] = GetY();
//CurrentPosition[THETA] = GetTheta();
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_A_TENTH_SECOND); /* checkpoint */
} else if (BinCheckPoints < 7)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
Drive(GENTLE_FORWARD);
wigglecounter++;
if (30 < wigglecounter)
{
ES_Event DummyEvent;
DummyEvent.EventType = DEPOSIT_COMMAND;
DummyEvent.EventParam = 1;
ES_PostList00(DummyEvent);
//printf("dpt tmr end - DUMP\n\r");
BinCheckPoints++;
wigglecounter = 0;
}
} else
{
static int done_wiggling;
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_WIGGLE_TIME;
if(done_wiggling < 22)
{
if (wigglecounter == 0)
{
float param = 50;
Drive(0);
Rotate(param);//*GENTLE_FORWARD);
puts("-");
ThinkTankEvent.EventParam = 3*WAIT_FOR_A_TENTH_SECOND; // 4?
} else if (wigglecounter == 1)
{
float param = -75;
puts("+");
Drive(0);
Rotate(param);//GENTLE_FORWARD);
} else if(wigglecounter == 2)
{
float param = 50;
Drive(0);
Rotate(param);//*GENTLE_FORWARD);
puts("-");
ThinkTankEvent.EventParam = 3*WAIT_FOR_A_TENTH_SECOND;
} else if(wigglecounter == 3)
{
float param = -75;
puts("+");
Drive(0);
Rotate(param);//GENTLE_FORWARD);
wigglecounter = 0;
}
} else if (done_wiggling == 6)
{
ES_Event DummyEvent;
Drive(0);
DummyEvent.EventType = DEPOSIT_COMPLETE;
DummyEvent.EventParam = 1;
//ES_PostList02(DummyEvent);
}
wigglecounter++;
done_wiggling++;
}
break;
case DEPOSIT_IN_BIN_FOUR :
if (BinCheckPoints < 1)
{
ThinkTankEvent = GenerateNextStep_RadialPath(CurrentPosition,RADIUS_OF_SAFETY-2*HALF_BOT_LENGTH,WAIT_FOR_POSITION_FOUND);
} else if (BinCheckPoints < 2)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = StagingPointForBinFourX;
GoalPoint[1] = StagingPointForBinFourY;
//NextGoalPoint[0] = BinOneDepositX;
//NextGoalPoint[1] = BinOneDepositY;
GameMonitorEvent.EventType = START_AVERAGING;
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else if (BinCheckPoints < 3)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
BinCheckPoints++;
} else if (BinCheckPoints < 4)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
BinCheckPoints++;
} else if (BinCheckPoints < 5)
{
GameMonitorEvent.EventType = STOP_AVERAGING;
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
BinCheckPoints++;
} else if (BinCheckPoints < 6)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = DepositPointForBinFourX;
GoalPoint[1] = DepositPointForBinFourY;
//UpdateCurrentPosition();
//CurrentPosition[X_COORDINATE] = GetX();
//CurrentPosition[Y_COORDINATE] = GetY();
//CurrentPosition[THETA] = GetTheta();
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_A_TENTH_SECOND); /* checkpoint */
} else if (BinCheckPoints < 7)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
Drive(GENTLE_FORWARD);
wigglecounter++;
if (30 < wigglecounter)
{
ES_Event DummyEvent;
DummyEvent.EventType = DEPOSIT_COMMAND;
DummyEvent.EventParam = 1;
ES_PostList00(DummyEvent);
//printf("dpt tmr end - DUMP\n\r");
BinCheckPoints++;
wigglecounter = 0;
}
} else
{
static int done_wiggling;
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_WIGGLE_TIME;
if(done_wiggling < 22)
{
if (wigglecounter == 0)
{
float param = 50;
Drive(0);
Rotate(param);//*GENTLE_FORWARD);
puts("-");
ThinkTankEvent.EventParam = 3*WAIT_FOR_A_TENTH_SECOND; // 4?
} else if (wigglecounter == 1)
{
float param = -75;
puts("+");
Drive(0);
Rotate(param);//GENTLE_FORWARD);
} else if(wigglecounter == 2)
{
float param = 50;
Drive(0);
Rotate(param);//*GENTLE_FORWARD);
puts("-");
ThinkTankEvent.EventParam = 3*WAIT_FOR_A_TENTH_SECOND;
} else if(wigglecounter == 3)
{
float param = -75;
puts("+");
Drive(0);
Rotate(param);//GENTLE_FORWARD);
wigglecounter = 0;
}
} else if (done_wiggling == 6)
{
ES_Event DummyEvent;
Drive(0);
DummyEvent.EventType = DEPOSIT_COMPLETE;
DummyEvent.EventParam = 1;
//ES_PostList02(DummyEvent);
}
wigglecounter++;
done_wiggling++;
}
break;
}
}
/*
switch(ThinkTankEvent.EventType)
{
case(TURN_COMMAND) :
printf("Current Event is TURN_COMMAND\n\r");
break;
case(GO_COMMAND) :
printf("Current Event is GO_COMMAND\n\r");
break;
case(WAIT_COMMAND) :
printf("Current Event is WAIT_COMMAND\n\r");
break;
case(ARRIVED) :
printf("Current Event is whimsical.\n\r");
default:
printf("epic fail\n\r");
}
*/
if(QueryTankSM()!=DISABLED)
ES_PostList00(ThinkTankEvent);
ES_PostList04(ThinkTankEvent);
ES_PostList03(GameMonitorEvent);
printf("We're at X = %f, Y= %f, and Theta = %f, Wall is at %d\n\r", CurrentPosition[0],CurrentPosition[1],CurrentPosition[2],CurrentWallAngle);
//ES_PostList02(TankEvent);
//printf("event param = %f",ThinkTankEvent.EventParam);
}
}
//Fonction TurnOnLeft-------------------------------------------------------------faux
void TurnOnLeft(int delay)
{
OCR1A = LEFT; //on change la fréquence de découpage du timer pour que l'interprétation faite par le second ATMEGA fasse que le gouvernail tourne à gauche
Rotate(delay);
}