void Basker<Int,Entry,Exe_Space>::DEBUG_PRINT() { //print_sep_bal(); #ifdef BASKER_2DL printL2D(); printLMTX(); #else //printL(); #endif std::cout << "L printed " << std::endl; printU(); printUMTX(); std::cout << "U printed" << std::endl; //printRHS(); std::cout << "RHS printed" << std::endl; //printSOL(); std::cout << "SOL printed" << std::endl; //printTree(); std::cout << "Tree printed" << std::endl; //Print out vectors if(match_flag == BASKER_TRUE) { printVec("match.csc", order_match_array, order_match_array.dimension_0()); } if(btf_flag == BASKER_TRUE) { printVec("btf.csc", order_btf_array, order_btf_array.dimension_0()); printVec("amdblk.csc", order_blk_amd_array, order_blk_amd_array.dimension_0()); } if(btf_tabs_offset != 0) { printVec("ND.csc", part_tree.permtab, part_tree.permtab.dimension_0()); } if(amd_flag == BASKER_TRUE) { printVec("camd.csc", order_csym_array, order_csym_array.dimension_0()); } }//end DEBUG_PRINT()
int main( int argn, char *arg[] ) { srand( 1234567 ); omp_set_num_threads(2); std::cout << "Number columns: " << N_COL << std::endl; std::cout << "Number rows: " << N_ROW << std::endl; std::cout << "Total Number Values: " << N_ROW * N_COL << std::endl; std::cout << "Number Non-Zeros: " << NNZ << std::endl; std::cout << "Max Number Threads: " << omp_get_max_threads() << std::endl; // allocate some memory // ... for matrix and fill it with random values double Aval[NNZ] = {1.0, 3.0, 4.0, 2.0, 5.0, 2.0, 1.0}; int AcolInd[NNZ] = {1, 3, 2, 0, 4, 2, 3}; int ArowPt[N_ROW+1] = {0, 2, 3, 5, 6}; // ... for vector and fill it with random, non-zero, values double Vval[N_COL] = {1.0, 3.0, 4.0, 2.0, 3.0}; // ... for result and make sure, it's zero everywhere double Yval[N_ROW]; for ( int i = 0; i < N_ROW; i++ ) { Yval[i] = 0; } // print input if dimenions not too high if ( N_COL < 10 && N_ROW < 10 && NNZ < 15 ) { printSparseMat( Aval, AcolInd, ArowPt ); printVec( Vval, N_COL ); } // multiply --- here we go! mxv( Aval, AcolInd, ArowPt, Vval, Yval ); // print result if dimenions not too high if ( N_ROW < 10 ) { printVec( Yval, N_ROW ); } // print squared norm of solution vector as a measurement for correctness double sqnorm = 0; for ( int i = 0; i < N_ROW; i++ ) { sqnorm += Yval[i] * Yval[i]; } std::cout << "Squared Norm of Y is: " << sqnorm << std::endl; }
void cross4h(double u[], double v0[], double v1[]){ u[0] = v0[1] * v1[2] - v0[2] * v1[1]; u[1] = v0[2] * v1[0] - v0[0] * v1[2]; u[2] = v0[0] * v1[1] - v0[1] * v1[0]; #ifdef DEBUG printf("外積:"); printVec(u); #endif }
int main(int argc, char** argv) { /* Declarations for calculations */ // Dynamically generated vectors int p; int my_rank; int i,j; double tempD; int tempI; char * pEnd; int n = 12;//atoi(argv[1]); MPI_Init(&argc, &argv); MPI_Comm_rank(MPI_COMM_WORLD, &my_rank); MPI_Comm_size(MPI_COMM_WORLD, &p); int nDivP = n/p; double avg; int myAbove = 0; int above; srand(time(NULL) - my_rank); double sum; double mySum; double* IQvec = malloc(sizeof(double)*nDivP); for (i = 0; i < nDivP; i++) { IQvec[i] = rand() % 20 + 90; mySum += IQvec[i]; } printVec(IQvec,nDivP,my_rank); // MPI_Reduce(&mySum,&sum,1,MPI_DOUBLE,MPI_SUM,0,MPI_COMM_WORLD); MPI_Bcast(&sum,1,MPI_DOUBLE,0,MPI_COMM_WORLD); avg = sum / n; // Everybody for (j = 0; j < nDivP; j++) if (IQvec[j] > avg) myAbove++; MPI_Reduce(&myAbove,&above,1,MPI_INT,MPI_SUM,0,MPI_COMM_WORLD); if (my_rank == 0) printf("\nSum = %lf. Above = %d. Avg = %lf \n\n",sum, above, avg); free(IQvec); MPI_Finalize(); return 0; }
void loop() { //Variable Initialization time ++; api.getMyZRState(myState); api.getOtherZRState(otherState); phase = game.getCurrentPhase(); fuel = game.getFuelRemaining(); myScore = game.getScore(); otherScore = game.getOtherScore(); dust = game.getRemainingMaterial(); chg = game.getCharge(); if (time < 2) { red = statePos(myState)[0] < floatZero; } //Debug Console DEBUG(("\nDevilTech Message Screen: Fall 2012\n")); #if printDebug DEBUG(("Debug Info\n")); DEBUG(("\tTime (s)\t%d\tPhase\t%d\tPlayer\t%s\n", time, phase, (red ? "Red" : "Blue"))); DEBUG(("\tFuel\t%.3f\tDust\t%.3f\tChg\t%d\n", fuel, dust, chg)); DEBUG(("\tScore\t%.2f\tOther Score\t%.2f\n", myScore, otherScore)); DEBUG(("\tOther Message\t%u\n", otherMessage)); printVec("\tMy Pos (m)", statePos(myState)); printVec("\tMy Vel (m/s)", stateVel(myState)); printVec("\tMy Att (unitvec)", stateAtt(myState)); printVec("\tMy AngVel (rad/s)", stateAngVel(myState)); printVec("\tOther Pos (m)", statePos(otherState)); printVec("\tOther Vel (m/s)", stateVel(otherState)); printVec("\tOther Att (unitvec)", stateAtt(otherState)); printVec("\tOther AngVel (rad/s)", stateAngVel(otherState)); #endif //Game Logic vec temp; game.getItemLocation(2, temp); if (!game.haveObject(0)) getItem(0);/* else if (!game.haveObject(1) && !game.otherHasObject(1) && count2 < 25) { getItem(1); count2 ++; }*/ else if (!game.haveObject(2) && !game.otherHasObject(2) && disBetweenPts(otherState, temp) > 0.075f && count3 < 25) { getItem(2); count3 ++; } else { smartGo(red ? 0.055f : -0.055f, -0.065f); if (phase == 3) removeObs(); } }
int main() { std::vector<int> vec { -10, -1, 0, 3, 10, 11, 30, 50, 100 }; printVec(vec); int fixedPoint = binarySearchFixedPoint(vec); if (fixedPoint != -1) { std::cout << "Above vector has a fixed point at index(0 indexed): " << fixedPoint << std::endl; } else { std::cout << "Above vector has no fixed point\n"; } return 0; }
int main() { vector<int> vec; for (int i = 0; i < 10; ++i) { vec.push_back(i); } printVec(vec); for (vector<int>::iterator iter = vec.begin(); iter != vec.end(); ++iter) { *iter *= 2; } printVec(vec); cout << vec.end() - vec.begin() << endl; //vector<int>::iterator iter2 = (vec.begin() + vec.end())/2; return 0; }
//END::PAGE::adef //BEGIN::PAGE::ctrl void overshoot(vec targetPos) { vec nPos, att; float dis = disBetweenPts(statePos(myState), targetPos); attToTarget(statePos(myState), targetPos, att); nPos[0] = targetPos[0] + att[0] * osMag * dis; //set target location beyond target nPos[1] = targetPos[1] + att[1] * osMag * dis; nPos[2] = targetPos[2] + att[2] * osMag * dis; float velComponent = mathVecInner(stateVel(myState), att, 3); float disLeft = dis - stillDis; float velDis = disToHalt(velComponent); #if printDebug DEBUG(("Overshoot Info\n")); DEBUG(("\tOvershoot Magnitude\t%.3f\n", osMag)); DEBUG(("\tOvershoot Threshold\t%.3f\n", lim)); DEBUG(("\tDis to Target (m)\t%.3f\n", dis)); DEBUG(("\tVel Component (m/s)\t%.3f\n", velComponent)); DEBUG(("\tCur Vel Dis (m)\t%.3f\n", velDis)); printVec("\tTarget Pos (m)", targetPos); printVec("\tTarget Att (unitvec)", att); printVec("\tNew Pos (m)", nPos); #endif if (osMag <= floatZero || hitTarget(myState, targetPos)) { #if printDebug DEBUG(("\tsetPositionTarget\n")); #endif api.setPositionTarget(targetPos); } else if (dis < lim && velDis > disLeft + 0.001f) { #if printDebug DEBUG(("\tHalting\n")); #endif api.setVelocityTarget(zeroVec); } else { //overshoot #if printDebug DEBUG(("\tSpeeding Up\n")); #endif api.setPositionTarget(nPos); } }
static void test_lemke() { real * A = makeRandMatrix(); real * b = makeRandVec2(); real * x = (real *)malloc(2*NN*sizeof(real)); real * xx = (real *)malloc(2*NN*sizeof(real)); real * AA = makeMatrix(); real * bb = makeRandVec2(); std::vector<real *> vx; // copyMatrix(A,exaples_m2); // for(int i =0;i<N;i++) // b[i] = exaples_q2[i]; copyMatrix(AA,A); memcpy(bb,b,NN*sizeof(real)); //memset(x,0,sizeof(real)*N); memset(x,0,NN*sizeof(real)); //memcpy(xx,b,N*sizeof(real)); memset(xx,0,sizeof(real)*NN); printf("--------------------------------------------------------\n"); printMat("solve lcp A=",A); printVec("lcp b=",b); printf("--------------------------------------------------------\n"); int result1 = lcp(A,b,vx,NN); int result2 = lcp_lemke(A,b,x,NN); printf("lcp solve:\n"); printf("--------------------------------------------------------\n"); printLCPVx(vx); printf("lcp_lemke solve %s (%d)\n",result2?"true":"false",result2); printVec("lcp_lemke=",x); check_lcp_result(AA,bb,x,NN); freeMatrix(A); freeMatrix(b); freeMatrix(x); freeMatrix(AA); freeMatrix(bb); freeLcpSolve(vx); }
int main() { vector<int> nums; int i=0; for(;i<15;i++) nums.push_back(rand()%100); vector<vector<int> >res=subsets(nums); for(i=0;i<(signed)res.size();i++) printVec(res[i]); return 0; }
static void test_pgs() { real * A = makeRandSPDMatrix(); real * b = makeRandVec2(); real * x = makeRandVec2(); real * xx = makeRandVec2(); real * AA = makeMatrix(); real * bb = makeRandVec2(); std::vector<real *> vx; copyMatrix(AA,A); memcpy(bb,b,NN*sizeof(real)); //memset(x,0,sizeof(real)*N); memset(x,0,NN*sizeof(real)); //memcpy(xx,b,N*sizeof(real)); memset(xx,0,sizeof(real)*NN); printf("--------------------------------------------------------\n"); printMat("solve lcp A=",A); printVec("lcp b=",b); printf("--------------------------------------------------------\n"); int result1 = lcp(A,b,vx,NN); int result2 = lcp_pgs(A,b,x,NN,15,0.001); int result3 = Solve_GaussSeidel(AA,bb,xx,NN,15); printf("lcp solve:\n"); printf("--------------------------------------------------------\n"); printLCPVx(vx); printf("lcp_pgs solve %s (%d)\n",result2?"true":"false",result2); printVec("lcp_pgs=",x); check_lcp_result(AA,bb,x,NN); printVec("gs solve :",xx); freeMatrix(A); freeMatrix(b); freeMatrix(x); freeMatrix(AA); freeMatrix(bb); freeLcpSolve(vx); }
BASKER_INLINE int Basker<Int,Entry,Exe_Space>::serial_backward_solve ( ENTRY_1DARRAY y, ENTRY_1DARRAY x ) { #ifdef BASKER_DEBUG_SOLVE_RHS printf("called serial backward solve \n"); #endif for(Int b = tree.nblks-1; b >=0; b--) { //printf("--HERE-\n"); #ifdef BASKER_DEBUG_SOLVE_RHS printf("Upper solve blk: %d \n", b); #endif BASKER_MATRIX &U = LU(b)(LU_size(b)-1); //printf("\n--------U------------\n"); //U.print(); //U\y -> x upper_tri_solve(U,y,x); for(Int bb = LU_size(b)-2; bb >= 0; bb--) { #ifdef BASKER_DEBUG_SOLVE_RHS printf("Upper solver spmv: %d %d \n", b, bb); #endif BASKER_MATRIX &UB = LU(b)(bb); //y = UB*x; neg_spmv(UB,x,y); } }//end over all blks #ifdef BASKER_DEBUG_SOLVE_RHS printf("Done with Upper Solve: \n"); printVec(x, gn); #endif return 0; }//end serial_backward_solve()
void testRotsNShifts(const FHEPubKey& publicKey, const FHESecKey& secretKey, const EncryptedArrayCx& ea, double epsilon) { std::srand(std::time(0)); // set seed, current time. int nplaces = rand() % static_cast<int>(ea.size()/2.0) + 1; if (verbose) cout << "Test Rotation of " << nplaces << ": "; Ctxt c1(publicKey); vector<cx_double> vd1; vector<cx_double> vd_dec; ea.random(vd1); ea.encrypt(c1, publicKey, vd1, /*size=*/1.0); #ifdef DEBUG_PRINTOUT printVec(cout<< "vd1=", vd1, 10)<<endl; #endif std::rotate(vd1.begin(), vd1.end()-nplaces, vd1.end()); ea.rotate(c1, nplaces); ea.decrypt(c1, secretKey, vd_dec); #ifdef DEBUG_PRINTOUT printVec(cout<< "vd1(rot)=", vd1, 10)<<endl; printVec(cout<<"res: ", vd_dec, 10)<<endl; #endif if (cx_equals(vd1, vd_dec, conv<double>(epsilon*c1.getPtxtMag()))) cout << "GOOD\n"; else { cout << "BAD:\n"; std::cout << " max(vd)="<<largestCoeff(vd_dec) << ", max(vd1)="<<largestCoeff(vd1) << ", maxDiff="<<calcMaxDiff(vd_dec,vd1) << endl<<endl; } }
bool makeObs (vec loc) { #if printDebug DEBUG(("makeObs()\n")); printVec("\tAt loc (m):", loc); #endif if (!cloudOn) { targetAtt[0] = -myState[6]; targetAtt[1] = -myState[7]; targetAtt[2] = -myState[8]; game.startObstacle(); cloudOn = true; } else if (hitTarget(myState, loc)) { game.stopObstacle(); cloudOn = false; return true; } #if printDebug printVec("\tTarget att", targetAtt); #endif api.setAttitudeTarget(targetAtt); api.setPositionTarget(loc); return false; }
BASKER_INLINE int Basker<Int,Entry,Exe_Space>::serial_forward_solve ( ENTRY_1DARRAY y, ENTRY_1DARRAY x ) { #ifdef BASKER_DEBUG_SOLVE_RHS printf("Called serial forward solve \n"); #endif //Forward solve on A for(Int b = 0; b < tree.nblks; ++b) //for(Int b = 0; b < 1; ++b) { #ifdef BASKER_DEBUG_SOLVE_RHS printf("Lower Solve blk: %d \n",b); #endif BASKER_MATRIX &L = LL(b)(0); //L\y -> x lower_tri_solve(L, y, x); //Update offdiag for(Int bb = 1; bb < LL_size(b); ++bb) { #ifdef BASKER_DEBUG_SOLVE_RHS printf("Lower Solver Update blk: %d %d \n", b, bb); #endif BASKER_MATRIX &LD = LL(b)(bb); //y = LD*x; neg_spmv_perm(LD, x, y); } //printf("spmv test\n"); //printVec(y,gn); } #ifdef BASKER_DEBUG_SOLVE_RHS printf("Done forward solve A \n"); printVec(x, gn); #endif return 0; }//end serial_forward_solve()
void setVelocity (vec targetVel) { #if printDebug DEBUG(("Velocity Targeting Info\n")); printVec("Target Vel (m)", targetVel); #endif vec temp; mathVecSubtract(temp, targetVel, stateVel(myState), 3); float magnitude = mathVecMagnitude(temp, 3); if (magnitude < 0.02f) { api.setVelocityTarget(targetVel); return; } vecScale(temp, 4.2f); magnitude = fabs(mathVecMagnitude(temp, 3)); if (magnitude > 0.11f) vecScale(temp, 0.11f / magnitude); api.setForces(temp); }
void getItem (int itemNo) { #if printDebug DEBUG(("getItem(%d) Info\n", itemNo)); #endif vec iloc; game.getItemLocation(itemNo, iloc); #if printDebug printVec("\tItemPos (m)", iloc); #endif api.setPositionTarget(iloc); if (hitTarget(myState, iloc)) { vec temp = {0.75f, floatZero, floatZero}; setTargetAngVel(temp); } else { setTargetAngVel(zeroVec); } }
int main(int argc, char** argv) { printVec(glm::dvec3(1, 2, 3)); //init GLUT and create windows glutInit(&argc, argv); glutInitWindowSize(1024, 768); glutInitWindowPosition(500, 110); glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH); glutCreateWindow("SPH"); init(); glutReshapeFunc(resize); glutDisplayFunc(display); glutSpecialFunc(processSpecialKeys); glutIdleFunc(idle); // enter GLUT event processing cycle glutMainLoop(); }
void getItem (int itemNo) { #if printDebug DEBUG(("getItem(%d) Info\n", itemNo)); #endif if (game.getCurrentPhase() == 1) { smartGo(red ? -0.45 : 0.45, statePos(myState)[1] < -0.05f ? 0.073f : 0.0505f); #if printDebug DEBUG(("\tMoving to Zone 2\n")); #endif return; } vec iloc; game.getItemLocation(itemNo, iloc); #if printDebug printVec("\tItemPos (m)", iloc); #endif overshoot(iloc); vec degs = {floatZero, floatZero, 0.62f}; setTargetAngVel(hitTarget(myState, iloc) ? degs : zeroVec); }
void remDust () { int i = obsDetected = game.getIdentifiedObstacles(obstacles); api.setAttitudeTarget(makeVec(floatZero, -1.0f, floatZero)); #if printDebug DEBUG(("remDust()\n")); DEBUG(("\tDetected\t%d\n", obsDetected)); #endif while (i -- && chg) { #if printDebug DEBUG(("\tObstacle ID\t%d\tRadius\t%.3f\n", obstacles[i].ID, obstacles[i].size)); printVec("\t\tObstacle Location", obstacles[i].loc); #endif if (obstacles[i].visible) { #if printDebug DEBUG(("\t\tShrinking\n")); #endif game.shrinkObstacle(obstacles[i].ID); return; } } }
void getItem (int itemNo, float x0, float x1, float y0, float y1, float z0, float z1, float acc) { float best = floatZero; int lim = 40; if (time < lim) lim = time; for (float x = x0; x < x1 + floatItsml; x += acc) { for (float y = y0; y < y1 + floatItsml; y += acc) { for (float z = z0; z < z1 + floatItsml; z += acc) { float temp = floatZero; for (int i = 0; i <= lim; i ++) { float diff = fabs(tridis[i][itemNo] - disBetweenPts(makeVec(x, y, z), tripos[i])); if (diff < 0.0101f) temp += floatOne - diff; } if (temp > best) { iloc[0] = x; iloc[1] = y; iloc[2] = z; best = temp; } } } } #if printDebug DEBUG(("getItem(%d)\n", itemNo)); printVec("\tItemPos (m)", iloc); DEBUG(("\tAccuracy Score\t%.3f\n", best / (time + 1.0f))); DEBUG(("\tVel Magnitude (m/s)\t%.3f\n", mathVecMagnitude(myState + 3, 3))); DEBUG(("\tAngvel Magnitude (deg/s)\t%.3f\n", mathVecMagnitude(myState + 9, 3) * 57.296f)); DEBUG(("\tDis from Item (m)\t%.3f\n", disBetweenPts(myState, iloc))); #endif api.setPositionTarget(iloc); if (hitTarget(myState, iloc)) { #if printDebug DEBUG(("\tHit target\n")); #endif setTargetAngVel(makeVec(floatZero, floatZero, pickUpSpeed)); } else { setTargetAngVel(zeroVec); } }
glm::mat4 lookAt4D(const glm::vec4 &from, const glm::vec4 &to, const glm::vec4 &up, const glm::vec4 &over) { glm::mat4 viewMat; glm::vec4 A, B, C, D; D = glm::normalize(from - to); std::cout << "D "; printVec(D, 4); std::cout << "A "; A = glm::normalize(cross4D(up, over, D)); std::cout << "B "; B = glm::normalize(cross4D(over, D, A)); std::cout << "C "; C = cross4D(D, A, B); viewMat[0] = A; viewMat[1] = B; viewMat[2] = C; viewMat[3] = D; // I hope this preserves "right-handedness" //assert(glm::normalize(cross4D(A, B, C)) == D); return glm::transpose(viewMat); }
void NeuralNetwork::addAllTrainData(vector <Vec > ins, vector< Vec> outs){ double maxError = 100000 ; double meanError = 100000; double prevError =0; Vec outp; int printRate = PRINTRATE; int steps=0; while(meanError>ERROR_THRESHOLD && steps!=MAXSTEPS){ meanError = 0; maxError = 0; steps++; for(int i =0;i< ins.size();i++){ addTrainData(ins[i],outs[i]); outp = layers[noOfLayers-1].getOutput(); sub(outp , outs[i]); maxError = max(maxError , mod(outp)); meanError += mod(outp); //cout<<mod(outp)<<" "<<endl; } if(!printRate--){ printf("%d mean:%f max:%f\n",steps, meanError , maxError); printVec(outp); cout<<endl; printRate =PRINTRATE; } if(meanError<prevError) NETA = NETA*1.2; else NETA = NETA/2; prevError = meanError; } printf("Training Done Error %f \n",meanError); printf("Total steps : %d\n",steps); printf("\n\n"); }
// FUNCTION ====== setup void setup() { //---- SDL initialization if ( SDL_Init(SDL_INIT_VIDEO) != 0 ) { printf("Unable to initialize SDL: %s\n", SDL_GetError()); getchar() ; } SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1); //screen = SDL_SetVideoMode( WIDTH, HEIGHT, COLDEPTH, SDL_OPENGL | SDL_SWSURFACE ); screen = SDL_SetVideoMode( WIDTH, HEIGHT, COLDEPTH, SDL_OPENGL ); if(!screen) { printf("Couldn't set %dx%d GL video mode: %s\n", WIDTH,HEIGHT, SDL_GetError()); SDL_Quit(); exit(2); } SDL_WM_SetCaption(" Prokop's test SDL+ OpenGL", "SDL+ OpenGL"); //materialy float ambient [] = { 0.25f, 0.25f, 0.25f, 1.0f}; glMaterialfv (GL_FRONT_AND_BACK, GL_AMBIENT, ambient); float diffuse [] = { 0.50f, 0.50f, 0.50f, 1.0f}; glMaterialfv (GL_FRONT_AND_BACK, GL_DIFFUSE, diffuse); float specular [] = { 0.00f, 0.00f, 0.00f, 1.0f}; glMaterialfv (GL_FRONT_AND_BACK, GL_SPECULAR, specular); float shininess[] = { 0.0f }; glMaterialfv (GL_FRONT_AND_BACK, GL_SHININESS, shininess); glEnable (GL_LIGHT0); float light0_pos [] = { 1000.0f, 2000.0f, 3000.0f, 1.00f}; glLightfv (GL_LIGHT0, GL_POSITION, light0_pos ); float light0_color[] = { 0.9f, 0.8f, 0.7f, 1.00f}; glLightfv (GL_LIGHT0, GL_DIFFUSE, light0_color ); float light0_ambient[] = { 0.5f, 0.6f, 0.7f, 1.00f}; glLightfv (GL_LIGHT0, GL_AMBIENT, ambient ); glEnable (GL_LIGHTING); glEnable (GL_COLOR_MATERIAL); glEnable (GL_NORMALIZE); glEnable (GL_DEPTH_TEST); //glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); //glEnable(GL_BLEND) ; //glBlendFunc (GL_SRC_ALPHA, GL_SRC_ALPHA); glHint(GL_LINE_SMOOTH_HINT, GL_NICEST); glShadeModel(GL_SMOOTH); glPolygonMode(GL_FRONT_AND_BACK,GL_FILL); glClearColor( 0.9, 0.9, 0.9, 0.0); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); qmouse.setOne(); srand(1234); buildings = makeBuildings( 10, 10, 100, 100, 0.5, 0.5, 50, 100 ); const int len = 5; double masses[len] = { 4,1,1,0.5f,0.5f }; Vec3d poss[len] = { {0,-0.2,1}, {-2,0,0}, {2,0,0}, {0,0,-3}, {0,0.2,-3} }; myCraft.from_mass_points( len, masses, poss ); myCraft.qrot.setOne(); //myCraft.qrot.set(0,0,-0.5,1); myCraft.qrot.normalize(); printVec(myCraft.pos); printf("pos\n"); myCraft.wingLeft .craft = &myCraft; myCraft.wingRight.craft = &myCraft; myCraft.elevator .craft = &myCraft; myCraft.rudder .craft = &myCraft; myCraft.wingLeft .lpos.set( poss[1] - myCraft.pos ); myCraft.wingRight.lpos.set( poss[2] - myCraft.pos ); myCraft.elevator .lpos.set( poss[3] - myCraft.pos ); myCraft.rudder .lpos.set( poss[4] - myCraft.pos ); /* myCraft.wingLeft .lpos.set( poss[1] ); myCraft.wingRight.lpos.set( poss[2] ); myCraft.elevator .lpos.set( poss[3] ); myCraft.rudder .lpos.set( poss[4] ); */ myCraft.wingLeft .lrot.set( { 1,0,0, 0,1,0, 0,0,1 } ); myCraft.wingLeft .lrot.rotate( -0.1, { 0,0,1 } ); myCraft.wingRight.lrot.set( { 1,0,0, 0,1,0, 0,0,1 } ); myCraft.wingRight .lrot.rotate( +0.1, { 0,0,1 } ); myCraft.elevator .lrot.set( { 1,0,0, 0,1,0, 0,0,1 } ); myCraft.elevator .lrot.rotate( +0.2, { 1,0,0 } ); printf("elevator lrot\n"); printMat( myCraft.elevator.lrot ); myCraft.rudder .lrot.set( { 0,1,0, 1,0,0, 0,0,1 } ); myCraft.wingLeft .C.set( 0.05, 1.0, 0.05 ); myCraft.wingRight.C.set( 0.05, 1.0, 0.05 ); myCraft.elevator .C.set( 0.05, 1.0, 0.05 ); myCraft.elevator.C *= 0.1; myCraft.rudder .C.set( 0.05, 1.0, 0.05 ); myCraft.rudder .C *= 0.1; myCraft.L.set(0,0,0); myCraft.init( ); myCraft.vel.set(0,0,0); myCraft.pos.set(0,500,0); printf("Ibody\n"); printMat(myCraft.Ibody); printf("invIbody\n"); printMat(myCraft.invIbody); }
ostream& printZZX(ostream& s, const ZZX& poly, long nCoeffs) { return printVec(s, poly.rep, nCoeffs); }
SEXP spMisalign(SEXP Y_r, SEXP X_r, SEXP p_r, SEXP n_r, SEXP m_r, SEXP coordsD_r, SEXP betaPrior_r, SEXP betaNorm_r, SEXP KPrior_r, SEXP KPriorName_r, SEXP PsiPrior_r, SEXP nuUnif_r, SEXP phiUnif_r, SEXP phiStarting_r, SEXP AStarting_r, SEXP PsiStarting_r, SEXP nuStarting_r, SEXP phiTuning_r, SEXP ATuning_r, SEXP PsiTuning_r, SEXP nuTuning_r, SEXP nugget_r, SEXP covModel_r, SEXP amcmc_r, SEXP nBatch_r, SEXP batchLength_r, SEXP acceptRate_r, SEXP verbose_r, SEXP nReport_r){ /***************************************** Common variables *****************************************/ int h, i, j, k, l, b, s, ii, jj, kk, info, nProtect= 0; char const *lower = "L"; char const *upper = "U"; char const *nUnit = "N"; char const *yUnit = "U"; char const *ntran = "N"; char const *ytran = "T"; char const *rside = "R"; char const *lside = "L"; const double one = 1.0; const double negOne = -1.0; const double zero = 0.0; const int incOne = 1; /***************************************** Set-up *****************************************/ double *Y = REAL(Y_r); double *X = REAL(X_r); int *p = INTEGER(p_r); int *n = INTEGER(n_r); int m = INTEGER(m_r)[0]; int nLTr = m*(m-1)/2+m; int N = 0; int P = 0; for(i = 0; i < m; i++){ N += n[i]; P += p[i]; } int mm = m*m; int NN = N*N; int NP = N*P; int PP = P*P; double *coordsD = REAL(coordsD_r); std::string covModel = CHAR(STRING_ELT(covModel_r,0)); //priors std::string betaPrior = CHAR(STRING_ELT(betaPrior_r,0)); double *betaMu = NULL; double *betaC = NULL; if(betaPrior == "normal"){ betaMu = (double *) R_alloc(P, sizeof(double)); F77_NAME(dcopy)(&P, REAL(VECTOR_ELT(betaNorm_r, 0)), &incOne, betaMu, &incOne); betaC = (double *) R_alloc(PP, sizeof(double)); F77_NAME(dcopy)(&PP, REAL(VECTOR_ELT(betaNorm_r, 1)), &incOne, betaC, &incOne); } double *phiUnif = REAL(phiUnif_r); std::string KPriorName = CHAR(STRING_ELT(KPriorName_r,0)); double KIW_df = 0; double *KIW_S = NULL; double *ANormMu = NULL; double *ANormC = NULL; if(KPriorName == "IW"){ KIW_S = (double *) R_alloc(mm, sizeof(double)); KIW_df = REAL(VECTOR_ELT(KPrior_r, 0))[0]; KIW_S = REAL(VECTOR_ELT(KPrior_r, 1)); }else{//assume A normal (can add more specifications later) ANormMu = (double *) R_alloc(nLTr, sizeof(double)); ANormC = (double *) R_alloc(nLTr, sizeof(double)); for(i = 0; i < nLTr; i++){ ANormMu[i] = REAL(VECTOR_ELT(KPrior_r, 0))[i]; ANormC[i] = REAL(VECTOR_ELT(KPrior_r, 1))[i]; } } bool nugget = static_cast<bool>(INTEGER(nugget_r)[0]); double *PsiIGa = NULL; double *PsiIGb = NULL; if(nugget){ PsiIGa = (double *) R_alloc(m, sizeof(double)); PsiIGb = (double *) R_alloc(m, sizeof(double)); for(i = 0; i < m; i++){ PsiIGa[i] = REAL(VECTOR_ELT(PsiPrior_r, 0))[i]; PsiIGb[i] = REAL(VECTOR_ELT(PsiPrior_r, 1))[i]; } } //matern double *nuUnif = NULL; if(covModel == "matern"){ nuUnif = REAL(nuUnif_r); } bool amcmc = static_cast<bool>(INTEGER(amcmc_r)[0]); int nBatch = INTEGER(nBatch_r)[0]; int batchLength = INTEGER(batchLength_r)[0]; double acceptRate = REAL(acceptRate_r)[0]; int nSamples = nBatch*batchLength; int verbose = INTEGER(verbose_r)[0]; int nReport = INTEGER(nReport_r)[0]; if(verbose){ Rprintf("----------------------------------------\n"); Rprintf("\tGeneral model description\n"); Rprintf("----------------------------------------\n"); Rprintf("Model fit with %i outcome variables.\n\n", m); Rprintf("Number of observations within each outcome:"); printVec(n, m); Rprintf("\nNumber of covariates for each outcome (including intercept if specified):"); printVec(p, m); Rprintf("\nTotal number of observations: %i\n\n", N); Rprintf("Total number of covariates (including intercept if specified): %i\n\n", P); Rprintf("Using the %s spatial correlation model.\n\n", covModel.c_str()); if(amcmc){ Rprintf("Using adaptive MCMC.\n\n"); Rprintf("\tNumber of batches %i.\n", nBatch); Rprintf("\tBatch length %i.\n", batchLength); Rprintf("\ttarget acceptance rate %.5f.\n", acceptRate); Rprintf("\n"); }else{ Rprintf("Number of MCMC samples %i.\n\n", nSamples); } if(!nugget){ Rprintf("Psi not included in the model (i.e., no nugget model).\n\n"); } Rprintf("Priors and hyperpriors:\n"); if(betaPrior == "flat"){ Rprintf("\tbeta flat.\n"); }else{ Rprintf("\tbeta normal:\n"); Rprintf("\tmu:"); printVec(betaMu, P); Rprintf("\tcov:\n"); printMtrx(betaC, P, P); } Rprintf("\n"); if(KPriorName == "IW"){ Rprintf("\tK IW hyperpriors df=%.5f, S=\n", KIW_df); printMtrx(KIW_S, m, m); }else{ Rprintf("\tA Normal hyperpriors\n"); Rprintf("\t\tparameter\tmean\tvar\n"); for(j = 0, i = 0; j < m; j++){ for(k = j; k < m; k++, i++){ Rprintf("\t\tA[%i,%i]\t\t%3.1f\t%1.2f\n", j+1, k+1, ANormMu[i], ANormC[i]); } } } Rprintf("\n"); if(nugget){ Rprintf("\tDiag(Psi) IG hyperpriors\n"); Rprintf("\t\tparameter\tshape\tscale\n"); for(j = 0; j < m; j++){ Rprintf("\t\tPsi[%i,%i]\t%3.1f\t%1.2f\n", j+1, j+1, PsiIGa[j], PsiIGb[j]); } } Rprintf("\n"); Rprintf("\tphi Unif hyperpriors\n"); Rprintf("\t\tparameter\ta\tb\n"); for(j = 0; j < m; j++){ Rprintf("\t\tphi[%i]\t\t%0.5f\t%0.5f\n", j+1, phiUnif[j*2], phiUnif[j*2+1]); } Rprintf("\n"); if(covModel == "matern"){ Rprintf("\tnu Unif hyperpriors\n"); for(j = 0; j < m; j++){ Rprintf("\t\tnu[%i]\t\t%0.5f\t%0.5f\n", j+1, nuUnif[j*2], nuUnif[j*2+1]); } Rprintf("\n"); } } /***************************************** Set-up MCMC sample matrices etc. *****************************************/ //spatial parameters int nParams, AIndx, PsiIndx, phiIndx, nuIndx; if(!nugget && covModel != "matern"){ nParams = nLTr+m;//A, phi AIndx = 0; phiIndx = nLTr; }else if(nugget && covModel != "matern"){ nParams = nLTr+m+m;//A, diag(Psi), phi AIndx = 0; PsiIndx = nLTr; phiIndx = PsiIndx+m; }else if(!nugget && covModel == "matern"){ nParams = nLTr+2*m;//A, phi, nu AIndx = 0; phiIndx = nLTr, nuIndx = phiIndx+m; }else{ nParams = nLTr+3*m;//A, diag(Psi), phi, nu AIndx = 0; PsiIndx = nLTr, phiIndx = PsiIndx+m, nuIndx = phiIndx+m; } double *params = (double *) R_alloc(nParams, sizeof(double)); //starting covTrans(REAL(AStarting_r), ¶ms[AIndx], m); if(nugget){ for(i = 0; i < m; i++){ params[PsiIndx+i] = log(REAL(PsiStarting_r)[i]); } } for(i = 0; i < m; i++){ params[phiIndx+i] = logit(REAL(phiStarting_r)[i], phiUnif[i*2], phiUnif[i*2+1]); if(covModel == "matern"){ params[nuIndx+i] = logit(REAL(nuStarting_r)[i], nuUnif[i*2], nuUnif[i*2+1]); } } //tuning and fixed double *tuning = (double *) R_alloc(nParams, sizeof(double)); int *fixed = (int *) R_alloc(nParams, sizeof(int)); zeros(fixed, nParams); for(i = 0; i < nLTr; i++){ tuning[AIndx+i] = REAL(ATuning_r)[i]; if(tuning[AIndx+i] == 0){ fixed[AIndx+i] = 1; } } if(nugget){ for(i = 0; i < m; i++){ tuning[PsiIndx+i] = REAL(PsiTuning_r)[i]; if(tuning[PsiIndx+i] == 0){ fixed[PsiIndx+i] = 1; } } } for(i = 0; i < m; i++){ tuning[phiIndx+i] = REAL(phiTuning_r)[i]; if(tuning[phiIndx+i] == 0){ fixed[phiIndx+i] = 1; } if(covModel == "matern"){ tuning[nuIndx+i] = REAL(nuTuning_r)[i]; if(tuning[nuIndx+i] == 0){ fixed[nuIndx+i] = 1; } } } for(i = 0; i < nParams; i++){ tuning[i] = log(sqrt(tuning[i])); } //return stuff SEXP samples_r, accept_r, tuning_r; PROTECT(samples_r = allocMatrix(REALSXP, nParams, nSamples)); nProtect++; if(amcmc){ PROTECT(accept_r = allocMatrix(REALSXP, nParams, nBatch)); nProtect++; PROTECT(tuning_r = allocMatrix(REALSXP, nParams, nBatch)); nProtect++; }else{ PROTECT(accept_r = allocMatrix(REALSXP, 1, nSamples/nReport)); nProtect++; } // /***************************************** // Set-up MCMC alg. vars. matrices etc. // *****************************************/ int status=1, batchAccept=0, reportCnt=0; double logMHRatio =0, logPostCurrent = R_NegInf, logPostCand = 0, det = 0, paramsjCurrent = 0; double Q, logDetK, SKtrace; double *paramsCurrent = (double *) R_alloc(nParams, sizeof(double)); double *accept = (double *) R_alloc(nParams, sizeof(double)); zeros(accept, nParams); double *C = (double *) R_alloc(NN, sizeof(double)); double *K = (double *) R_alloc(mm, sizeof(double)); double *Psi = (double *) R_alloc(m, sizeof(double)); double *A = (double *) R_alloc(mm, sizeof(double)); double *phi = (double *) R_alloc(m, sizeof(double)); double *nu = (double *) R_alloc(m, sizeof(double)); int P1 = P+1; double *vU = (double *) R_alloc(N*P1, sizeof(double)); double *z = (double *) R_alloc(N, sizeof(double)); double *tmp_N = (double *) R_alloc(N, sizeof(double)); double *tmp_mm = (double *) R_alloc(mm, sizeof(double)); double *tmp_PP = (double *) R_alloc(PP, sizeof(double)); double *tmp_P = (double *) R_alloc(P, sizeof(double)); double *tmp_NN = NULL; double *Cbeta = NULL; if(betaPrior == "normal"){ tmp_NN = (double *) R_alloc(NN, sizeof(double)); Cbeta = (double *) R_alloc(NN, sizeof(double)); F77_NAME(dgemv)(ntran, &N, &P, &negOne, X, &N, betaMu, &incOne, &zero, z, &incOne); F77_NAME(daxpy)(&N, &one, Y, &incOne, z, &incOne); F77_NAME(dsymm)(rside, lower, &N, &P, &one, betaC, &P, X, &N, &zero, vU, &N); F77_NAME(dgemm)(ntran, ytran, &N, &N, &P, &one, vU, &N, X, &N, &zero, tmp_NN, &N); } int sl, sk; if(verbose){ Rprintf("-------------------------------------------------\n"); Rprintf("\t\tSampling\n"); Rprintf("-------------------------------------------------\n"); #ifdef Win32 R_FlushConsole(); #endif } GetRNGstate(); for(b = 0, s = 0; b < nBatch; b++){ for(i = 0; i < batchLength; i++, s++){ for(j = 0; j < nParams; j++){ //propose if(amcmc){ if(fixed[j] == 1){ paramsjCurrent = params[j]; }else{ paramsjCurrent = params[j]; params[j] = rnorm(paramsjCurrent, exp(tuning[j])); } }else{ F77_NAME(dcopy)(&nParams, params, &incOne, paramsCurrent, &incOne); for(j = 0; j < nParams; j++){ if(fixed[j] == 1){ params[j] = params[j]; }else{ params[j] = rnorm(params[j], exp(tuning[j])); } } } //extract and transform covTransInvExpand(¶ms[AIndx], A, m); for(k = 0; k < m; k++){ phi[k] = logitInv(params[phiIndx+k], phiUnif[k*2], phiUnif[k*2+1]); if(covModel == "matern"){ nu[k] = logitInv(params[nuIndx+k], nuUnif[k*2], nuUnif[k*2+1]); } } if(nugget){ for(k = 0; k < m; k++){ Psi[k] = exp(params[PsiIndx+k]); } } //construct covariance matrix sl = sk = 0; for(k = 0; k < m; k++){ sl = 0; for(l = 0; l < m; l++){ for(kk = 0; kk < n[k]; kk++){ for(jj = 0; jj < n[l]; jj++){ C[(sl+jj)*N+(sk+kk)] = 0.0; for(ii = 0; ii < m; ii++){ C[(sl+jj)*N+(sk+kk)] += A[k+m*ii]*A[l+m*ii]*spCor(coordsD[(sl+jj)*N+(sk+kk)], phi[ii], nu[ii], covModel); } } } sl += n[l]; } sk += n[k]; } if(nugget){ sl = 0; for(l = 0; l < m; l++){ for(k = 0; k < n[l]; k++){ C[(sl+k)*N+(sl+k)] += Psi[l]; } sl += n[l]; } } if(betaPrior == "normal"){ for(k = 0; k < N; k++){ for(l = k; l < N; l++){ Cbeta[k*N+l] = C[k*N+l]+tmp_NN[k*N+l]; } } det = 0; F77_NAME(dpotrf)(lower, &N, Cbeta, &N, &info); if(info != 0){error("c++ error: dpotrf failed\n");} for(k = 0; k < N; k++) det += 2*log(Cbeta[k*N+k]); F77_NAME(dcopy)(&N, z, &incOne, tmp_N, &incOne); F77_NAME(dtrsv)(lower, ntran, nUnit, &N, Cbeta, &N, tmp_N, &incOne);//u = L^{-1}(y-X'beta) Q = pow(F77_NAME(dnrm2)(&N, tmp_N, &incOne),2); }else{//beta flat det = 0; F77_NAME(dpotrf)(lower, &N, C, &N, &info); if(info != 0){error("c++ error: dpotrf failed\n");} for(k = 0; k < N; k++) det += 2*log(C[k*N+k]); F77_NAME(dcopy)(&N, Y, &incOne, vU, &incOne); F77_NAME(dcopy)(&NP, X, &incOne, &vU[N], &incOne); F77_NAME(dtrsm)(lside, lower, ntran, nUnit, &N, &P1, &one, C, &N, vU, &N);//L^{-1}[v:U] = [y:X] F77_NAME(dgemm)(ytran, ntran, &P, &P, &N, &one, &vU[N], &N, &vU[N], &N, &zero, tmp_PP, &P); //U'U F77_NAME(dpotrf)(lower, &P, tmp_PP, &P, &info); if(info != 0){error("c++ error: dpotrf failed\n");} for(k = 0; k < P; k++) det += 2*log(tmp_PP[k*P+k]); F77_NAME(dgemv)(ytran, &N, &P, &one, &vU[N], &N, vU, &incOne, &zero, tmp_P, &incOne); //U'v F77_NAME(dtrsv)(lower, ntran, nUnit, &P, tmp_PP, &P, tmp_P, &incOne); Q = pow(F77_NAME(dnrm2)(&N, vU, &incOne),2) - pow(F77_NAME(dnrm2)(&P, tmp_P, &incOne),2) ; } // //priors, jacobian adjustments, and likelihood // logPostCand = 0.0; if(KPriorName == "IW"){ logDetK = 0.0; SKtrace = 0.0; for(k = 0; k < m; k++){logDetK += 2*log(A[k*m+k]);} //jacobian \sum_{i=1}^{m} (m-i+1)*log(a_ii)+log(a_ii) for(k = 0; k < m; k++){logPostCand += (m-k)*log(A[k*m+k])+log(A[k*m+k]);} //S*K^-1 F77_NAME(dpotri)(lower, &m, A, &m, &info); if(info != 0){error("c++ error: dpotri failed\n");} F77_NAME(dsymm)(rside, lower, &m, &m, &one, A, &m, KIW_S, &m, &zero, tmp_mm, &m); for(k = 0; k < m; k++){SKtrace += tmp_mm[k*m+k];} logPostCand += -0.5*(KIW_df+m+1)*logDetK - 0.5*SKtrace; }else{ for(k = 0; k < nLTr; k++){ logPostCand += dnorm(params[AIndx+k], ANormMu[k], sqrt(ANormC[k]), 1); } } if(nugget){ for(k = 0; k < m; k++){ logPostCand += -1.0*(1.0+PsiIGa[k])*log(Psi[k])-PsiIGb[k]/Psi[k]+log(Psi[k]); } } for(k = 0; k < m; k++){ logPostCand += log(phi[k] - phiUnif[k*2]) + log(phiUnif[k*2+1] - phi[k]); if(covModel == "matern"){ logPostCand += log(nu[k] - nuUnif[k*2]) + log(nuUnif[k*2+1] - nu[k]); } } logPostCand += -0.5*det-0.5*Q; // //MH accept/reject // logMHRatio = logPostCand - logPostCurrent; if(runif(0.0,1.0) <= exp(logMHRatio)){ logPostCurrent = logPostCand; if(amcmc){ accept[j]++; }else{ accept[0]++; batchAccept++; } }else{ if(amcmc){ params[j] = paramsjCurrent; }else{ F77_NAME(dcopy)(&nParams, paramsCurrent, &incOne, params, &incOne); } } if(!amcmc){ break; } }//end params /****************************** Save samples *******************************/ F77_NAME(dcopy)(&nParams, params, &incOne, &REAL(samples_r)[s*nParams], &incOne); R_CheckUserInterrupt(); }//end batch //adjust tuning if(amcmc){ for(j = 0; j < nParams; j++){ REAL(accept_r)[b*nParams+j] = accept[j]/batchLength; REAL(tuning_r)[b*nParams+j] = tuning[j]; if(accept[j]/batchLength > acceptRate){ tuning[j] += std::min(0.01, 1.0/sqrt(static_cast<double>(b))); }else{ tuning[j] -= std::min(0.01, 1.0/sqrt(static_cast<double>(b))); } accept[j] = 0.0; } } //report if(status == nReport){ if(verbose){ if(amcmc){ Rprintf("Batch: %i of %i, %3.2f%%\n", b+1, nBatch, 100.0*(b+1)/nBatch); Rprintf("\tparameter\tacceptance\ttuning\n"); for(j = 0, i = 0; j < m; j++){ for(k = j; k < m; k++, i++){ Rprintf("\tA[%i,%i]\t\t%3.1f\t\t%1.2f\n", j+1, k+1, 100.0*REAL(accept_r)[b*nParams+AIndx+i], exp(tuning[AIndx+i])); } } if(nugget){ for(j = 0; j < m; j++){ Rprintf("\tPsi[%i,%i]\t%3.1f\t\t%1.2f\n", j+1, j+1, 100.0*REAL(accept_r)[b*nParams+PsiIndx+j], exp(tuning[PsiIndx+j])); } } for(j = 0; j < m; j++){ Rprintf("\tphi[%i]\t\t%3.1f\t\t%1.2f\n", j+1, 100.0*REAL(accept_r)[b*nParams+phiIndx+j], exp(tuning[phiIndx+j])); } if(covModel == "matern"){ Rprintf("\n"); for(j = 0; j < m; j++){ Rprintf("\tnu[%i]\t\t%3.1f\t\t%1.2f\n", j+1, 100.0*REAL(accept_r)[b*nParams+nuIndx+j], exp(tuning[nuIndx+j])); } } }else{ Rprintf("Sampled: %i of %i, %3.2f%%\n", s, nSamples, 100.0*s/nSamples); Rprintf("Report interval Metrop. Acceptance rate: %3.2f%%\n", 100.0*batchAccept/nReport); Rprintf("Overall Metrop. Acceptance rate: %3.2f%%\n", 100.0*accept[0]/s); } Rprintf("-------------------------------------------------\n"); #ifdef Win32 R_FlushConsole(); #endif } if(!amcmc){ REAL(accept_r)[reportCnt] = 100.0*batchAccept/nReport; reportCnt++; } status = 0; batchAccept = 0; } status++; }//end sample loop PutRNGstate(); //untransform variance variables for(s = 0; s < nSamples; s++){ covTransInv(&REAL(samples_r)[s*nParams+AIndx], &REAL(samples_r)[s*nParams+AIndx], m); if(nugget){ for(i = 0; i < m; i++){ REAL(samples_r)[s*nParams+PsiIndx+i] = exp(REAL(samples_r)[s*nParams+PsiIndx+i]); } } for(i = 0; i < m; i++){ REAL(samples_r)[s*nParams+phiIndx+i] = logitInv(REAL(samples_r)[s*nParams+phiIndx+i], phiUnif[i*2], phiUnif[i*2+1]); if(covModel == "matern"){ REAL(samples_r)[s*nParams+nuIndx+i] = logitInv(REAL(samples_r)[s*nParams+nuIndx+i], nuUnif[i*2], nuUnif[i*2+1]); } } } //make return object SEXP result_r, resultName_r; int nResultListObjs = 2; if(amcmc){ nResultListObjs++; } PROTECT(result_r = allocVector(VECSXP, nResultListObjs)); nProtect++; PROTECT(resultName_r = allocVector(VECSXP, nResultListObjs)); nProtect++; //samples SET_VECTOR_ELT(result_r, 0, samples_r); SET_VECTOR_ELT(resultName_r, 0, mkChar("p.theta.samples")); SET_VECTOR_ELT(result_r, 1, accept_r); SET_VECTOR_ELT(resultName_r, 1, mkChar("acceptance")); if(amcmc){ SET_VECTOR_ELT(result_r, 2, tuning_r); SET_VECTOR_ELT(resultName_r, 2, mkChar("tuning")); } namesgets(result_r, resultName_r); //unprotect UNPROTECT(nProtect); return(result_r); }
void testComplexArith(const FHEPubKey& publicKey, const FHESecKey& secretKey, const EncryptedArrayCx& ea, double epsilon) { // Test complex conjugate Ctxt c1(publicKey), c2(publicKey); vector<cx_double> vd; vector<cx_double> vd1, vd2; ea.random(vd1); ea.random(vd2); ea.encrypt(c1, publicKey, vd1, /*size=*/1.0); ea.encrypt(c2, publicKey, vd2, /*size=*/1.0); if (verbose) cout << "Test Conjugate: "; for_each(vd1.begin(), vd1.end(), [](cx_double& d){d=std::conj(d);}); c1.complexConj(); ea.decrypt(c1, secretKey, vd); #ifdef DEBUG_PRINTOUT printVec(cout<<"vd1=", vd1, 10)<<endl; printVec(cout<<"res=", vd, 10)<<endl; #endif if (cx_equals(vd, vd1, conv<double>(epsilon*c1.getPtxtMag()))) cout << "GOOD\n"; else { cout << "BAD:\n"; std::cout << " max(vd)="<<largestCoeff(vd) << ", max(vd1)="<<largestCoeff(vd1) << ", maxDiff="<<calcMaxDiff(vd,vd1) << endl<<endl; } // Test that real and imaginary parts are actually extracted. Ctxt realCtxt(c2), imCtxt(c2); vector<cx_double> realParts(vd2), real_dec; vector<cx_double> imParts(vd2), im_dec; if (verbose) cout << "Test Real and Im parts: "; for_each(realParts.begin(), realParts.end(), [](cx_double& d){d=std::real(d);}); for_each(imParts.begin(), imParts.end(), [](cx_double& d){d=std::imag(d);}); ea.extractRealPart(realCtxt); ea.decrypt(realCtxt, secretKey, real_dec); ea.extractImPart(imCtxt); ea.decrypt(imCtxt, secretKey, im_dec); #ifdef DEBUG_PRINTOUT printVec(cout<<"vd2=", vd2, 10)<<endl; printVec(cout<<"real=", realParts, 10)<<endl; printVec(cout<<"res=", real_dec, 10)<<endl; printVec(cout<<"im=", imParts, 10)<<endl; printVec(cout<<"res=", im_dec, 10)<<endl; #endif if (cx_equals(realParts,real_dec,conv<double>(epsilon*realCtxt.getPtxtMag())) && cx_equals(imParts, im_dec, conv<double>(epsilon*imCtxt.getPtxtMag()))) cout << "GOOD\n"; else { cout << "BAD:\n"; std::cout << " max(re)="<<largestCoeff(realParts) << ", max(re1)="<<largestCoeff(real_dec) << ", maxDiff="<<calcMaxDiff(realParts,real_dec) << endl; std::cout << " max(im)="<<largestCoeff(imParts) << ", max(im1)="<<largestCoeff(im_dec) << ", maxDiff="<<calcMaxDiff(imParts,im_dec) << endl<<endl; } }
void testBasicArith(const FHEPubKey& publicKey, const FHESecKey& secretKey, const EncryptedArrayCx& ea, double epsilon) { if (verbose) cout << "Test Arithmetic "; // Test objects Ctxt c1(publicKey), c2(publicKey), c3(publicKey); vector<cx_double> vd; vector<cx_double> vd1, vd2, vd3; ea.random(vd1); ea.random(vd2); // test encoding of shorter vectors vd1.resize(vd1.size()-2); ea.encrypt(c1, publicKey, vd1, /*size=*/1.0); vd1.resize(vd1.size()+2, 0.0); ea.encrypt(c2, publicKey, vd2, /*size=*/1.0); // Test - Multiplication c1 *= c2; for (long i=0; i<lsize(vd1); i++) vd1[i] *= vd2[i]; ZZX poly; ea.random(vd3); ea.encode(poly, vd3, /*size=*/1.0); c1.addConstant(poly); // vd1*vd2 + vd3 for (long i=0; i<lsize(vd1); i++) vd1[i] += vd3[i]; // Test encoding, encryption of a single number double xx = NTL::RandomLen_long(16)/double(1L<<16); // random in [0,1] ea.encryptOneNum(c2, publicKey, xx); c1 += c2; for (auto& x : vd1) x += xx; // Test - Multiply by a mask vector<long> mask(lsize(vd1), 1); for (long i=0; i*(i+1)<lsize(mask); i++) { mask[i*i] = 0; mask[i*(i+1)] = -1; } ea.encode(poly,mask, /*size=*/1.0); c1.multByConstant(poly); // mask*(vd1*vd2 + vd3) for (long i=0; i<lsize(vd1); i++) vd1[i] *= mask[i]; // Test - Addition ea.random(vd3); ea.encrypt(c3, publicKey, vd3, /*size=*/1.0); c1 += c3; for (long i=0; i<lsize(vd1); i++) vd1[i] += vd3[i]; c1.negate(); c1.addConstant(to_ZZ(1)); for (long i=0; i<lsize(vd1); i++) vd1[i] = 1.0 - vd1[i]; // Diff between approxNums HE scheme and plaintext floating ea.decrypt(c1, secretKey, vd); #ifdef DEBUG_PRINTOUT printVec(cout<<"res=", vd, 10)<<endl; printVec(cout<<"vec=", vd1, 10)<<endl; #endif if (verbose) cout << "(max |res-vec|_{infty}="<< calcMaxDiff(vd, vd1) << "): "; if (cx_equals(vd, vd1, conv<double>(epsilon*c1.getPtxtMag()))) cout << "GOOD\n"; else { cout << "BAD:\n"; std::cout << " max(vd)="<<largestCoeff(vd) << ", max(vd1)="<<largestCoeff(vd1) << ", maxDiff="<<calcMaxDiff(vd,vd1) << endl<<endl; } }
// The MAIN function, from here we start the application and run the // game loop int main() { std::cout << "Starting GLFW context, OpenGL 3.3" << std::endl; // Init GLFW glfwInit(); // Set all the required options for GLFW glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3); glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3); // OS X requests 3.3 differently. #ifdef __APPLEOS__ glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE); #endif glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE); glfwWindowHint(GLFW_RESIZABLE, GL_FALSE); // Create a GLFWwindow object that we can use for GLFW's functions GLFWwindow* window = glfwCreateWindow(WIDTH, HEIGHT, "LearnOpenGL", nullptr, nullptr); if (window == nullptr) { std::cout << "Failed to create GLFW window" << std::endl; glfwTerminate(); return -1; } glfwMakeContextCurrent(window); // Set the required callback functions glfwSetKeyCallback(window, key_callback); // Set this to true so GLEW knows to use a modern approach to retrieving // function pointers and extensions // More hacking for my autocomplete. #ifndef CLANG_COMPLETE_ONLY glewExperimental = GL_TRUE; // Initialize GLEW to setup the OpenGL Function pointers if (glewInit() != GLEW_OK) { std::cout << "Failed to initialize GLEW" << std::endl; return -1; } #endif // Define the viewport dimensions // Corrected from the tutorial, should request framebuffer size instead. // Technically should also write a callback when the window is resized // with framebuffer_size_callback. See GLFW docs. This will do for now. int fbwidth, fbheight; glfwGetFramebufferSize(window, &fbwidth, &fbheight); // Options glViewport(0, 0, WIDTH, HEIGHT); glEnable(GL_DEPTH_TEST); // ---------- BEGIN OPENGL ----------- // // Shader creation Shader edgeShader("vertex_shader_unsliced.glsl", "fragment_shader_unsliced.glsl"); // Data GLfloat vertices[] = { // Positions 0.6f, 0.6f, 0.6f, 0.6f, 0.6f, -0.6f, 0.6f, 0.6f, -0.6f, -0.6f, 0.6f, 0.6f, -0.6f, 0.6f, 0.6f, 0.6f, 0.6f, 0.6f, -0.6f, 0.6f, 0.6f, -0.6f, -0.6f, 0.6f, -0.6f, -0.6f, -0.6f, 0.6f, -0.6f, 0.6f, -0.6f, 0.6f, 0.8f, 0.8f, 0.8f, -0.8f, 0.8f, -0.8f, 0.8f, -0.8f, -0.8f, -0.8f, 0.8f, -0.8f, -0.8f, 0.8f, 0.8f, -0.8f, 0.8f, 0.8f, -0.8f, -0.8f, 0.8f, -0.8f, -0.8f, -0.8f, -0.8f, -0.8f, -0.8f, -0.8f, -0.8f, 0.8f, -0.8f, -0.8f, }; GLuint indices[] = { // Near Cube 0, 2, 1, 0, 3, 2, 7, 5, 6, 7, 4, 5, 3, 4, 7, 3, 0, 4, 6, 1, 2, 6, 5, 1, 6, 3, 7, 6, 2, 3, 1, 4, 0, 1, 5, 4, // Far Cube 8, 10, 9, 8, 11, 10, 15, 13, 14, 15, 12, 13, 11, 12, 15, 11, 8, 12, 14, 9, 10, 14, 13, 9, 14, 11, 15, 14, 10, 11, 9, 12, 8, 9, 13, 12, // Front Cube 0, 2, 1, 0, 3, 2, 11, 9, 10, 11, 8, 9, 3, 0, 8, 3, 8, 11, 10, 1, 2, 10, 9, 1, 10, 3, 11, 10, 2, 3, 1, 8, 0, 1, 9, 8, // Back Cube 5, 7, 4, 5, 6, 7, 14, 12, 15, 14, 13, 12, 6, 5, 13, 6, 13, 14, 15, 4, 7, 15, 12, 4, 15, 6, 14, 15, 7, 6, 4, 13, 5, 4, 12, 13, // Top Cube 4, 7, 3, 4, 3, 0, 15, 8, 11, 15, 12, 8, 7, 12, 15, 7, 4, 12, 0, 11, 8, 0, 3, 11, 3, 7, 15, 3, 15, 11, 4, 0, 8, 4, 8, 12, // Bottom Cube 1, 2, 6, 1, 6, 5, 10, 13, 14, 10, 9, 13, 2, 9, 10, 2, 1, 9, 5, 14, 13, 5, 6, 14, 6, 2, 10, 6, 10, 14, 1, 5, 13, 1, 13, 9, // Left Cube 3, 7, 6, 3, 6, 2, 15, 11, 10, 15, 10, 14, 11, 15, 7, 11, 7, 3, 14, 2, 6, 14, 10, 2, 7, 15, 14, 7, 14, 6, 11, 3, 2, 11, 2, 10, // Right Cube 1, 5, 4, 1, 4, 0, 13, 9, 8, 13, 8, 12, 9, 13, 5, 9, 5, 1, 12, 0, 4, 12, 8, 0, 5, 13, 12, 5, 12, 4, 9, 1, 0, 9, 0, 8, }; // Textures // Set up buffer stuff GLuint VAO, VBO, EBO; glGenVertexArrays(1, &VAO); glGenBuffers(1, &VBO); glGenBuffers(1, &EBO); // Game loop while (!glfwWindowShouldClose(window)) { // Check if any events have been activated (key pressed, mouse moved // etc.) and call corresponding response functions glfwPollEvents(); // Setup stuff glClearColor(0.2f, 0.3f, 0.3f, 1.0f); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glEnable(GL_BLEND); glLineWidth(3); // 4D-3D Transformations glm::mat4 model4D(1.0f), view4D; GLfloat theta = glm::radians((GLfloat)glfwGetTime() * 50.0f); GLfloat cs = cos(theta), sn = sin(theta); model4D[0][0] = cs; model4D[0][3] = -sn; model4D[3][0] = sn; model4D[3][3] = cs; glm::mat4x3 projection4D; glm::vec4 from(0.0f, 0.0f, 0.0f, 4.0f), to(0.0f, 0.0f, 0.0f, 0.0f); glm::vec4 up(0.0f, 1.0f, 0.0f, 0.0f), over(0.0f, 0.0f, 1.0f, 0.0f); view4D = lookAt4D(from, to, up, over); std::cout << "View Mat: " << std::endl; printMat(view4D, 4, 4); projection4D = proj4D(); GLfloat *projVert = new GLfloat[16*7]; std::cout << "--------------------------------" << std::endl; for(int i = 0; i != 16; i++) { // Project each vertex to the 3D space glm::vec4 vert4(vertices[i*4], vertices[i*4+1], vertices[i*4+2], vertices[i*4+3]); glm::vec4 viewVert = view4D * (model4D * vert4 - from); glm::vec3 vert3 = projection4D * view4D * (model4D * vert4 - from); printVec(viewVert, 4); projVert[i*7] = vert3.x; projVert[i*7+1] = vert3.y; projVert[i*7+2] = vert3.z; if (i < 8) { projVert[i*7+3] = 1.0f; projVert[i*7+4] = 0.0f; projVert[i*7+5] = 0.0f; } else { projVert[i*7+3] = 0.0f; projVert[i*7+4] = 0.0f; projVert[i*7+5] = 1.0f; } projVert[i*7+6] = (viewVert.w + 5.0f)/2.0f; } // 3D-2D Transformations glm::mat4 view3D = glm::lookAt(glm::vec3(3.0f, 1.2f, 2.0f), glm::vec3(0.0f, 0.0f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f)); glm::mat4 proj3D = glm::perspective(glm::radians(45.0f), (float)WIDTH/(float)HEIGHT, 0.1f, 100.0f); // Shader Uniforms edgeShader.Use(); GLint viewLoc = glGetUniformLocation(edgeShader.Program, "view3D"); GLint projectionLoc = glGetUniformLocation(edgeShader.Program, "projection3D"); glUniformMatrix4fv(viewLoc, 1, GL_FALSE, glm::value_ptr(view3D)); glUniformMatrix4fv(projectionLoc, 1, GL_FALSE, glm::value_ptr(proj3D)); // Load Vertices glBindVertexArray(VAO); glBindBuffer(GL_ARRAY_BUFFER, VBO); glBufferData(GL_ARRAY_BUFFER, 16*7*sizeof(GL_FLOAT), projVert, GL_DYNAMIC_DRAW); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, EBO); glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(indices), indices, GL_DYNAMIC_DRAW); glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 7*sizeof(GL_FLOAT), (GLvoid*)0); glEnableVertexAttribArray(0); glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 7*sizeof(GL_FLOAT), (GLvoid*)(3*sizeof(GL_FLOAT))); glEnableVertexAttribArray(1); glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); glDrawElements(GL_TRIANGLES, 8*12*3, GL_UNSIGNED_INT, 0); glBindVertexArray(0); // Swap the screen buffers glfwSwapBuffers(window); delete[] projVert; } // Terminate GLFW, clearing any resources allocated by GLFW. glfwTerminate(); glDeleteVertexArrays(1, &VAO); glDeleteBuffers(1, &VBO); glDeleteBuffers(1, &EBO); return 0; }
int main(int argc, char **argv) { GDISPLAY *video, *feedback; GPOINT *gPoints; XEvent event; WSETUP *wayv; WGESTURE *gaction; MATRIX *gesture; VECTOR *vector; int gPosition = 0; char *file; printf("wayV version %s\n", VERSION); /* Decide what configuration file to open */ if(argc != 2) file = strdup(SETUPFILE); else file = strdup(argv[1]); if((wayv = readSetup(file)) == NULL) exit(BAD); // writeSetup(wayv, SETUPOUT); /* Setup the display */ if(!(video = setupDrawable(wayv->pretty, wayv->pretty->window, argc, argv))) { printf("Cannot connected to X server\n"); return BAD; } /* Setup the mouse/pointer as an input device */ setupPointer(wayv, video); /* Allocate memory to store the gesture points */ gPoints = setupGPoints(wayv->universe->maxgpoints); if(wayv->pretty->display[0] == 'i') handleDrawableWindow(video, MAP); setupSignals(); /* Process the events */ while(1) { XNextEvent(video->display, &event); switch(event.type) { case MotionNotify: if(gPosition < wayv->universe->maxgpoints) { gPoints[gPosition].x = event.xmotion.x; gPoints[gPosition].y = event.xmotion.y; if(wayv->pretty->display[0] == 'y') writePixels(video, (int)gPoints[gPosition-1].x - GDEF_BORDER, (int)gPoints[gPosition-1].y - GDEF_BORDER, (int)gPoints[gPosition].x - GDEF_BORDER, (int)gPoints[gPosition].y - GDEF_BORDER); else if(wayv->pretty->display[0] == 'i') writePixels(video, (int)gPoints[gPosition-1].x, (int)gPoints[gPosition-1].y, (int)gPoints[gPosition].x, (int)gPoints[gPosition].y); gPosition++; } break; case ButtonPress: if(wayv->pretty->display[0] == 'y') handleDrawableWindow(video, MAP); gPosition = 0; gPoints[gPosition].x = event.xbutton.x; gPoints[gPosition].y = event.xbutton.y; gPosition++; break; case ButtonRelease: if(wayv->pretty->display[0] == 'y') handleDrawableWindow(video, UNMAP); gPoints[gPosition].x = GEND; gPoints[gPosition].y = GEND; gesture = gridGPoints(gPoints, wayv->universe->xgrid, wayv->universe->ygrid); vector = vectGPoints(gPoints, wayv->universe->xgrid, wayv->universe->ygrid, wayv->universe->maxvectors); printf("\n\n=======Gesture======="); printMat(gesture); printVec(vector); if(wayv->pretty->display[0] != 'n') handleDrawableWindow(video, CLEAR); if((gaction = findGesture(wayv, gesture, vector, wayv->think))) { printf("Is Action : %s\n", gaction->name); performAction(wayv, gaction, video, gPoints); /* Fork off program to handle feedback window */ if(wayv->pretty->feedback[0] == 'y' && fork() == 0) { feedback = writeText(gaction->action); waitMilli(wayv->pretty->wait); handleDrawableWindow(feedback, DESTROY); close(ConnectionNumber(video->display)); setsid(); exit(BAD); } } freeMat(gesture); free(vector); break; case ClientMessage: if(event.xclient.data.l[0] == video->wmdelete) { closeDisplay(video); exit(GOOD); } break; } } }