int examineExample(int i, F2D* a, float* b, float C, F2D* e, F2D* X, F2D* Y, float tolerance, int N, float eps, int dim) { int ret, j, k, m, n; float E, r1, randVal; float maxDiff, temp; if( ( asubsref(a,i) > 0) && ( asubsref(a,i) <C) ) E = asubsref(e,i); else E = cal_learned_func(i, a, b, N, Y, X, dim) - asubsref(Y,i); r1 = subsref(Y,i,0) * E; if( ((r1 < (-1*tolerance)) && ( asubsref(a,i) < C)) || ((r1 >tolerance) && ( asubsref(a,i) > 0)) ) { maxDiff = 0; j = i; for(k=0; k<N; k++) { if( ( asubsref(a,k) > 0) && ( asubsref(a,k) < C) ) { temp = fabsf( E - asubsref(e,k)); if (temp > maxDiff) j = k; } } if ( i!=j) { ret = takeStep(i, j, a, C, e, Y, X, eps, b, N, dim); if(ret == 1) return ret; } randVal = 1.0; for( k= (randVal*(N-2)); k<N; k++) { if( ( asubsref(a,k) > 0) && ( asubsref(a,k) <C) ) { ret = takeStep(i, k, a, C, e, Y, X, eps, b, N, dim); if (ret == 1) return ret; } } for(k=0; k<N; k++) { ret = takeStep(i, k, a, C, e, Y, X, eps, b, N, dim); if(ret == 1) return ret; } } ret = 0; return ret; }
void takeNextSteps(gridNode *steppingPoint, gridNodeStack *toSolve) { moveConstraints allowedMoves = calculateMoveConstraints( steppingPoint->entry, steppingPoint->pathTerminus.x, steppingPoint->pathTerminus.y); if (allowedMoves.allowDown) { gridNodeStack_push(toSolve, takeStep(steppingPoint, -1)); } if (allowedMoves.allowFlat) { gridNodeStack_push(toSolve, takeStep(steppingPoint, 0)); } if (allowedMoves.allowUp) { gridNodeStack_push(toSolve, takeStep(steppingPoint, 1)); } }
void Transient::execute() { preExecute(); // NOTE: if you remove this line, you will see a subset of tests failing. Those tests might have a wrong answer and might need to be regolded. // The reason is that we actually move the solution back in time before we actually start solving (which I think is wrong). So this call here // is to maintain backward compatibility and so that MOOSE is giving the same answer. However, we might remove this call and regold the test // in the future eventually. if (!_app.isRecovering()) _problem.copyOldSolutions(); // Start time loop... while (true) { if (_first != true) incrementStepOrReject(); _first = false; if (!keepGoing()) break; computeDT(); takeStep(); endStep(); _steps_taken++; } postExecute(); }
double evaluate(Plan& P) { //Compute Phase One ADDvector xPhase1(P.getNumStepsWithUnknownDurration() + P.getNumSteps() + P.getNumSlackVars() + 1); for(int i = 0; i < xPhase1.count(); i++) { xPhase1[i] = mgr.addZero(); } xPhase1[xPhase1.count()-1] = mgr.constant(10000); //inital value for s ADDvector lambda(P.getNumFunctions()); for(int i = 0; i < lambda.count(); i++) { lambda[i] = mgr.addZero(); } ADDvector nu(P.getNumFunctions()); for(int i = 0; i < nu.count(); i++) { nu[i] = mgr.addZero(); } while(checkTeminationConditions(P, xPhase1, lambda, nu, true)) { ADDvector DxPhase1(xPhase1.count()); ADDvector Dlambda(lambda.count()); ADDvector Dnu(nu.count()); calcStepDirection( xPhase1, lambda, nu, DxPhase1, Dlambda, Dnu, P, true); takeStep( xPhase1, lambda, nu, DxPhase1, Dlambda, Dnu); } //Compute Phase two ADDvector x(xPhase1.count() -1); for(int i = 0; i < x.count(); i++) { x[i] = xPhase1[i]; } while(checkTeminationConditions(P, x, lambda, nu, false)) { ADDvector Dx(x.count()); ADDvector Dlambda(lambda.count()); ADDvector Dnu(nu.count()); calcStepDirection( x, lambda, nu, Dx, Dlambda, Dnu, P, false); takeStep( x, lambda, nu, Dx, Dlambda, Dnu); } //Really I need to do some max's and sum's here but that can be added later. //Right now I need to add something to test all of this code. return 0; }
void NonlinearEigen::execute() { preExecute(); takeStep(); postExecute(); }
void NonlinearEigen::execute() { if (_app.isRecovering()) return; preExecute(); takeStep(); postExecute(); }
void InversePowerMethod::execute() { if (_app.isRecovering()) return; preExecute(); takeStep(); postExecute(); }
int SmoTutor::examineFirstChoice(int i2, double e2) { if (minimum > -1) { if (fabs(e2 - error[minimum]) > fabs(e2 - error[maximum])) { if (takeStep(minimum, i2, e2)) { return 1; } } else { if (takeStep(maximum, i2, e2)) { return 1; } } } return 0; }
void AdaptiveTransient::execute() { preExecute(); // Start time loop... while (keepGoing()) { takeStep(); if (lastSolveConverged()) endStep(); } postExecute(); }
int SmoTutor::examineBound(int i2, double e2) { int start = rand() % ntp; for (int i = 0; i < ntp; i++) { int i1 = (i + start) % ntp; if (alpha[i1] == 0.0 || alpha[i1] == C[i1]) { if (takeStep(i1, i2, e2)) { return 1; } } } return 0; }
/* -------------------------------------------------------------- Finds the second Lagrange multiplayer to be optimize. -------------------------------------------------------------- */ long examineExample( long i1 ) { double y1, alpha1, E1, r1; double tmax; double E2, temp; long k, i2; long k0; y1 = target[i1]; alpha1 = alpha[i1]; E1 = w*data[i1] - *b - y1; r1 = y1 * E1; if(( r1 < -tolerance && alpha1 < C ) || (r1 > tolerance && alpha1 > 0)) { /* Try i2 by three ways; if successful, then immediately return 1; */ for( i2 = (-1), tmax = 0, k = 0; k < num_data; k++ ) { if( alpha[k] > 0 && alpha[k] < C ) { E2 = w*data[k] - *b - target[k]; temp = fabs(E1 - E2); if( temp > tmax ) { tmax = temp; i2 = k; } } } if( i2 >= 0 ) { if( takeStep(i1,i2) ) return( 1 ); } #ifdef RANDOM for( k0 = rand(), k = k0; k < num_data + k0; k++ ) { i2 = k % num_data; #else for( k = 0; k < num_data; k++) { i2 = k; #endif if( alpha[i2] > 0 && alpha[i2] < C ) { if( takeStep(i1,i2) ) return( 1 ); } } #ifdef RANDOM for( k0 = rand(), k = k0; k < num_data + k0; k++ ) { i2 = k % num_data; #else for( k = 0; k < num_data; k++) { i2 = k; #endif if( takeStep(i1,i2) ) return( 1 ); } } /* if( ... ) */ return( 0 ); } /* -------------------------------------------------------------- Main SMO optimization cycle. -------------------------------------------------------------- */ void runSMO( void ) { long numChanged = 0; long examineAll = 1; long k; while( numChanged > 0 || examineAll ) { numChanged = 0; if( examineAll ) { for( k = 0; k < num_data; k++ ) { numChanged += examineExample( k ); } } else { for( k = 0; k < num_data; k++ ) { if( alpha[k] != 0 && alpha[k] != C ) numChanged += examineExample( k ); } } if( examineAll == 1 ) examineAll = 0; else if( numChanged == 0 ) examineAll = 1; } } /* ============================================================== Main MEX function - interface to Matlab. ============================================================== */ void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray*prhs[] ) { long i,j ; double *labels12, *nsv, *trn_err, *margin; double nerr; /* ---- check number of input arguments ------------- */ if(nrhs != 5 ) mexErrMsgTxt("Incorrect number of input arguments."); if(nlhs < 2) mexErrMsgTxt("Not enough output arguments."); /* ---- get input arguments ----------------------- */ labels12 = mxGetPr(prhs[1]); /* labels (1,2) */ data = mxGetPr(prhs[0]); /* pointer at data */ dim = mxGetM(prhs[0]); /* data dimension */ num_data = mxGetN(prhs[0]); /* number of data */ C = mxGetScalar( prhs[2] ); eps = mxGetScalar( prhs[3] ); tolerance = mxGetScalar( prhs[4] ); /* ---- init variables ------------------------------- */ ker_cnt=0; /* num of dot product evaluations */ /* allocate memory for targets (labels) (1,-1) */ if( (target = (double*)mxCalloc(num_data, sizeof(double) )) == NULL) { mexErrMsgTxt("Not enough memory."); } /* transform labels12 (1,2) from to targets (1,-1) */ for( i = 0; i < num_data; i++ ) { target[i] = - labels12[i]*2 + 3; } /* create output variable for bias */ plhs[1] = mxCreateDoubleMatrix(1,1,mxREAL); b = mxGetPr(plhs[1]); *b= 0; /* create vector for Lagrangeians */ plhs[0] = mxCreateDoubleMatrix(num_data,1,mxREAL); alpha = mxGetPr(plhs[0]); /* inicialize alpha */ for( i = 0; i < num_data; i++ ) { alpha[i] = 0; } w=0; /* ---- run SMO ------------------------------------------- */ runSMO(); /* ---- outputs ---------------------------------- */ if( nlhs >= 3 ) { /* count number of support vectors */ plhs[2] = mxCreateDoubleMatrix(1,1,mxREAL); nsv = mxGetPr(plhs[2]); *nsv = 0; for( i = 0; i < num_data; i++ ) { if( alpha[i] > 0) (*nsv)++; } } if( nlhs >= 4 ) { /* number of used iterations */ plhs[3] = mxCreateDoubleMatrix(1,1,mxREAL); (*mxGetPr(plhs[3])) = (double)ker_cnt; } if( nlhs >= 5) { /* evaluates classification error on traning patterns */ plhs[4] = mxCreateDoubleMatrix(1,1,mxREAL); trn_err = mxGetPr(plhs[4]); *trn_err = 0; for( i = 0; i < num_data; i++ ) { if( target[i]*(w*data[i]-*b) < 0) (*trn_err)++; } *trn_err = (*trn_err)/(double)num_data; } if( nlhs >= 6) { /* compute margin */ plhs[5] = mxCreateDoubleMatrix(1,1,mxREAL); margin = mxGetPr(plhs[5]); *margin = 1/sqrt(w*w); } /* decision function of type <w,x>+b is used */ *b = -*b; /* ----- free memory --------------------------------------- */ mxFree( target ); }
bool svm::examineExample(const int& i2) { //debug("Examining sample " << i2 << ":\n"); y2=currentTarget->at(i2); alph2=currentAlpha->at(i2); e2=errorCache.at(i2); double r2=e2*y2; //debug("i2=" << i2 << ", y2=" << y2 << ", alpha2=" << alph2 << ", e2=" << e2 << ", r2=" << r2 << "\n"); // check KKT condition for example i2 if ((r2 < -tolerance && alph2 < C) || (r2 > tolerance && alph2 > 0)) { // example violates KKT condition, choose it for optimization // init array of non-zero and non-C alphas int* tmpAlpha=new int[currentAlpha->size()]; int j=0,i=0; for (i=0; i<currentAlpha->size(); i++) { if (currentAlpha->at(i) != 0.0 && currentAlpha->at(i) != C) { tmpAlpha[j++]=i; } } // is there more than one non-bound alpha? if (j > 1) { // yes, then choose one that is expected to // maximize the optimization step size double emax=std::numeric_limits<double>::min(); int k=0; for (int i=0; i<errorCache.size(); i++) { if (errorCache.at(i) > emax) { k=i; emax=errorCache.at(i); } } if (takeStep(k,i2)) { delete[] tmpAlpha; return true; } } // iterate over all non-zero and non-C alphas // start at random index int start; if (j > 0) { start=rand()%j; for (int i=start,count=0; count<j; count++, i=(i+1)%j) { if (takeStep(tmpAlpha[i],i2)) { delete[] tmpAlpha; return true; } } } // iterate over all possible i start=rand()%currentAlpha->size(); j=trainData->rows(); int count; for (count=0,i=start; count<currentAlpha->size(); count++, i=(i+1)%currentAlpha->size()) { if (takeStep(i,i2)) { delete[] tmpAlpha; return true; } } delete[] tmpAlpha; } return false; }
//Apply physics from key presses and set animation void Player::ApplyPhysics(Tile *tiles[], std::list <LevelObject> levelObjects, std::list <DestroyableObject> objects) { //Set frames for animations frameTime++; if (60 / frameTime <= 6) { frameTime = 0; currentFrame += 1; if (currentFrame >= 4) currentFrame = 0; } //Reduce hit cooldown so player can take damage again hitCooldown--; if (hitCooldown <= 0) hitCooldown = 0; flashTimer--; if (flashTimer <= 0) flashTimer = 0; landingTimer--; if (landingTimer <= 0) landingTimer = 0; if (!attacking) attackBox = { 0, 0, 0, 0 }; if (whipLevel == 1 || whipLevel == 2) whipLength = 44; else if (whipLevel == 3) whipLength = 76; //Set flip for animations if (!onStairs && hitCooldown <= 60) { if (velX > 0) flip = false; if (velX < 0) flip = true; } //Apply physics and set animations for when not on stairs if (!goToStairs && !onStairs) { if (jumping) { jumpTime--; if (jumpTime <= 30) { velY += 0.7f; if (!attacking && !damaged) currentAnimation = IDLE; else if (!attacking && damaged) currentAnimation = DAMAGED; } } if (attacking && !throwing) { attackTimer--; if (attackTimer <= 36) { attacking = false; currentAnimation = IDLE; } if (attackTimer <= 48) { if (!flip) { if (!crouching || jumping) { attackBox.x = playerBox.x + PLAYER_WIDTH + 12; attackBox.y = playerBox.y + (PLAYER_HEIGHT / 2) - 16; attackBox.w = whipLength; attackBox.h = 16; } else { attackBox.x = playerBox.x + PLAYER_WIDTH + 12; attackBox.y = playerBox.y + (PLAYER_HEIGHT / 2); attackBox.w = whipLength; attackBox.h = 16; } } else { if (!crouching || jumping) { attackBox.x = playerBox.x - (12 + whipLength); attackBox.y = playerBox.y + (PLAYER_HEIGHT / 2) - 16; attackBox.w = whipLength; attackBox.h = 16; } else { attackBox.x = playerBox.x - (12 + whipLength); attackBox.y = playerBox.y + (PLAYER_HEIGHT / 2); attackBox.w = whipLength; attackBox.h = 16; } } } } else if (throwing && attacking) { attackTimer--; if (attackTimer <= 30) { throwing = false; attacking = false; currentAnimation = IDLE; } } if (!jumping && !attacking && !crouching) { velY += 0.7f; if (rightPress && !leftPress) { velX = MoveSpeed; currentAnimation = MOVING; } if (!rightPress && leftPress) { velX = 0 - MoveSpeed; currentAnimation = MOVING; } if (!rightPress && !leftPress) { velX = 0; currentAnimation = IDLE; } if (rightPress && leftPress) { velX = 0; currentAnimation = IDLE; } } if (crouching && !attacking && !jumping) { velX = 0; currentAnimation = CROUCHING; } velY += 0.3f; if (autoWalk) { velX = 0; playerBox.x += 1; currentAnimation = MOVING; } playerBox.x += velX; //If the player reaches a wall if ((playerBox.x < 0) || (playerBox.x + PLAYER_WIDTH > levelWidth) || touchesWall(playerBox, tiles) || touchesDestroyableObject(playerBox, objects)) { //Move back playerBox.x -= velX; } //Move the player up or down playerBox.y += (int)round(velY); //If the player touches floor or roof if ((playerBox.y < 0) || (playerBox.y + PLAYER_HEIGHT > levelHeight) || touchesWall(playerBox, tiles) || touchesLevelObject(playerBox, levelObjects) || touchesDestroyableObject(playerBox, objects)) { //Move back playerBox.y -= (int)round(velY); if (velY > 15 && !damaged && !attacking && !jumping) { velX = 0; landingTimer = 20; } jumping = false; onGround = true; if (landingTimer <= 18 && landingTimer > 0) crouching = true; velY = 0; if (attacking) velX = 0; if (damaged && !playerDead) { crouching = true; flashTimer = 80; hitCooldown = 80; damaged = false; } if (health <= 0) playerDeath(); if (flashTimer <= 60 && landingTimer <= 0) if (!downPress) crouching = false; } else onGround = false; } //Apply physics and set animatons for when on stairs if (onStairs) goToStairs = false; if (goToStairs) { moveToStairs(playerBox, tiles); } //Check to move into position to go down stairs if (crouching && touchesStairs(playerBox, tiles, true)) { currentAnimation = MOVING; moveToStairsDown(playerBox, tiles); } //Movement for when on stairs, different for right and left stairs if (onStairs && !attacking) { secondStep--; if (rightStair) { if (secondStep == 0 && stepUp) { takeStep(true, true); stepFrame = 0; } if (secondStep == 0 && !stepUp) { takeStep(false, false); stepFrame = 0; } if (upPress && !downPress && !leftPress || rightPress && !leftPress && !downPress) { currentAnimation = STAIRS_UP; flip = false; stepTime++; if (stepTime % 16 == 0) { secondStep = 8; stepUp = true; takeStep(true, true); stepFrame = 1; } } if (!upPress && downPress && !rightPress || leftPress && !rightPress && !upPress) { currentAnimation = STAIRS_DOWN; flip = true; stepTime++; if (stepTime % 16 == 0) { secondStep = 8; stepUp = false; takeStep(false, false); stepFrame = 1; } } } if (leftStair) { if (secondStep == 0 && stepUp) { stepFrame++; takeStep(true, false); if (stepFrame >= 2) stepFrame = 0; } if (secondStep == 0 && !stepUp) { stepFrame++; takeStep(false, true); if (stepFrame >= 2) stepFrame = 0; } if (upPress && !downPress && !rightPress || leftPress && !rightPress && !downPress) { currentAnimation = STAIRS_UP; flip = true; stepTime++; stepUp = true; if (stepTime % 16 == 0) { secondStep = 8; stepFrame++; takeStep(true, false); if (stepFrame >= 2) stepFrame = 0; } } if (!upPress && downPress && !leftPress || rightPress && !leftPress && !upPress) { currentAnimation = STAIRS_DOWN; flip = false; stepTime++; stepUp = false; if (stepTime % 16 == 0) { secondStep = 8; stepFrame++; takeStep(false, true); if (stepFrame >= 2) stepFrame = 0; } } } } if (onStairs && attacking && !throwing) { attackTimer--; if (rightStair) { if (attackTimer <= 36) { attacking = false; if (!flip) currentAnimation = STAIRS_UP; else if (flip) currentAnimation = STAIRS_DOWN; } if (attackTimer <= 48) { if (!flip) { attackBox.x = playerBox.x + PLAYER_WIDTH + 12; attackBox.y = playerBox.y + (PLAYER_HEIGHT / 2) - 16; attackBox.w = 44; attackBox.h = 16; } else if (flip) { attackBox.x = playerBox.x - 56; attackBox.y = playerBox.y + (PLAYER_HEIGHT / 2) - 16; attackBox.w = 44; attackBox.h = 16; } } } if (leftStair) { if (attackTimer <= 36) { attacking = false; if (!flip) currentAnimation = STAIRS_DOWN; else if (flip) currentAnimation = STAIRS_UP; } if (attackTimer <= 48) { if (!flip) { attackBox.x = playerBox.x + PLAYER_WIDTH + 12; attackBox.y = playerBox.y + (PLAYER_HEIGHT / 2) - 16; attackBox.w = 44; attackBox.h = 16; } else if (flip) { attackBox.x = playerBox.x - 56; attackBox.y = playerBox.y + (PLAYER_HEIGHT / 2) - 16; attackBox.w = 44; attackBox.h = 16; } } } } else if (onStairs && throwing && attacking) { attackTimer--; if (attackTimer <= 36) { throwing = false; attacking = false; currentAnimation = STAIRS_UP; } } //Stop stair movement if player reaches top of stairs if (onStairs && !touchesStairs(playerBox, tiles, false)) { if (rightStair) { onStairs = false; playerBox.x -= 8; } if (leftStair) { onStairs = false; playerBox.x += 8; } } //Stop stair movement when player reaches floor if (onStairs && touchesWall(playerBox, tiles)) { if (rightStair) { if (downPress || leftPress) { onStairs = false; playerBox.y -= 8; playerBox.x += 8; } } if (leftStair) { if (downPress || rightPress) { onStairs = false; playerBox.y -= 8; playerBox.x -= 8; } } } if (onStairs) { if (cameraStart + cameraStep < playerBox.x) { cameraStep++; } else if (cameraStart + cameraStep > playerBox.x) { cameraStep--; } if (touchesExit(playerBox, tiles, true)) { reachedExitStair = true; } if (touchesExit(playerBox, tiles, false)) { reachedExitStair2 = true; } if (touchesEntrance(playerBox, tiles, true)) { reachedEntrance = true; } if (touchesEntrance(playerBox, tiles, false)) { reachedEntrance2 = true; } } }
void Player::moveToStairsDown(SDL_Rect box, Tile* tiles[]) { int playerRight = box.x + box.w; int playerLeft = box.x; for (int i = 0; i < totalTiles; ++i) { if (tiles[i]->Type() == 'R') { Tile* tempTile = new Tile(tiles[i]->Box().x + 32, tiles[i]->Box().y - 16, 'R'); if (playerRight > tempTile->Box().x && playerRight < (tempTile->Box().x + tempTile->Box().w)) { //Not exactly sure why this works, I assume it needs to override a movement the other way playerBox.x += 2; } if (playerLeft > tempTile->Box().x && playerLeft < (tempTile->Box().x + tempTile->Box().w)) { playerBox.x -= 1; } if (playerLeft == tempTile->Box().x) { onStairs = true; rightStair = true; leftStair = false; secondStep = 8; stepUp = false; takeStep(false, false); crouching = false; stepFrame = 1; cameraStart = playerBox.x; cameraStep = 0; } } if (tiles[i]->Type() == 'L') { Tile* tempTile = new Tile(tiles[i]->Box().x - 32, tiles[i]->Box().y - 16, 'L'); if (playerRight > tempTile->Box().x && playerRight < (tempTile->Box().x + tempTile->Box().w)) { playerBox.x += 1; } if (playerLeft > tempTile->Box().x && playerLeft < (tempTile->Box().x + tempTile->Box().w)) { playerBox.x -= 2; } if (playerLeft == tempTile->Box().x) { onStairs = true; leftStair = true; rightStair = false; secondStep = 8; stepUp = false; takeStep(false, true); crouching = false; stepFrame = 1; cameraStart = playerBox.x; cameraStep = 0; } } } }
int main(int argc, char *argv[]) { /* Initialize *******************************/ // long seed = 6781; int i; double nuRedScale; double nuBlueScale; /* Phase population distributions */ double ****Red, ****Blue; Lx = 80; Ly = 20; Lz = 20; //Adds layers to ensure periodic BCs // Lx += (2 * layers); /* Allocate memory for global arrays. */ Solid = create3DInt(Lz, Ly, Lx); Red = create4DDouble(Lz, Ly, Lx, b); Blue = create4DDouble(Lz, Ly, Lx, b); f = create4DDouble(Lz, Ly, Lx, b); N = new double[b]; /**********************************************************/ /* Initialization of solid matrix. */ /* */ /* Regular geometry for testing purposes */ /* */ /**********************************************************/ // Scaled global constants // If tau = 1 no scaling ExForceConst = tau*(double)Dim/(bm*c1*c1); nuRedScale = tau/(h*h)*nuRed; nuBlueScale = tau/(h*h)*nuBlue; // Relaxation parameters lambdaRed = (-2.0/(6*nuRedScale+1)); lambdaBlue = (-2.0/(6*nuBlueScale+1)); // Initial body-force FluxForce=0.0; // Initial volumetric flux J=0.0; Jfix = 0.0; startTime = 0; // timesteps ite = startTime; // Sets the initial phase populations initializeFluid(Lz,Ly,Lx,Red,Blue); // Calculates certain quantities and output to file // Not a part of the simulation massMomentumCalc(Red,Blue,Solid,"outputh","outputl","LBMflow","LBMflow"); // Starts simulation takeStep(Red,Blue); // Cleaning up delete4DDouble(Red, Lz, Ly, Lx); delete4DDouble(Blue, Lz, Ly, Lx); delete4DDouble(f, Lz, Ly, Lx); delete3DInt(Solid, Lz, Ly); free(N); return 0; }
int SMO::examineExample(int i1) { double y1, alpha1, E1, r1; y1 = target[i1]; alpha1 = alpha[i1]; // Check E1 in error cache, if not present then E1 = SVM output on point[i1] - y1. if (alpha1 > 0 && alpha1 < C) E1 = errorCache[i1]; else E1 = predict(points[i1]) - y1; r1 = y1 * E1; // Check if alpha1 violated KKT condition by more than tolerance, if it does then look for alpha2 and optimise them by calling takeStep(i1,i2) if ((r1 < -tolerance && alpha1 < C) || (r1 > tolerance && alpha1 > 0)) { unsigned int k; int i2; double dEmax = 0.0; i2 = -1; // Try i2 using second choice heuristic as described in section 2.2 by choosing an error to maximize step size. // The i2 is taken which maximize dE. for (k = 0; k < points.size(); k++) if (alpha[k] > 0 && alpha[k] < C) { double E2, dE; E2 = errorCache[k]; dE = fabs(E1 - E2); if (dE > dEmax) { dEmax = dE; i2 = k; } } if (i2 >= 0) { if (takeStep (i1, i2)) return 1; } // Loop over all non-zero and non-C alpha, starting at a random point. i2 = (int)(rand() % points.size()); for (k = 0; k < points.size(); k++) { if (alpha[i2] > 0 && alpha[i2] < C) { if (takeStep(i1, i2)) return 1; } i2 = (i2 + 1)%points.size(); } // Loop over all possible i2, starting at a random point. i2 = (int)(rand() % points.size()); for (k = 0; k < points.size(); k++) { if (takeStep(i1, i2)) return 1; i2 = (i2 + 1)%points.size(); } } return 0; }
/* Convenience function to do a full simulation at once. * Simply does takeStep() the required number of times. */ void MCMC::run(){ int i; for(i = 0; i < maxSteps; i++){ takeStep(); } }
/* -------------------------------------------------------------- Finds the second Lagrange multiplayer to be optimize. -------------------------------------------------------------- */ long examineExample( long i1 ) { double y1, alpha1, E1, r1; double tmax; double E2, temp; long k, i2; long k0; y1 = target[i1]; alpha1 = alpha[i1]; if( alpha1 > 0 && alpha1 < C(i1) ) E1 = error_cache[i1]; else E1 = learned_func(i1) - y1; r1 = y1 * E1; if(( r1 < -tolerance && alpha1 < C(i1) ) || (r1 > tolerance && alpha1 > 0)) { /* Try i2 by three ways; if successful, then immediately return 1; */ for( i2 = (-1), tmax = 0, k = 0; k < N; k++ ) { if( alpha[k] > 0 && alpha[k] < C(k) ) { E2 = error_cache[k]; temp = fabs(E1 - E2); if( temp > tmax ) { tmax = temp; i2 = k; } } } if( i2 >= 0 ) { if( takeStep(i1,i2) ) return( 1 ); } #ifdef RANDOM for( k0 = rand(), k = k0; k < N + k0; k++ ) { i2 = k % N; #else for( k = 0; k < N; k++) { i2 = k; #endif if( alpha[i2] > 0 && alpha[i2] < C(i2) ) { if( takeStep(i1,i2) ) return( 1 ); } } #ifdef RANDOM for( k0 = rand(), k = k0; k < N + k0; k++ ) { i2 = k % N; #else for( k = 0; k < N; k++) { i2 = k; #endif if( takeStep(i1,i2) ) return( 1 ); } } /* if( ... ) */ return( 0 ); } /* -------------------------------------------------------------- Main SMO optimization cycle. -------------------------------------------------------------- */ void runSMO( void ) { long numChanged = 0; long examineAll = 1; long k; while( numChanged > 0 || examineAll ) { numChanged = 0; if( examineAll ) { for( k = 0; k < N; k++ ) { numChanged += examineExample( k ); } } else { for( k = 0; k < N; k++ ) { if( alpha[k] != 0 && alpha[k] != C(k) ) numChanged += examineExample( k ); } } if( examineAll == 1 ) examineAll = 0; else if( numChanged == 0 ) examineAll = 1; } } /* ============================================================== Main MEX function - interface to Matlab. ============================================================== */ void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray*prhs[] ) { long i,j ; double *labels12, *initAlpha, *nsv, *tmp, *trn_err, *margin; double nerr; double C1, C2; /* ---- get input arguments ----------------------- */ if(nrhs < 5) mexErrMsgTxt("Not enough input arguments."); /* data matrix [dim x N ] */ if( !mxIsNumeric(prhs[0]) || !mxIsDouble(prhs[0]) || mxIsEmpty(prhs[0]) || mxIsComplex(prhs[0]) ) mexErrMsgTxt("Input X must be a real matrix."); /* vector of labels (1,2) */ if( !mxIsNumeric(prhs[1]) || !mxIsDouble(prhs[1]) || mxIsEmpty(prhs[1]) || mxIsComplex(prhs[1]) || (mxGetN(prhs[1]) != 1 && mxGetM(prhs[1]) != 1)) mexErrMsgTxt("Input I must be a real vector."); labels12 = mxGetPr(prhs[1]); /* labels (1,2) */ dataA = mxGetPr(prhs[0]); /* pointer at patterns */ dataB = dataA; dim = mxGetM(prhs[0]); /* data dimension */ N = mxGetN(prhs[0]); /* number of data */ /* kernel identifier */ ker = kernel_id( prhs[2] ); if( ker == -1 ) mexErrMsgTxt("Improper kernel identifier."); /* get pointer to arguments */ arg1 = mxGetPr(prhs[3]); /* one or two real trade-off constant(s) */ if( !mxIsNumeric(prhs[4]) || !mxIsDouble(prhs[4]) || mxIsEmpty(prhs[4]) || mxIsComplex(prhs[4]) || (mxGetN(prhs[4]) != 1 && mxGetM(prhs[4]) != 1 )) mexErrMsgTxt("Improper input argument C."); else { /* allocate memory for constant C */ if( (const_C = mxCalloc(N, sizeof(double) )) == NULL) { mexErrMsgTxt("Not enough memory."); } if( MAX( mxGetN(prhs[4]), mxGetM(prhs[4])) == 1 ) { C1 = mxGetScalar(prhs[4]); for( i=0; i < N; i++ ) const_C[i] = C1; } else if( MAX( mxGetN(prhs[4]), mxGetM(prhs[4])) == 2 ) { tmp = mxGetPr(prhs[4]); C1 = tmp[0]; C2 = tmp[1]; for( i=0; i < N; i++ ) { if( labels12[i]==1) const_C[i] = C1; else const_C[i] = C2; } } else if( MAX( mxGetN(prhs[4]), mxGetM(prhs[4])) == N ) { tmp = mxGetPr(prhs[4]); for( i=0; i < N; i++ ) const_C[i] = tmp[i]; } else { mexErrMsgTxt("Improper argument C."); } } /* real parameter eps */ if( nrhs >= 6 ) { if( !mxIsNumeric(prhs[5]) || !mxIsDouble(prhs[5]) || mxIsEmpty(prhs[5]) || mxIsComplex(prhs[5]) || mxGetN(prhs[5]) != 1 || mxGetM(prhs[5]) != 1 ) mexErrMsgTxt("Input eps must be a scalar."); else eps = mxGetScalar(prhs[5]); /* take eps argument */ } /* real parameter tol */ if(nrhs >= 7) { if( !mxIsNumeric(prhs[6]) || !mxIsDouble(prhs[6]) || mxIsEmpty(prhs[6]) || mxIsComplex(prhs[6]) || mxGetN(prhs[6]) != 1 || mxGetM(prhs[6]) != 1 ) mexErrMsgTxt("Input tol must be a scalar."); else tolerance = mxGetScalar(prhs[6]); /* take tolerance argument */ } /* real vector of Lagrangeian multipliers */ if(nrhs >= 8) { if( !mxIsNumeric(prhs[7]) || !mxIsDouble(prhs[7]) || mxIsEmpty(prhs[7]) || mxIsComplex(prhs[7]) || (mxGetN(prhs[7]) != 1 && mxGetM(prhs[7]) != 1 )) mexErrMsgTxt("Input Alpha must be a vector."); } /* real scalar - bias */ if( nrhs >= 9 ) { if( !mxIsNumeric(prhs[8]) || !mxIsDouble(prhs[8]) || mxIsEmpty(prhs[8]) || mxIsComplex(prhs[8]) || mxGetN(prhs[8]) != 1 || mxGetM(prhs[8]) != 1 ) mexErrMsgTxt("Input bias must be a scalar."); } /* ---- init variables ------------------------------- */ ker_cnt = 0; /* allocate memory for targets (labels) (1,-1) */ if( (target = mxCalloc(N, sizeof(double) )) == NULL) { mexErrMsgTxt("Not enough memory."); } /* transform labels12 (1,2) from to targets (1,-1) */ for( i = 0; i < N; i++ ) { target[i] = - labels12[i]*2 + 3; } /* create output variable for bias */ plhs[1] = mxCreateDoubleMatrix(1,1,mxREAL); b = mxGetPr(plhs[1]); /* take init value of bias if given */ if( nrhs >= 9 ) { *b = -mxGetScalar(prhs[8]); } /* allocate memory for error_cache */ if( (error_cache = mxCalloc(N, sizeof(double) )) == NULL) { mexErrMsgTxt("Not enough memory for error cache."); } /* create vector for Lagrangeians */ plhs[0] = mxCreateDoubleMatrix(N,1,mxREAL); alpha = mxGetPr(plhs[0]); /* if Lagrangeians given then use them as initial values */ if( nrhs >= 8 ) { initAlpha = mxGetPr(prhs[7]); for( i = 0; i < N; i++ ) { alpha[i] = initAlpha[i]; } /* Init error cache for non-bound multipliers. */ for( i = 0; i < N; i++ ) { if( alpha[i] != 0 && alpha[i] != C(i) ) { error_cache[i] = learned_func(i) - target[i]; } } } /* ---- run SMO ------------------------------------------- */ runSMO(); /* ---- outputs --------------------------------- */ if( nlhs >= 3 ) { /* count number of support vectors */ plhs[2] = mxCreateDoubleMatrix(1,1,mxREAL); nsv = mxGetPr(plhs[2]); *nsv = 0; for( i = 0; i < N; i++ ) { if( alpha[i] > ZERO_LIM ) (*nsv)++; else alpha[i] = 0; } } if( nlhs >= 4 ) { plhs[3] = mxCreateDoubleMatrix(1,1,mxREAL); (*mxGetPr(plhs[3])) = (double)ker_cnt; } if( nlhs >= 5) { /* evaluates classification error on traning patterns */ plhs[4] = mxCreateDoubleMatrix(1,1,mxREAL); trn_err = mxGetPr(plhs[4]); nerr = 0; for( i = 0; i < N; i++ ) { if( target[i] == 1 ) { if( learned_func(i) < 0 ) nerr++; } else if( learned_func(i) >= 0 ) nerr++; } *trn_err = nerr/N; } if( nlhs >= 6) { /* compute margin */ plhs[5] = mxCreateDoubleMatrix(1,1,mxREAL); margin = mxGetPr(plhs[5]); *margin = 0; for( i = 0; i < N; i++ ) { for( j = 0; j < N; j++ ) { if( alpha[i] > 0 && alpha[j] > 0 ) *margin += alpha[i]*alpha[j]*target[i]*target[j]*kernel(i,j); } } *margin = 1/sqrt(*margin); } /* decision function of type <w,x>+b is used */ *b = -*b; /* ----- free memory --------------------------------------- */ mxFree( error_cache ); mxFree( target ); }
/****************************************************************** * Main-function */ int main(void) { /************************************** * Set the clock to run at 80 MHz */ SysCtlClockSet(SYSCTL_SYSDIV_2_5|SYSCTL_USE_PLL|SYSCTL_XTAL_16MHZ|SYSCTL_OSC_MAIN); /************************************** * Enable the floating-point-unit */ FPUEnable(); // We also want Lazystacking, so enable that too FPULazyStackingEnable(); // We also want to put numbers close to zero to zero FPUFlushToZeroModeSet(FPU_FLUSH_TO_ZERO_EN); /************************************** * Init variables */ uint16_t mapData[MAP_DATA_SIZE]; uint8_t stepDir = 0; /************************************** * Init peripherals used */ bluetooth_init(); init_stepper(); // Init the GPIOs used for the stepper and loading LED InitI2C1(); // Init the communication with the lidar-unit through I2C InitPWM(); setupTimers(); /************************************** * State 2 */ // Disable the timers that are used disableTimer(TIMER1_BASE); disableTimer(TIMER2_BASE); // Enable all interrupts IntMasterEnable(); /************************************** * State 3 */ // Indicate we should start with a scan regardless of what other things we have already got // from UART-interrupt // This means setting the appropriate bit in the status vector stat_vec |= TAKE_MEAS; /************************************** * State 4 */ // Contains main-loop where decisions should be made for ( ; ; ) { /********************************** * Decision tree */ // Highest priority case first // Check both interrupts at each iteration in the loop if ( int_vec & UART_INT ) { // Reset the indication int_vec &= ~UART_INT; // Remove drive-stop flag to enable movement stat_vec &= ~DRIVE_STOP; // Init data array uint8_t dataArr[MAX_UART_MSG_SIZE]; // Collect the message if ( readUARTMessage(dataArr, MAX_UART_MSG_SIZE) < SUCCESS ) { // If we have recieved more data than fits in the vector we should simply // go in here again and grab data int_vec |= UART_INT; } // We have gathered a message // and now need to determine what the message is parseMsg(dataArr, MAX_UART_MSG_SIZE); } // Checking drive (movement) interrupt if ( int_vec & TIMER2_INT ) { int_vec &= ~TIMER2_INT; // Disable TIMER2 disableTimer(TIMER2_BASE); // Set drive-stop in status vector stat_vec |= DRIVE_STOP; } // Checking measure interrupt if ( int_vec & TIMER1_INT ) { int_vec &= ~TIMER1_INT; // Disable TIMER1 disableTimer(TIMER1_BASE); // Take reading from LIDAR mapData[stepCount++] = readLidar(); SysCtlDelay(2000); // Take step // Note: We need to take double meas at randvillkor (100) !!!!! if ( stepCount > 0 && stepCount < 100 ) { stepDir = 1; } else if ( stepCount >= 100 && stepCount < 200) { stepDir = 0; } else { stepDir = 1; stepCount = 0; // Reset busy-flag stat_vec &= ~TAKE_MEAS; } step = takeStep(step, stepDir); // Request reading from LIDAR reqLidarMeas(); if ( stat_vec & TAKE_MEAS ) { // Restart TIMER1 enableTimer(TIMER1_BASE, (SYSCLOCK / MEASUREMENT_DELAY)); } else { sendUARTDataVector(mapData, MAP_DATA_SIZE); stat_vec &= ~BUSY; } } // Check the drive_stop flag, which always should be set unless we should move if ( stat_vec & DRIVE_STOP ) { // Stop all movement SetPWMLevel(0,0); halt(); // MAKE SURE all drive-flags are not set stat_vec &= ~(DRIVE_F | DRIVE_L | DRIVE_R | DRIVE_LL | BUSY); } // Should we drive? else if ( stat_vec & DRIVE ) { // Remove drive flag stat_vec &= ~DRIVE; // Increase PWM increase_PWM(0,MAX_FORWARD_SPEED,0,MAX_FORWARD_SPEED); if ( stat_vec & DRIVE_F ) { enableTimer(TIMER2_BASE, DRIVE_FORWARD_TIME); } else if ( stat_vec & DRIVE_LL ) { enableTimer(TIMER2_BASE, DRIVE_TURN_180_TIME); } else { enableTimer(TIMER2_BASE, DRIVE_TURN_TIME); } } if ( !(stat_vec & BUSY) ) { // Tasks switch ( stat_vec ) { case ((uint8_t)DRIVE_F) : // Call drive function go_forward(); // Set the drive flag & BUSY stat_vec |= DRIVE | BUSY; break; case ((uint8_t)DRIVE_L) : // Call drive-left function go_left(); // Set the drive flag stat_vec |= DRIVE | BUSY; break; case ((uint8_t)DRIVE_R) : // Call drive-right function go_right(); // Set the drive flag stat_vec |= DRIVE | BUSY; break; case ((uint8_t)DRIVE_LL) : // Call turn 180-degrees function go_back(); // Set the drive flag stat_vec |= DRIVE | BUSY; break; case ((uint8_t)TAKE_MEAS) : // Request reading from LIDAR reqLidarMeas(); // Start TIMER1 enableTimer(TIMER1_BASE, (SYSCLOCK / MEASUREMENT_DELAY)); // if sysclock = 1 s, 1/120 = 8.3 ms // We are busy stat_vec |= BUSY; break; default: break; } } } }