/*! 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;
}
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;
}