void draw_screen( void ) {
// Step
you_angle += ( 10 * you_velocity * you_turn ) / fps;
float you_x_new = you_x - ( (float)sin(you_angle*piover180) * you_velocity * you_dir ) / fps;
float you_z_new = you_z + ( (float)cos(you_angle*piover180) * you_velocity * you_dir ) / fps;
int joel = boundCheck(you_x_new, you_x_new, you_z_new, you_z_new); // TODO change this to avoid repetition
if (joel == 1) {
you_x = you_x_new;
you_z = you_z_new;
}
glEnableClientState(GL_VERTEX_ARRAY);
glEnableClientState(GL_NORMAL_ARRAY);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Clear The Screen and The Depth Buffer
/* We don't want to modify the projection matrix. */
glMatrixMode( GL_MODELVIEW );
glLoadIdentity(); you_compensate();
// Some nice material/colour settings
glColorMaterial ( GL_FRONT_AND_BACK, GL_EMISSION ) ;
glEnable ( GL_COLOR_MATERIAL ) ;
glLoadIdentity(); you_compensate();
glTranslatef( 0.0f, 3.0f, 0.0f );
draw_cube();
DrawWorld();
if (debug) {
glLoadIdentity(); glTranslatef(0, 0, -1);
glColor3f(1, 1, 1);
glRasterPos2f(-0.7f, -0.5f);
glPrint("X: %f Z: %f Angle: %f Velocity: %f", you_x, you_z, you_angle, you_velocity);
glRasterPos2f(-0.7f, 0.5f);
glPrint("FPS: %f", fps);
}
if (joel == 12) {
glLoadIdentity(); glTranslatef(0, 0, -1);
glColor3f(1, 1, 1);
glRasterPos2f(-0.7f, 0.5f);
glPrint("The Cake,");
glRasterPos2f(-0.7f, 0.4f);
glPrint("Is a Lie!!");
// glRasterPos2f(-0.7f, 0.3f);
// glPrint("Third Line");
}
Frames++;
GLint t = SDL_GetTicks();
if (t - T0 >= 100) {
GLfloat seconds = (t - T0) / 1000.0;
fps = Frames / seconds;
T0 = t;
Frames = 0;
}
SDL_GL_SwapBuffers( );
}
// Method to update location
void Boid::update(vector<Boid> &boids) {
flock(boids);
vel += acc; // Update velocity
vel.x = clamp(vel.x, -maxspeed, maxspeed); // Limit speed
vel.y = clamp(vel.y, -maxspeed, maxspeed); // Limit speed
loc += vel;
acc.setval(0,0); // Resetval accelertion to 0 each cycle
orient = (float)atan2(vel.y,vel.x) * 180/PI;
boundCheck(5); //!@#
}
// Moves the Missile
void Missile::moveObject(){
this->rect.translate(0, -TRANSLATE_POINT);
boundCheck();
}
// Moves the Snowball
void Snowball::moveObject(){
this->rect.translate(0, TRANSLATE_POINT);
boundCheck();
}
void ArmController::joystickData(int X1,int Y1,int LT,int X2,int Y2,int RT)
{
//Dead Zone in joystick
X1 = deadzoneCheck(X1,X1_DEADZONE);
Y1 = deadzoneCheck(Y1,Y1_DEADZONE);
X2 = deadzoneCheck(X2,X2_DEADZONE);
Y2 = deadzoneCheck(Y2,Y2_DEADZONE);
//Single Joint Control
//Joystick 1
int Elbow = Y1*3/32175; //Y Axis
//Joystick 2
int Base = -X2*3/32175; //X Axis
int Shoulder = -Y2*3/32175; //Y Axis
//Trigers
//int Claw = servoList.at(5)->microSeconds; //Triggers
int Wrist = -(LT-RT)*10/32175; ;
//Angle Caluculations
//TODO
int Claw;
int WristR;
if(ClawOn)
{
Claw = X1*50/32157;
WristR = servoList.at(4)->microSeconds;
}else
{
WristR = X1*50/32157; //X Axis
Claw = servoList.at(5)->microSeconds;
}
//TODO Replace Names above with Interger array for for loop
int x[6];
x[0] = Base;
x[1] = Shoulder;
x[2] = Elbow;
x[3] = Wrist;
x[4] = WristR;
x[5] = Claw;
QString dataPart = ""; //Intialist string to build command
//Make sure adding values dont make value go outside of bounds - set to zero if it does
int temp;
for(int i = 0;i < 6; i++)
{
temp = boundCheck(x[i],servoList.at(i)); //bound check
dataPart += QString::number(temp) + "/"; //build datapack
}
//Send DataPack Here
if (dataSent != dataPart)
{
emit Send("40/"+dataPart); //Arm Control has command Identifer 40
dataSent = dataPart;
}else{count++;}
}
int main(int argc, char** argv)
{
bool srcbin = 0;
bool invbk = 0;
if(argc < 3){
printf("Not enough args!\narg1: target image\narg2: source image\narg3: do source image adaptive threshold or not\narg4: invert back ground or not\n");
getchar();
return 1;
}
if(argc >= 4){
if(!strcmp(argv[3], "1"))
srcbin = 1;
}
if(argc >= 5){
if(!strcmp(argv[4], "1"))
invbk = 1;
}
IplImage* srcimg= 0, *srcimgb= 0, *srcimgb2 = 0, *bimg = 0, *b2img = 0,*bugimg = 0, *alg2dst = 0;
srcimg= cvLoadImage(argv[2], 1);
if (!srcimg)
{
printf("src img %s load failed!\n", argv[2]);
getchar();
return 1;
}
//choosing the parameters for our ccl
int bn = 8; //how many partitions
int nwidth = 512;
if(srcimg->width > 512){
nwidth = 1024;
bn = 6;
}
if(srcimg->width > 1024){
nwidth = 2048;
bn = 3;
}
if(srcimg->width > 2048){
printf("warning, image too wide, max support 2048. image is truncated.\n");
getchar();
return 1;
}
//start selection gpu devices
int devCount;
int smCnt = 0;
cudaGetDeviceCount(&devCount);
// Iterate through devices
int devChosen = 0;
for (int i = 0; i < devCount; ++i)
{
cudaDeviceProp devProp;
cudaGetDeviceProperties(&devProp, i);
if(devProp.major >= 2){//only one device supported
smCnt = max(smCnt, devProp.multiProcessorCount);
if(devProp.multiProcessorCount == smCnt)
devChosen = i;
}
}
if(smCnt == 0){
//our ccl require CUDA cap 2.0 or above, but the Ostava's ccl can be run on any CUDA gpu
printf("Error, no device with cap 2.x found. Only cpu alg will be run.\n");
getchar();
return 1;
}
if(smCnt != 0){
cudaSetDevice(devChosen);
bn = bn * smCnt;
}
int nheight = (cvGetSize(srcimg).height-2) / (2*bn);
if((nheight*2*bn+2) < cvGetSize(srcimg).height)
nheight++;
nheight = nheight*2*bn+2;
if(smCnt != 0)
printf("gpu ccl for image width 512, 1024, 2048.\nchoosing device %d, width %d, height %d, blocks %d\n", devChosen, nwidth, nheight, bn);
srcimgb= cvCreateImage(cvSize(nwidth, cvGetSize(srcimg).height),IPL_DEPTH_8U,1);
srcimgb2= cvCreateImage(cvSize(nwidth, cvGetSize(srcimg).height),IPL_DEPTH_8U,1);
cvSetImageROI(srcimg, cvRect(0, 0, min(cvGetSize(srcimg).width, nwidth), cvGetSize(srcimg).height));
cvSetImageROI(srcimgb2, cvRect(0, 0, min(cvGetSize(srcimg).width, nwidth), cvGetSize(srcimg).height));
cvSet(srcimgb2, cvScalar(0,0,0));
cvCvtColor(srcimg, srcimgb2, CV_BGRA2GRAY);
cvResetImageROI(srcimgb2);
cvReleaseImage(&srcimg);
if(srcbin)
cvAdaptiveThreshold(srcimgb2, srcimgb, 1.0, CV_ADAPTIVE_THRESH_MEAN_C, invbk ? CV_THRESH_BINARY_INV : CV_THRESH_BINARY);
else
cvThreshold(srcimgb2, srcimgb, 0.0, 1.0, invbk ? CV_THRESH_BINARY_INV : CV_THRESH_BINARY);
boundCheck(srcimgb);
cvScale(srcimgb, srcimgb2, 255);
//the source binary image to be labeled is saved as bsrc.bmp
cvSaveImage("bsrc.bmp", srcimgb2);
cvSet(srcimgb2, cvScalar(0,0,0));
float elapsedMilliSeconds1;
{//begin cpu labeling algorithm, the SBLA proposed by Zhao
LABELDATATYPE *data=(LABELDATATYPE *)malloc(srcimgb->width * srcimgb->height * sizeof(LABELDATATYPE));
for(int j = 0; j<srcimgb->height; j++)
for(int i = 0; i<srcimgb->width; i++)
data[i + j*srcimgb->width] = (srcimgb->imageData[i + j*srcimgb->widthStep]) ? 1 : 0;
int iNumLabels;
CPerformanceCounter perf;
perf.Start();
iNumLabels = LabelSBLA(data, srcimgb->width, srcimgb->height);
elapsedMilliSeconds1 = (float)perf.GetElapsedMilliSeconds();
printf("cpu SBLA used %f ms, total labels %u\n", elapsedMilliSeconds1, iNumLabels);
free(data);
}
IplImage *src2(0),*dst2(0);
int iNumLabels;
float elapsedMilliSeconds2;
{//begin cpu labeling algorithm, the BBDT proposed by C. Grana, D. Borghesani, R. Cucchiara
CPerformanceCounter perf;
src2 = cvCreateImage( cvGetSize(srcimgb), IPL_DEPTH_8U, 1 );
cvCopyImage(srcimgb,src2);
dst2 = cvCreateImage( cvGetSize(srcimgb), IPL_DEPTH_32S, 1 );
perf.Start();
cvLabelingImageLab(src2, dst2, 1, &iNumLabels);
elapsedMilliSeconds2 = (float)perf.GetElapsedMilliSeconds();
printf("cpu BBDT used %f ms, total labels %u\n", elapsedMilliSeconds2, iNumLabels);
cvSaveImage("bbdt.bmp", dst2);
// cvReleaseImage(&src2);
// cvReleaseImage(&dst2);
}
if(smCnt != 0){
bugimg = cvCreateImage(cvSize(nwidth, 9*bn),IPL_DEPTH_8U,1);
bimg = cvCreateImage(cvSize(nwidth, 2*bn),IPL_DEPTH_8U,1);
b2img = cvCreateImage(cvSize(nwidth, 2*bn),IPL_DEPTH_8U,1);
// cvNamedWindow("src",CV_WINDOW_AUTOSIZE);
// cvShowImage("src",srcimg);
//prepare buffers for our gpu algorithm
CudaBuffer srcBuf, dstBuf, dstBuf2, bBuf, b2Buf, errBuf, glabel;
srcBuf.Create2D(nwidth, nheight); //the binary image to be processed
dstBuf.Create2D(nwidth, (nheight-2)/2); //the label result, only about 1/4 the size of source image contains the final labels
dstBuf2.Create2D(nwidth,(nheight-2)/2); //a copy of the pass1 temp result, for debug purpose
glabel.Create2D(4, 1); //a int size global buffer for unique final label
errBuf.Create2D(nwidth, 9*bn); //a buffer for debug info
bBuf.Create2D(nwidth, 2 * bn); //the intersection info used by pass2
b2Buf.Create2D(nwidth, 2 * bn); //a copy of bBuf for debug purpose
srcBuf.SetZeroData();
srcBuf.CopyFrom(srcimgb->imageData, srcimgb->widthStep, nwidth, cvGetSize(srcimgb).height);
float elapsedTimeInMs = 0.0f;
//-------------------gpu part----------------------------
cudaEvent_t start, stop;
cutilSafeCall ( cudaEventCreate( &start ) );
cutilSafeCall ( cudaEventCreate( &stop ) );
cutilSafeCall( cudaEventRecord( start, 0 ) );
if(nwidth == 512)
label_512(&dstBuf, &dstBuf2, &srcBuf, &bBuf, &b2Buf, &glabel, nheight, bn, &errBuf);
else if(nwidth == 1024)
label_1024(&dstBuf, &dstBuf2, &srcBuf, &bBuf, &b2Buf, &glabel, nheight, bn, &errBuf);
else if(nwidth == 2048)
label_2048(&dstBuf, &dstBuf2, &srcBuf, &bBuf, &b2Buf, &glabel, nheight, bn, &errBuf);
cutilSafeCall( cudaEventRecord( stop, 0 ) );
// cutilCheckMsg("kernel launch failure");
cudaEventSynchronize(stop);
cutilSafeCall( cudaEventElapsedTime( &elapsedTimeInMs, start, stop ) );
uint tlabel = 0;
cudaMemcpy(&tlabel, glabel.GetData(), 4, cudaMemcpyDeviceToHost);
printf("gpu alg 1 used %f ms, total labels %u\n", elapsedTimeInMs, tlabel);
dstBuf.CopyToHost(srcimgb->imageData, srcimgb->widthStep, nwidth, (nheight-2)/2);
dstBuf2.CopyToHost(srcimgb2->imageData, srcimgb->widthStep, nwidth, (nheight-2)/2);
errBuf.CopyToHost(bugimg->imageData, bugimg->widthStep, nwidth, 9*bn);
bBuf.CopyToHost(bimg->imageData, bimg->widthStep, nwidth, 2*bn);
b2Buf.CopyToHost(b2img->imageData, bimg->widthStep, nwidth, 2*bn);
// cvNamedWindow("gpu",CV_WINDOW_AUTOSIZE);
// cvShowImage("gpu",srcimgb);
cvSaveImage(argv[1], srcimgb);
cvSaveImage("gpu2.bmp", srcimgb2); //the final labels of our algorithm
cvSaveImage("bug.bmp", bugimg);
cvSaveImage("b.bmp", bimg);
cvSaveImage("b2.bmp", b2img);
//now start the gpu ccl implemented by Ostava
alg2dst= cvCreateImage(cvSize(nwidth*4, cvGetSize(srcimgb).height),IPL_DEPTH_8U,1);
CCLBase* m_ccl;
m_ccl = new CCL();
m_ccl->FindRegions(nwidth, cvGetSize(srcimgb).height, &srcBuf);
m_ccl->GetConnectedRegionsBuffer()->CopyToHost(alg2dst->imageData, alg2dst->widthStep, nwidth*4, cvGetSize(srcimgb).height);
delete m_ccl;
cvSaveImage("alg2.bmp", alg2dst);
cvReleaseImage(&bugimg);
cvReleaseImage(&bimg);
cvReleaseImage(&b2img);
cvReleaseImage(&alg2dst);
// }
//cvWaitKey(0);
//now start cross compare label results of our ccl and the BBDT, to check the correctness
// if(smCnt != 0){
ushort *gpures, *cpures;
uint sz = nwidth * (cvGetSize(srcimgb).height/2);
gpures = (ushort*)malloc(sz);
cpures = (ushort*)malloc(sz);
dstBuf.CopyToHost(gpures, nwidth, nwidth, (cvGetSize(srcimgb).height/2));
//first, reduce cpu labels from one label for each pixel to one label for a 2x2 block, assuming 8-connectivity
for(int j = 0; j < (cvGetSize(srcimgb).height/2); j++)
for(int i = 0; i < (nwidth/2); i++){
uint* cpup;
ushort res = LBMAX;
uint y = j*2, x = i*2;
cpup = (uint*)(dst2->imageData + y*dst2->widthStep);
// if(y < cvGetSize(srcimgb).height){
if(cpup[x] != 0)
res = cpup[x]-1;
if(cpup[x+1] != 0)
res = cpup[x+1]-1;
// }
y++;
cpup = (uint*)(dst2->imageData + y*dst2->widthStep);
// if(y < cvGetSize(srcimgb).height){
if(cpup[x] != 0)
res = cpup[x]-1;
if(cpup[x+1] != 0)
res = cpup[x+1]-1;
// }
cpures[i + j*(nwidth/2)] = res;
}
//our algo use unsigned short to represent a label, the first label starts a 0, and maximun labels is LBMAX
if(iNumLabels > LBMAX)
printf("too much cc, compare abort.\n");
else{
//create a error
//cpures[5] = 12;
//cpures[15] = 18;
printf("Checking correctness of gpu alg1\nChecking gpu ref by cpu.\n");
checkLabels(cpures, gpures, nwidth/2, cvGetSize(srcimgb).height/2, iNumLabels);
printf("Checking cpu ref by gpu.\n");
checkLabels(gpures, cpures, nwidth/2, cvGetSize(srcimgb).height/2, tlabel);
}
free(gpures);
free(cpures);
printf("speedup is %f, %f, %f\n", gpu2time/elapsedTimeInMs, elapsedMilliSeconds1/elapsedTimeInMs, elapsedMilliSeconds2/elapsedTimeInMs);
}
cvReleaseImage(&srcimgb);
cvReleaseImage(&srcimgb2);
cvReleaseImage(&dst2);
cvReleaseImage(&src2);
cutilSafeCall( cudaThreadExit() );
return 0;
}
