コード例 #1
0
ファイル: main.c プロジェクト: pobot/Pobot-Playground
//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);
}
コード例 #2
0
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());
			}
        }
    }
}
コード例 #3
0
void Asteroid::tick(double DT)
{
   Translate((motionProperties.speed * motionProperties.direction) * static_cast<float>(DT));
   Rotate(motionProperties.angularSpeed * static_cast<float>(DT) * motionProperties.rotation);
}
コード例 #4
0
ファイル: Matrix.cpp プロジェクト: Fat-Zer/FreeCAD_sf_master
	void	Matrix::Rotate(double angle, int Axis)
	{	// Rotation (Axis 1 = x , 2 = y , 3 = z 
		Rotate(sin(angle), cos(angle), Axis);
	}
コード例 #5
0
ファイル: SceneData.cpp プロジェクト: gnub/itugameengine
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;
}
コード例 #6
0
	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);
	}
コード例 #7
0
ファイル: Transformable.cpp プロジェクト: aLaix/RadicalEngine
	inline void RotateRel(const float RAngle, const float NormalX, const float NormalY, const float NormalZ)
	{
		Rotate(rangle+RAngle, NormalX, NormalY, NormalZ);
	}
コード例 #8
0
ファイル: Lander.cpp プロジェクト: whyandrew/sim
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);
  }
 }
}
コード例 #9
0
ファイル: logistics.cpp プロジェクト: Jacic/MechCommander2
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);
}
コード例 #10
0
ファイル: jacobi.cpp プロジェクト: CalebVDW/smr-motion
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);
}
コード例 #11
0
ファイル: Lander.cpp プロジェクト: whyandrew/sim
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);
}
コード例 #12
0
ファイル: edit_tool.cpp プロジェクト: morio/kicad
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;
}
コード例 #13
0
ファイル: mesh_utils.cpp プロジェクト: ybabel/Go4D
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;
}
コード例 #14
0
ファイル: Sphere.C プロジェクト: 1fanfan1/SUM2016
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);
    }
  } */  
}
コード例 #15
0
	void HawkTransform::Rotate(HawkSegment2D& oSegmen,const HawkVector2D& oRefPt,const HawkRadian& oRad)
	{
		Rotate(oSegmen.Start,oRefPt,oRad);
		Rotate(oSegmen.End,oRefPt,oRad);
	}
コード例 #16
0
ファイル: render_map.cpp プロジェクト: jxsl13/HClient
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();
}
コード例 #17
0
	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);
	}
コード例 #18
0
	void HawkTransform::Rotate(HawkLine3D& oLine,const HawkVector3D& oRefAxis,const HawkRadian& oRad)
	{
		Rotate(oLine.Point,oRefAxis,oRad);
		Rotate(oLine.Direction,oRefAxis,oRad);
	}
コード例 #19
0
ファイル: Transformable.cpp プロジェクト: aLaix/RadicalEngine
	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);
	}
コード例 #20
0
	void HawkTransform::Rotate(HawkRay3D& oRay,const HawkVector3D& oRefAxis,const HawkRadian& oRad)
	{
		Rotate(oRay.Point,oRefAxis,oRad);
		Rotate(oRay.Direction,oRefAxis,oRad);
	}
コード例 #21
0
ファイル: Matrix.cpp プロジェクト: Fat-Zer/FreeCAD_sf_master
	void	Matrix::Rotate(double angle, Vector3d *rotAxis) {
		/// Rotation about rotAxis with angle
		Rotate(sin(angle), cos(angle), rotAxis);
	}
コード例 #22
0
	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);
	}
コード例 #23
0
ファイル: api.cpp プロジェクト: tdapper/pbrt-v3
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));)
}
コード例 #24
0
	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);
	}
コード例 #25
0
ファイル: gedit.cpp プロジェクト: deepmatrix/blaxxun-cc3d
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);
}
コード例 #26
0
	void HawkTransform::Rotate(HawkLine2D& oLine,const HawkVector2D& oRefPt,const HawkRadian& oRad)
	{
		Rotate(oLine.Point,oRefPt,oRad);
		Rotate(oLine.Direction,HawkVector2D(0,0),oRad);
	}
コード例 #27
0
void DG8Graphics::Rotate(float *rotate)
{
	Rotate(rotate[0],rotate[1],rotate[2],rotate[3]);
}
コード例 #28
0
	void HawkTransform::Rotate(HawkRay2D& oRay,const HawkVector2D& oRefPt,const HawkRadian& oRad)
	{
		Rotate(oRay.Point,oRefPt,oRad);
		Rotate(oRay.Direction,HawkVector2D(0,0),oRad);
	}
コード例 #29
0
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);
}
}
コード例 #30
0
ファイル: main.c プロジェクト: pobot/Pobot-Playground
//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);
}