/*! This function takes a Jacobian with all columns (6dof per joint
	and returns a version that has only those columns that correspond
	to the actuated dofs in each joint (e.g. the z axis for revolute 
	joints) 
*/
Matrix
KinematicChain::actuatedJacobian(const Matrix &fullColumnJ) const
{
	std::vector<DynJoint*> dynJoints;
	getDynamicJoints(&dynJoints);
	int activeRows = fullColumnJ.rows();
	if (!activeRows) return Matrix(0,0);
	//first count the constraints
	int numConstrained = 0;
	int numActuated = 0;
	for(int dj=0; dj<(int)dynJoints.size(); dj++) {
		numConstrained += dynJoints[dj]->getNumConstraints();
		numActuated += 6 - dynJoints[dj]->getNumConstraints();
	}
	assert(numActuated + numConstrained == 6*numLinks);
	assert(fullColumnJ.cols() == 6*numLinks);
	Matrix blockJ(activeRows, numActuated);
	//then copy only actuated columns in the block jacobian.
	int actIndex = 0;
	for(int dj=0; dj<(int)dynJoints.size(); dj++) {
		char constraints[6];
		dynJoints[dj]->getConstraints(constraints);
		for (int c=0; c<6; c++) {
			if (!constraints[c]) {
				//actuated direction
				blockJ.copySubBlock(0, actIndex, activeRows, 1, fullColumnJ, 0, 6*dj+c);
				actIndex++;
			}
		}
	}
	assert(actIndex == numActuated);
	return blockJ;
}
Exemple #2
0
int main ( int argc, char** argv )
{

    if ( SDL_Init( SDL_INIT_VIDEO ) < 0 )
    {
        printf( "Unable to init SDL: %s\n", SDL_GetError() );
        return 1;
    }

     atexit(SDL_Quit);

     screen = SDL_SetVideoMode(SCREENW, SCREENH, 32,
                                           SDL_HWSURFACE|SDL_DOUBLEBUF);
    if ( !screen )
    {
        printf("Unable to set 800x600 video: %s\n", SDL_GetError());
        return 1;
    }
	SDL_WM_SetCaption("Szit the Game", NULL);
	if( TTF_Init() == -1 )
    {
		printf("Unable to initialize font");
        return 1;
    }

    font=TTF_OpenFont("tahoma.ttf", 28);
    if(font){printf("Success");}

    // centre the bitmap on screen

	CGameManager manager;

    bool done = false;
    CBlock blockT(0, -1, 0, 0, 0, 1, 0, 0, -1, 100, 100, 100); //t
    blockT.TryTurn();
    blockT.Turn();
    blockT.TryTurn();
    blockT.Turn();

    CBlock blockZ(0, -1,-1,0,-1,0,0,1,0, 198,255,126); //z
    CBlock blockS(0, -1,0,0,0,0,-1,1,-1, 126,196,255); //s
    CBlock blockO(1, 0,0,1,0,0,1,1,1, 100, 100, 0); //o
    CBlock blockI(0, 0,-1,0,0,0,1,0,2, 255,159,126); //i
    CBlock blockJ(0, -1,1,0,1,0,0,0,-1, 139,77,156); //j
    CBlock blockL(0, 0,-1,0,0,0,1,1,1, 255,229,126); //l

    CBlock GameBlock(0, -1, 0, 0, 0, 1, 0, 0, -1, 179,206,221); //creating t
    CBlock NextBlock(0, -1, 0, 0, 0, 1, 0, 0, -1, 179,206,221); //t
    //CBlock GameBlock(0, -1,0,0,0,0,-1,1,-1); //s
    //CBlock GameBlock(0, 0,-1,0,0,0,1,0,2); //i


	CButton start(300, 350, 200, 50, 100, 100, 100, 150, 150, 150, 1, "Start");
	CButton help(300, 425, 200, 50, 100, 100, 100, 150, 150, 150, 0, "Help");
	CButton exit(300, 500, 200, 50, 100, 100, 100, 150, 150, 150, 0, "Exit");
	CButton easy(100, 350, 200, 50, 200, 200, 200, 255, 255, 255, 0, "Easy");
	CButton medium(300, 350, 200, 50, 200, 200, 200, 255, 255, 255, 1, "Medium");
	CButton hard(500, 350, 200, 50, 200, 200, 200, 255, 255, 255, 0, "Hard");
	manager.Refresh(&GameBlock, &NextBlock);
	manager.Refresh(&GameBlock, &NextBlock);


	srand(time(NULL));
			switch(rand()%7){
					case 0: GameBlock.Set(0, -1, 0, 0, 0, 1, 0, 0, -1,179,206,221);break; //t
					case 1: GameBlock.Set(0, -1,-1,0,-1,0,0,1,0, 198,255,126);break; //z
					case 2: GameBlock.Set(0, -1,0,0,0,0,-1,1,-1, 126,196,255);break; //s
					case 3: GameBlock.Set(1, 0,0,1,0,0,1,1,1, 100, 100, 0);break; //o
					case 4: GameBlock.Set(0, 0,-1,0,0,0,1,0,2, 255,159,126);break; //i
					case 5: GameBlock.Set(0, -1,1,0,1,0,0,0,-1, 139,77,156);break; //j
					case 6: GameBlock.Set(0, 0,-1,0,0,0,1,1,1, 255,229,126);break; //l
			}

	srand(time(NULL)+SDL_GetTicks());
			switch(rand()%7){
					case 0: NextBlock.Set(0, -1, 0, 0, 0, 1, 0, 0, -1,179,206,221);break; //t
					case 1: NextBlock.Set(0, -1,-1,0,-1,0,0,1,0, 198,255,126);break; //z
					case 2: NextBlock.Set(0, -1,0,0,0,0,-1,1,-1, 126,196,255);break; //s
					case 3: NextBlock.Set(1, 0,0,1,0,0,1,1,1, 100, 100, 0);break; //o
					case 4: NextBlock.Set(0, 0,-1,0,0,0,1,0,2, 255,159,126);break; //i
					case 5: NextBlock.Set(0, -1,1,0,1,0,0,0,-1, 139,77,156);break; //j
					case 6: NextBlock.Set(0, 0,-1,0,0,0,1,1,1, 255,229,126);break; //l
			}

    // program main loop
    while (!done)
    {
        // message processing loop
        SDL_Event event;
        ++frame;
		SetFPS();
		LastFrame=SDL_GetTicks();

		if(gamestatus==1){manager.Refresh(&GameBlock, &NextBlock);
		}

        while (SDL_PollEvent(&event))
        {
            // check for messages
            switch (event.type)
            {
                // exit if the window is closed
            case SDL_QUIT:
                done = true;
                break;

                // check for keypresses
            case SDL_KEYDOWN:
                {
                    // exit if ESCAPE is pressed
                    if (event.key.keysym.sym == SDLK_ESCAPE)
                        {done = true;}

					if(!gamestatus)
					{
						if(event.key.keysym.sym == SDLK_DOWN){
							if(start.Active()){start.ChangeActivity(); help.ChangeActivity();}else
							if(help.Active()){help.ChangeActivity(); exit.ChangeActivity();}else
							if(exit.Active()){exit.ChangeActivity(); start.ChangeActivity();}
							}
						else if(event.key.keysym.sym == SDLK_UP){
							if(start.Active()){start.ChangeActivity(); exit.ChangeActivity();}else
							if(help.Active()){help.ChangeActivity(); start.ChangeActivity();}else
							if(exit.Active()){exit.ChangeActivity(); help.ChangeActivity();}
						}
						if(event.key.keysym.sym == SDLK_RETURN){
							if(start.Active()){gamestatus=3;}
							else if(help.Active()){gamestatus=5;}
							else if(exit.Active()){done=1;}
						}
					}else
					if(gamestatus==1){
						if(event.key.keysym.sym == SDLK_DOWN){
								manager.MoveBlockDown(&GameBlock, 1);
						}
						if(event.key.keysym.sym == SDLK_SPACE){
								manager.TurnGameBlock(&GameBlock);
						}
						if(event.key.keysym.sym == SDLK_LEFT){
								manager.MoveBlockLeft(&GameBlock);
						}
						if(event.key.keysym.sym == SDLK_RIGHT){
								manager.MoveBlockRight(&GameBlock);
						}
						if(event.key.keysym.sym == SDLK_w){
								manager.SpeedUp();
						}
						if(event.key.keysym.sym ==SDLK_p){
							gamestatus=2;
						}
					}else
					if(gamestatus==2){
						if(event.key.keysym.sym ==SDLK_p){
							gamestatus=1;
						}
					}else
					if(gamestatus==3){
						if(event.key.keysym.sym == SDLK_RETURN){
							if(easy.Active()){manager.SetLevel(1);}else
							if(medium.Active()){manager.SetLevel(2);}else
							if(hard.Active()){manager.SetLevel(3);}
							gamestatus=1;
						}else
						if(event.key.keysym.sym == SDLK_RIGHT){
							if(easy.Active()){easy.ChangeActivity(); medium.ChangeActivity();}else
							if(medium.Active()){medium.ChangeActivity(); hard.ChangeActivity();}else
							if(hard.Active()){hard.ChangeActivity(); easy.ChangeActivity();}
						}else
						if(event.key.keysym.sym == SDLK_LEFT){
							if(easy.Active()){easy.ChangeActivity(); hard.ChangeActivity();}else
							if(medium.Active()){medium.ChangeActivity(); easy.ChangeActivity();}else
							if(hard.Active()){hard.ChangeActivity(); medium.ChangeActivity();}
						}
					}else
					if(gamestatus==4){
							if(event.key.keysym.sym){
									SDL_Delay(1000);
									gamestatus=0;
									manager.ClearGame();
							}
					}else
					if(gamestatus==5){
							if(event.key.keysym.sym){
									gamestatus=0;
							}
					}
                    break;
                }
            } // end switch
         }// end of message processing

        // DRAWING STARTS HERE


        // clear screen
        SDL_FillRect(screen, 0, SDL_MapRGB(screen->format, 0, 0, 0));

        // draw bitmap




		if(gamestatus==0){
			int a=SDL_GetTicks();

			RBC_DrawRect(0, 0, screen->w, screen->h, 0, 0, 50);

			RBC_DrawRect((0+frame/2)%screen->w, 0, screen->w/5, screen->h, 105,210,231);
			if(((0+frame/2)%screen->w+screen->w/5)>screen->w){RBC_DrawRect(0, 0, ((0+frame/2)%screen->w+screen->w/5)-screen->w, screen->h, 105,210,231);}

			RBC_DrawRect((screen->w/5+frame/2)%screen->w, 0, screen->w/5, screen->h, 167,219,216);
			if(((screen->w/5+frame/2)%screen->w+screen->w/5)>screen->w){RBC_DrawRect(0, 0, ((screen->w/5+frame/2)%screen->w+screen->w/5)-screen->w, screen->h, 167,219,216);}

			RBC_DrawRect((screen->w/5*2+frame/2)%screen->w, 0, screen->w/5, screen->h, 224,228,204);
			if(((screen->w/5*2+frame/2)%screen->w+screen->w/5)>screen->w){RBC_DrawRect(0, 0, ((screen->w/5*2+frame/2)%screen->w+screen->w/5)-screen->w, screen->h, 224,228,204);}

			RBC_DrawRect((screen->w/5*3+frame/2)%screen->w, 0, screen->w/5, screen->h, 243,134,48);
			if(((screen->w/5*3+frame/2)%screen->w+screen->w/5)>screen->w){RBC_DrawRect(0, 0, ((screen->w/5*3+frame/2)%screen->w+screen->w/5)-screen->w, screen->h, 243,134,48);}

			RBC_DrawRect((screen->w/5*4+frame/2)%screen->w, 0, screen->w/5, screen->h, 250,105,0);
			if(((screen->w/5*4+frame/2)%screen->w+screen->w/5)>screen->w){RBC_DrawRect(0, 0, ((screen->w/5*4+frame/2)%screen->w+screen->w/5)-screen->w, screen->h, 250,105,0);}


			blockS.Draw(-100, 90+sin(a/100)*50);
			blockZ.Draw(50, 90+sin(a/100+10)*50);
			blockI.Draw(200, 90+sin(a/100+20)*50);
			blockT.Draw(350, 90+sin(a/100+30)*50);

			start.Draw();
			help.Draw();
			exit.Draw();

		}else
		if(gamestatus==1){
			switch(manager.ReturnSpeed()){
					case 2:RBC_DrawRect(0, 0, screen->w, screen->h, 105,210,231);break;
					case 4:RBC_DrawRect(0, 0, screen->w, screen->h, 167,219,216);break;
					case 6:RBC_DrawRect(0, 0, screen->w, screen->h, 224,228,204);break;
					case 8:RBC_DrawRect(0, 0, screen->w, screen->h, 243,134,48);break;
					case 10:RBC_DrawRect(0, 0, screen->w, screen->h, 250,105,0);break;
					case 12:RBC_DrawRect(0, 0, screen->w, screen->h, 250,70,0);break;
					case 14:RBC_DrawRect(0, 0, screen->w, screen->h, 250,35,0);break;
					case 16:RBC_DrawRect(0, 0, screen->w, screen->h, 250,10,0);break;
					case 18:RBC_DrawRect(0, 0, screen->w, screen->h, 250,0,0);break;

			}
			RBC_DrawRect(245, 0, 310, 600, 0,0,0);
			RBC_DrawRect(250, 0, 300, 600, 85,98,112);
			RBC_DrawRect(570, 70, 210, 210, 0,0,0);
			RBC_DrawRect(575, 75, 200, 200, 200, 200, 200);
			GameBlock.Draw(manager.GetGameBlockX()*30, manager.GetGameBlockY()*30);
			NextBlock.Draw(375, 150);
			manager.DrawTable();
			RBC_Write(575, 300, "Speed:", screen);
			RBC_Write(675, 300, manager.ReturnSpeed(), screen);
			RBC_Write(575, 330, "Lines:", screen);
			RBC_Write(675, 330, manager.ReturnDone(), screen);
			RBC_Write(575, 360, "Score:", screen);
			RBC_Write(675, 360, manager.ReturnScore(), screen);

		}else
		if(gamestatus==2){
			switch(manager.ReturnSpeed()){
					case 2:RBC_DrawRect(0, 0, screen->w, screen->h, 105,210,231);break;
					case 4:RBC_DrawRect(0, 0, screen->w, screen->h, 167,219,216);break;
					case 6:RBC_DrawRect(0, 0, screen->w, screen->h, 224,228,204);break;
					case 8:RBC_DrawRect(0, 0, screen->w, screen->h, 243,134,48);break;
					case 10:RBC_DrawRect(0, 0, screen->w, screen->h, 250,105,0);break;
					case 12:RBC_DrawRect(0, 0, screen->w, screen->h, 250,70,0);break;
					case 14:RBC_DrawRect(0, 0, screen->w, screen->h, 250,35,0);break;
					case 16:RBC_DrawRect(0, 0, screen->w, screen->h, 250,10,0);break;
					case 18:RBC_DrawRect(0, 0, screen->w, screen->h, 250,0,0);break;

			}
			RBC_DrawRect(245, 0, 310, 600, 0,0,0);
			RBC_DrawRect(250, 0, 300, 600, 85,98,112);
			RBC_Write(360, 250, "Pause", screen);
		}else
		if(gamestatus==3){
			RBC_DrawRect((0+frame/2)%screen->w, 0, screen->w/5, screen->h, 105,210,231);
			if(((0+frame/2)%screen->w+screen->w/5)>screen->w){RBC_DrawRect(0, 0, ((0+frame/2)%screen->w+screen->w/5)-screen->w, screen->h, 105,210,231);}

			RBC_DrawRect((screen->w/5+frame/2)%screen->w, 0, screen->w/5, screen->h, 167,219,216);
			if(((screen->w/5+frame/2)%screen->w+screen->w/5)>screen->w){RBC_DrawRect(0, 0, ((screen->w/5+frame/2)%screen->w+screen->w/5)-screen->w, screen->h, 167,219,216);}

			RBC_DrawRect((screen->w/5*2+frame/2)%screen->w, 0, screen->w/5, screen->h, 224,228,204);
			if(((screen->w/5*2+frame/2)%screen->w+screen->w/5)>screen->w){RBC_DrawRect(0, 0, ((screen->w/5*2+frame/2)%screen->w+screen->w/5)-screen->w, screen->h, 224,228,204);}

			RBC_DrawRect((screen->w/5*3+frame/2)%screen->w, 0, screen->w/5, screen->h, 243,134,48);
			if(((screen->w/5*3+frame/2)%screen->w+screen->w/5)>screen->w){RBC_DrawRect(0, 0, ((screen->w/5*3+frame/2)%screen->w+screen->w/5)-screen->w, screen->h, 243,134,48);}

			RBC_DrawRect((screen->w/5*4+frame/2)%screen->w, 0, screen->w/5, screen->h, 250,105,0);
			if(((screen->w/5*4+frame/2)%screen->w+screen->w/5)>screen->w){RBC_DrawRect(0, 0, ((screen->w/5*4+frame/2)%screen->w+screen->w/5)-screen->w, screen->h, 250,105,0);}


					easy.Draw();
					medium.Draw();
					hard.Draw();
		}else
		if(gamestatus==4){
			switch(manager.ReturnSpeed()){
					case 2:RBC_DrawRect(0, 0, screen->w, screen->h, 105,210,231);break;
					case 4:RBC_DrawRect(0, 0, screen->w, screen->h, 167,219,216);break;
					case 6:RBC_DrawRect(0, 0, screen->w, screen->h, 224,228,204);break;
					case 8:RBC_DrawRect(0, 0, screen->w, screen->h, 243,134,48);break;
					case 10:RBC_DrawRect(0, 0, screen->w, screen->h, 250,105,0);break;
					case 12:RBC_DrawRect(0, 0, screen->w, screen->h, 250,70,0);break;
					case 14:RBC_DrawRect(0, 0, screen->w, screen->h, 250,35,0);break;
					case 16:RBC_DrawRect(0, 0, screen->w, screen->h, 250,10,0);break;
					case 18:RBC_DrawRect(0, 0, screen->w, screen->h, 250,0,0);break;

			}
			RBC_DrawRect(245, 0, 310, 600, 0,0,0);
			RBC_DrawRect(250, 0, 300, 600, 85,98,112);
			RBC_DrawRect(570, 70, 210, 210, 0,0,0);
			RBC_DrawRect(575, 75, 200, 200, 200, 200, 200);
			GameBlock.Draw(manager.GetGameBlockX()*30, manager.GetGameBlockY()*30);
			NextBlock.Draw(375, 150);
			manager.DrawTable();
			RBC_Write(575, 300, "Speed:", screen);
			RBC_Write(675, 300, manager.ReturnSpeed(), screen);
			RBC_Write(575, 330, "Lines:", screen);
			RBC_Write(675, 330, manager.ReturnDone(), screen);
			RBC_Write(575, 360, "Score:", screen);
			RBC_Write(675, 360, manager.ReturnScore(), screen);

			for(int i=0; i<screen->w/2; i++){
					for(int j=0; j<screen->h/2; j++){
							RBC_DrawRect(i*2, j*2, 1,1, 0,0,0);
					}
			}


				RBC_Write(300, 300, "Game Over", screen);
				RBC_Write(300, 350, manager.ReturnScore(), screen);

		}else
		if(gamestatus==5){
			RBC_DrawRect((0+frame/2)%screen->w, 0, screen->w/5, screen->h, 105,210,231);
			if(((0+frame/2)%screen->w+screen->w/5)>screen->w){RBC_DrawRect(0, 0, ((0+frame/2)%screen->w+screen->w/5)-screen->w, screen->h, 105,210,231);}

			RBC_DrawRect((screen->w/5+frame/2)%screen->w, 0, screen->w/5, screen->h, 167,219,216);
			if(((screen->w/5+frame/2)%screen->w+screen->w/5)>screen->w){RBC_DrawRect(0, 0, ((screen->w/5+frame/2)%screen->w+screen->w/5)-screen->w, screen->h, 167,219,216);}

			RBC_DrawRect((screen->w/5*2+frame/2)%screen->w, 0, screen->w/5, screen->h, 224,228,204);
			if(((screen->w/5*2+frame/2)%screen->w+screen->w/5)>screen->w){RBC_DrawRect(0, 0, ((screen->w/5*2+frame/2)%screen->w+screen->w/5)-screen->w, screen->h, 224,228,204);}

			RBC_DrawRect((screen->w/5*3+frame/2)%screen->w, 0, screen->w/5, screen->h, 243,134,48);
			if(((screen->w/5*3+frame/2)%screen->w+screen->w/5)>screen->w){RBC_DrawRect(0, 0, ((screen->w/5*3+frame/2)%screen->w+screen->w/5)-screen->w, screen->h, 243,134,48);}

			RBC_DrawRect((screen->w/5*4+frame/2)%screen->w, 0, screen->w/5, screen->h, 250,105,0);
			if(((screen->w/5*4+frame/2)%screen->w+screen->w/5)>screen->w){RBC_DrawRect(0, 0, ((screen->w/5*4+frame/2)%screen->w+screen->w/5)-screen->w, screen->h, 250,105,0);}

			RBC_Write(20, 300, "Use left and right arrow to move block sideways", screen);
			RBC_Write(20, 350, "Use down arrow to move block down faster for extra points", screen);
			RBC_Write(20, 400, "Use space to turn falling tetramino", screen);
			RBC_Write(20, 450, "Hit 'P' to pause game", screen);
		}

        RBC_Write(10, 20, "FPS: ", screen);RBC_Write(70, 20, GetFPS(), screen);

        // DRAWING ENDS HERE

        // finally, update the screen :)
        SDL_Flip(screen);
    } // end main loop

    // free loaded bitmap
	TTF_CloseFont(font);
	TTF_Quit();
    // all is well ;)
    printf("Exited cleanly\n");
    return 0;
}