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;
}
}
}
}
