Пример #1
0
TwoDoubles f22(double* x) {
    /* Gallagher with 21 Gaussian peaks, condition up to 1000, one global rotation*/
    int i, j, k, rseed; /*Loop over dim*/
    static unsigned int funcId = 22;
    static double fitvalues[2] = {1.1, 9.1};
    static double maxcondition = 1000.;
    static double arrCondition[NHIGHPEAKS22];
    static double peakvalues[NHIGHPEAKS22];
    static double a = 0.1;
    double tmp2, f = 0., Fadd, Fval, tmp, Fpen = 0., Ftrue = 0.;
    double fac = -0.5 / (double)DIM;
    TwoDoubles res;

    if (!isInitDone)
    {
        rseed = funcId + 10000 * trialid;
        /*INITIALIZATION*/
        Fopt = computeFopt(funcId, trialid);
        computeRotation(rotation, rseed, DIM);
        peaks = peaks22;
        unif(peaks, NHIGHPEAKS22 - 1, rseed);
        rperm = rperm22;
        for (i = 0; i < NHIGHPEAKS22 - 1; i++)
            rperm[i] = i;
        qsort(rperm, NHIGHPEAKS22 - 1, sizeof(int), compare_doubles);
        /* Random permutation*/
        arrCondition[0] = maxcondition;
        peakvalues[0] = 10;
        for (i = 1; i < NHIGHPEAKS22; i++)
        {
            arrCondition[i] = pow(maxcondition, (double)(rperm[i-1])/((double)(NHIGHPEAKS22-2)));
            peakvalues[i] = (double)(i-1)/(double)(NHIGHPEAKS22-2) * (fitvalues[1] - fitvalues[0]) + fitvalues[0];
        }
        arrScales = arrScales22;
        for (i = 0; i < NHIGHPEAKS22; i++)
        {
            unif(peaks, DIM, rseed + 1000 * i);
            for (j = 0; j < DIM; j++)
                rperm[j] = j;
            qsort(rperm, DIM, sizeof(int), compare_doubles);
            for (j = 0; j < DIM; j++)
            {
                arrScales[i][j] = pow(arrCondition[i], ((double)rperm[j])/((double)(DIM-1)) - 0.5);
            }
        }

        unif(peaks, DIM * NHIGHPEAKS22, rseed);
        Xlocal = Xlocal22;
        for (i = 0; i < DIM; i++)
        {
            Xopt[i] = 0.8 * (9.8 * peaks[i] -4.9);
            for (j = 0; j < NHIGHPEAKS22; j++)
            {
                Xlocal[i][j] = 0.;
                for (k = 0; k < DIM; k++)
                {
                    Xlocal[i][j] += rotation[i][k] * (9.8 * peaks[j * DIM + k] -4.9);
                }
                if (j == 0)
                    Xlocal[i][j] *= 0.8;
            }
        }
        isInitDone = 1;
    }
    Fadd = Fopt;

    /* BOUNDARY HANDLING*/
    for (i = 0; i < DIM; i++)
    {
        tmp = fabs(x[i]) - 5.;
        if (tmp > 0.)
        {
            Fpen += tmp * tmp;
        }
    }
    Fadd += Fpen;

    /* TRANSFORMATION IN SEARCH SPACE*/
    for (i = 0; i < DIM; i++)
    {
        tmx[i] = 0.;
        for (j = 0; j < DIM; j++)
        {
            tmx[i] += rotation[i][j] * x[j];
        }
    }

    /* COMPUTATION core*/
    for (i = 0; i < NHIGHPEAKS22; i++)
    {
        tmp2 = 0.;
        for (j = 0; j < DIM; j++)
        {
            tmp = (tmx[j] - Xlocal[j][i]);
            tmp2 += arrScales[i][j] * tmp * tmp;
        }
        tmp2 = peakvalues[i] * exp(fac * tmp2);
        f = fmax(f, tmp2);
    }

    f = 10. - f;
    if (f > 0)
    {
        Ftrue = log(f)/a;
        Ftrue = pow(exp(Ftrue + 0.49*(sin(Ftrue) + sin(0.79*Ftrue))), a);
    }
    else if (f < 0)
    {
        Ftrue = log(-f)/a;
        Ftrue = -pow(exp(Ftrue + 0.49*(sin(0.55 * Ftrue) + sin(0.31*Ftrue))), a);
    }
    else
        Ftrue = f;

    Ftrue *= Ftrue;
    Ftrue += Fadd;
    Fval = Ftrue; /* without noise*/
    /* free(Xopt); //Not used!*/

    res.Fval = Fval;
    res.Ftrue = Ftrue;
    return res;
}
Пример #2
0
void display(void)
{

  // Clear the screen____________________________________


if((cam.x < goalx + 2 && cam.x > goalx - 2) && (cam.z < goalz + 2 && cam.z > goalz - 2))
{
	
	third_person = true;

}
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);



  // Variables
    
  mat4 total, modelView, camMatrix;
  GLfloat t =(GLfloat)glutGet(GLUT_ELAPSED_TIME)/1000; //Scale with constant to make animation slower.
 


  //Read input_____________________________________________
  if (third_person)
    {

	goal_found = true;

      if (glutKeyIsDown('t'))
	{ 
	  third_person = false;  
	}
  
  vec3 pos =  {200, 200, 150};// SetVector(keyx,keyy,keyz);
      //Mouse control_____________________________________________
      //fmin and fmax doesn't really make sense but looks good. 
      GLfloat mousexlocal = 0;
      GLfloat mouseylocal = 0;

      if (mousex < 350 ||  mousex > 250)
	{
	  mousexlocal = 300-mousex;
	}
      else
	{
	  mousexlocal = 0;
	}

      deltaphi = deltaphi + deltaphi*-mousexlocal;
      deltaphi = fmin(0.0001,deltaphi);
      deltaphi = fmax(deltaphi,2 * PI - 0.0001);
      forward = MultVec3(Ry(deltaphi*-mousexlocal), forward);

      vec3 crossvec = CrossProduct(forward,up);

      if (mousey < 350 || mousey > 250)
	{
	  mouseylocal = 300-mousey;
	}  
      else
	{
	  mouseylocal = 0;
	}

      deltatheta = deltatheta + deltatheta* -mouseylocal;
      deltatheta = fmin(0.0001,deltatheta);
      deltatheta = fmax(deltatheta,2*PI - 0.0001);
      forward = MultVec3( ArbRotate(crossvec,deltatheta*-mouseylocal), forward);

      //Create camera matrix_____________________________________________
      camMatrix = lookAtv(pos,VectorAdd(forward,pos),up);
    }




// FIRST PERSON VIEW --------------------------------------------------------
  else
    {
      
      vec3 moveVec;
      
      if (glutKeyIsDown('w')) // forwards, obviously
	{
	  vec3 oldcam = cam; //Save old position to check if its legal.
	  moveVec = ScalarMult(VectorSub(cam, SetVector(lookAtPoint.x, cam.y, lookAtPoint.z)), 0.1); // VectorSub makes a vector in the direction we're looking.
	  cam = VectorSub(cam, moveVec); // The new camera position is the old camera position + the move vector
	  cam.y = getHeight(&ttex,cam.x, cam.z)+2; // Get the correct height from the new position  
	  GLfloat maze_height = getHeight(&ttexm, cam.x, cam.z); // Get the height of the maze at the new (possible) camera position
	  GLfloat ground_height = getHeight(&ttex, cam.x, cam.z); // Get the height of the ground at the new (possible) camera position


	  //Check if the new position is legal
	    if ((ground_height - maze_height) < ground_height) // If 0, shouldn't move ever: OK!
						   // When does it start to move??
	      {
		printf("woot");
		cam = oldcam;
	      }
	}
       else if (glutKeyIsDown('t'))
	 {
	   third_person = true;
	 }

       GLfloat mousexlocal = 0;
      GLfloat mouseylocal = 0;

      if (mousex < 350 ||  mousex > 250)
	{
	  mousexlocal = 300-mousex;
	}
      else
	{
	  mousexlocal = 0;
	}

      deltaphi = deltaphi + deltaphi*-mousexlocal;
      deltaphi = fmin(0.0001,deltaphi);
      deltaphi = fmax(deltaphi,2 * PI - 0.0001);
      forward = MultVec3(Ry(deltaphi*-mousexlocal), forward);

      vec3 crossvec = CrossProduct(forward,up);

      if (mousey < 350 || mousey > 250)
	{
	  mouseylocal = 300-mousey;
	}  
      else
	{
	  mouseylocal = 0;
	}

      deltatheta = deltatheta + deltatheta* -mouseylocal;
      deltatheta = fmin(0.0001,deltatheta);
      deltatheta = fmax(deltatheta,2*PI - 0.0001);
      forward = MultVec3( ArbRotate(crossvec,deltatheta*-mouseylocal), forward);
      lookAtPoint = VectorAdd(forward,cam);
      //Create camera matrix_____________________________________________
      camMatrix = lookAtv(cam,lookAtPoint,up);

    }

  
  // --------------------------------------------------------//
  // SKYBOX
  // --------------------------------------------------------//

	glDisable(GL_DEPTH_TEST);
	glDisable(GL_CULL_FACE);

	glUseProgram(programSky);
	glActiveTexture(GL_TEXTURE2);
	//glBindTexture(GL_TEXTURE_2D, tex2);
	trans = T(0.0, 0.0, 0.0);

       	rot = Rx(0);
	total = Mult(trans, rot);
       	glUniformMatrix4fv(glGetUniformLocation(programSky, "mdlMatrix"), 1, GL_TRUE, total.m);

	lookSky = camMatrix;

	lookSky.m[3] = 0;
	lookSky.m[7] = 0;
	lookSky.m[11] = 0;

	glUniformMatrix4fv(glGetUniformLocation(programSky, "lookMatrix"), 1, GL_TRUE, lookSky.m);
	glUniformMatrix4fv(glGetUniformLocation(programSky, "projectionMatrix"), 1, GL_TRUE, projectionMatrix.m);

	glUniform1i(glGetUniformLocation(programSky, "texUnit2"), 2); // Texture unit 2
	DrawModel(skybox, programSky, "in_Position", NULL, "inTexCoord");


	glUseProgram(program);
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_CULL_FACE);

	//------------------------------------------------------//
	//------------------------------------------------------//



 
  modelView = IdentityMatrix();
  total = Mult(camMatrix, modelView);

  glActiveTexture(GL_TEXTURE1);
  glUseProgram(program);
  glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
  glBindTexture(GL_TEXTURE_2D, tex2);		// Bind Our Texture tex1
  DrawModel(tm, program,"inPosition", "inNormal", "inTexCoord");

  glUseProgram(program);
  glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 0, GL_TRUE, total.m);
  glBindTexture(GL_TEXTURE_2D, tex1);		// Bind Our Texture tex1
  DrawModel(mm, program,"inPosition", "inNormal", "inTexCoord");

  GLfloat y = getHeight(&ttexm, 200, 147);
  mat4 trans = T(goalx, 2, goalz);
  mat4 scale = S(1, 1, 1);
 total = Mult(total, trans);
 total = Mult(total, scale);

 
 if (goal_found)
{
	char* string = "GOOD JOB!! You won :D";
	sfDrawString(100, 100, string);
	//while(true){};
}



  glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
  glBindTexture(GL_TEXTURE_2D, tex3);		// Bind Our Texture tex1


  DrawModel(goal, program,"inPosition", "inNormal", "inTexCoord");


  



  printError("display 2");
	
  glutSwapBuffers();
  printf("x %f\n", cam.x);
  printf("y %f\n", cam.y);
  printf("z %f\n", cam.z);
  
  /* if ( (cam.x < 95) && (cam.x > 85) && */
  /*      (cam.z < 85) && (cam.z > 75)) */

  
}
Пример #3
0
// prime_server stuff
worker_t::result_t Paparazzi::work (const std::list<zmq::message_t>& job, void* request_info){
    //false means this is going back to the client, there is no next stage of the pipeline
    worker_t::result_t result{false};

    //this type differs per protocol hence the void* fun
    auto& info = *static_cast<http_request_t::info_t*>(request_info);

    // Try to generate a response 
    http_response_t response;
    try {
        // double start_call = getTime();

        //TODO: 
        //   - actually use/validate the request parameters
        auto request = http_request_t::from_string(static_cast<const char*>(job.front().data()), job.front().size());

        if (request.path == "/check") {
            // ELB check
            response = http_response_t(200, "OK", "OK", headers_t{CORS, TXT_MIME});
        } else {
            //  SCENE
            //  ---------------------
            auto scene_itr = request.query.find("scene");
            if (scene_itr == request.query.cend() || scene_itr->second.size() == 0) {
                // If there is NO SCENE QUERY value 
                if (request.body.empty()) 
                    // if there is not POST body content return error...
                    throw std::runtime_error("scene is required punk");

                // ... other whise load content
                setSceneContent(request.body);
                // The size of the custom scene is unique enough
                result.heart_beat = std::to_string(request.body.size());
            }
            else {
                // If there IS a SCENE QUERRY value load it
                setScene(scene_itr->second.front());
                result.heart_beat = scene_itr->second.front();
            }

            bool size_and_pos = true;
            float pixel_density = 1.0f;

            //  SIZE
            //  ---------------------
            auto width_itr = request.query.find("width");
            if (width_itr == request.query.cend() || width_itr->second.size() == 0)
                size_and_pos = false;
            auto height_itr = request.query.find("height");
            if (height_itr == request.query.cend() || height_itr->second.size() == 0)
                size_and_pos = false;
            auto density_itr = request.query.find("density");
            if (density_itr != request.query.cend() && density_itr->second.size() > 0)
                pixel_density = fmax(1.,std::stof(density_itr->second.front()));
            //  POSITION
            //  ---------------------
            auto lat_itr = request.query.find("lat");
            if (lat_itr == request.query.cend() || lat_itr->second.size() == 0)
                size_and_pos = false;
            auto lon_itr = request.query.find("lon");
            if (lon_itr == request.query.cend() || lon_itr->second.size() == 0)
                size_and_pos = false;
            auto zoom_itr = request.query.find("zoom");
            if (zoom_itr == request.query.cend() || zoom_itr->second.size() == 0)
                size_and_pos = false;
            

            if (size_and_pos) {
                // Set Map and OpenGL context size
                setSize(std::stoi(width_itr->second.front()), std::stoi(height_itr->second.front()), pixel_density);
                setPosition(std::stod(lon_itr->second.front()), std::stod(lat_itr->second.front()));
                setZoom(std::stof(zoom_itr->second.front()));
            } else {
                const std::regex re("\\/(\\d*)\\/(\\d*)\\/(\\d*)\\.png");
                std::smatch match;

                if (std::regex_search(request.path, match, re) && match.size() == 4) {
                    setSize(256,256, pixel_density);

                    int tile_coord[3] = {0,0,0};
                    for (int i = 0; i < 3; i++) {
                        std::istringstream cur(match.str(i+1));
                        cur >> tile_coord[i];
                    }
                    futile_coord_s tile;
                    tile.z = tile_coord[0];
                    setZoom(tile.z);

                    tile.x = tile_coord[1];
                    tile.y = tile_coord[2];
                    futile_bounds_s bounds;
                    futile_coord_to_bounds(&tile, &bounds);

                    setPosition(bounds.minx + (bounds.maxx-bounds.minx)*0.5,bounds.miny + (bounds.maxy-bounds.miny)*0.5);
                }
                else {
                    throw std::runtime_error("not enought data to construct image");
                }
            }

            //  OPTIONAL tilt and rotation
            //  ---------------------
            auto tilt_itr = request.query.find("tilt");
            if (tilt_itr != request.query.cend() && tilt_itr->second.size() != 0) {
                // If TILT QUERRY is provided assigned ...
                setTilt(std::stof(tilt_itr->second.front()));
            }
            else {
                // othewise use default (0.)
                setTilt(0.0f);
            }

            auto rotation_itr = request.query.find("rotation");
            if (rotation_itr != request.query.cend() && rotation_itr->second.size() != 0) {
                // If ROTATION QUERRY is provided assigned ...
                setRotation(std::stof(rotation_itr->second.front()));
            }
            else {
                // othewise use default (0.)
                setRotation(0.0f);
            }

            // Time to render
            //  ---------------------
            std::string image;
            if (m_map) {
                update();

                // Render Tangram Scene
                m_aab->bind();
                m_map->render();
                m_aab->unbind();
   
                // Once the main FBO is draw take a picture
                m_aab->getPixelsAsString(image);
                // double total_time = getTime()-start_call;
                // LOG("TOTAL CALL: %f", total_time);
                // LOG("TOTAL speed: %f millisec per pixel", (total_time/((m_width * m_height)/1000.0)));
            }

            response = http_response_t(200, "OK", image, headers_t{CORS, PNG_MIME});
        }
Пример #4
0
LinePatternPos LineAtlas::addDash(const std::vector<float>& dasharray, LinePatternCap patternCap) {
    int n = patternCap == LinePatternCap::Round ? 7 : 0;
    int dashheight = 2 * n + 1;
    const uint8_t offset = 128;

    if (nextRow + dashheight > height) {
        Log::Warning(Event::OpenGL, "line atlas bitmap overflow");
        return LinePatternPos();
    }

    float length = 0;
    for (const float part : dasharray) {
        length += part;
    }

    float stretch = width / length;
    float halfWidth = stretch * 0.5;
    // If dasharray has an odd length, both the first and last parts
    // are dashes and should be joined seamlessly.
    bool oddLength = dasharray.size() % 2 == 1;

    for (int y = -n; y <= n; y++) {
        int row = nextRow + n + y;
        int index = width * row;

        float left = 0;
        float right = dasharray[0];
        unsigned int partIndex = 1;

        if (oddLength) {
            left -= dasharray.back();
        }

        for (int x = 0; x < width; x++) {

            while (right < x / stretch) {
                left = right;
                right = right + dasharray[partIndex];

                if (oddLength && partIndex == dasharray.size() - 1) {
                    right += dasharray.front();
                }

                partIndex++;
            }

            float distLeft = fabs(x - left * stretch);
            float distRight = fabs(x - right * stretch);
            float dist = fmin(distLeft, distRight);
            bool inside = (partIndex % 2) == 1;
            int signedDistance;

            if (patternCap == LinePatternCap::Round) {
                float distMiddle = n ? (float)y / n * (halfWidth + 1) : 0;
                if (inside) {
                    float distEdge = halfWidth - fabs(distMiddle);
                    signedDistance = sqrt(dist * dist + distEdge * distEdge);
                } else {
                    signedDistance = halfWidth - sqrt(dist * dist + distMiddle * distMiddle);
                }

            } else {
                signedDistance = int((inside ? 1 : -1) * dist);
            }

            data[index + x] = fmax(0, fmin(255, signedDistance + offset));
        }
    }

    LinePatternPos position;
    position.y = (0.5 + nextRow + n) / height;
    position.height = (2.0 * n) / height;
    position.width = length;

    nextRow += dashheight;

    dirty = true;

    return position;
}
Пример #5
0
void EarlyRegression(int Gid){

    char buf[512]; FILE *_fp = NULL;
    vector<Point6f> feat; vector<int> label;
    for(int gid = 2; gid < 3; ++gid){
        sprintf(buf, "FlowFeature/%i.txt", gid);
        _fp = fopen(buf, "r");
        float f[6] = {0};
        while(fscanf(_fp,"%f\t%f\t%f\t%f\t%f\t%f\n",&f[0],&f[1],&f[2],&f[3],&f[4], &f[5]) != EOF){
            feat.push_back(Point6f(f));
        } fclose(_fp);
        sprintf(buf, "FlowFeature/%d_gt.txt", gid);
        _fp = fopen(buf, "r"); int t = 0;
        while(fscanf(_fp,"%d\n", &t) != EOF) label.push_back(t);
        fclose(_fp);
    }
    float _fmin[6]={INFINITY,INFINITY,INFINITY,INFINITY,INFINITY,INFINITY};
    float _fmax[6]={-1,-1,-1,-1,-1,-1};
    Point6f fmin(_fmin), fmax(_fmax);
    for(int i = 0; i < feat.size(); ++i){ 
        fmin = min(fmin,feat[i]);
        fmax = max(fmax,feat[i]);
    }
    float fscore = 0, opt_rec = 0, opt_prec = 0, grid = 1000;
    for(float th = fmin.u; th < fmax.u; th += (fmax.u-fmin.u)/grid){
        for(float th2 = fmin.x; th2 < fmax.x; th2 += (fmax.x-fmin.x)/grid){
            int tp(0),fp(0),fn(0),tn(0);
            for(int i = 0 ; i < feat.size(); ++i){
                if(feat[i].u > th && feat[i].x > th2 ){
                    if(label[i] == 1)++tp;
                    else ++fp;
                }else{
                    if(label[i] == 1)++fn;
                    else ++tn;
                }
            }
            float _prec = float(tp)/float(max(1,tp+fp)), _rec = float(tp)/float(max(1,tp+fn));
            float _f1 = F_Measure(2.0, _prec, _rec);
            if(_f1 > fscore){ fscore = _f1, opt_rec = th;}
        }
    }
    
    for(int g = 0; g < DATASET_SIZE; g++){ /*for all data*/
        sprintf(buf, "FlowFeature/%i.txt", g);
        _fp = fopen(buf, "r");
        float f[6]; feat.clear(); label.clear();
        while(fscanf(_fp,"%f\t%f\t%f\t%f\t%f\t%f\n",&f[0],&f[1],&f[2],&f[3],&f[4], &f[5]) != EOF){
            feat.push_back(Point6f(f));
        } fclose(_fp);
        sprintf(buf, "FlowFeature/%d_gt.txt", g);
        _fp = fopen(buf, "r"); int t = 0;
        while(fscanf(_fp,"%d\n", &t) != EOF) label.push_back(t);
        fclose(_fp);
        
        ////////////////////////////
        int tp(0),fp(0),fn(0),tn(0);
        for(int i = 0 ; i < feat.size(); ++i){
            
            if(feat[i].u > opt_rec && feat[i].x > opt_prec){
                if(label[i] == 1)++tp;
                else ++fp;
            }else{
                if(label[i] == 1)++fn;
                else ++tn;
            }
        }
        float _prec = float(tp)/float(max(1,tp+fp)), _rec = float(tp)/float(max(1,tp+fn));
        float _f1 = 2.0*_prec*_rec/max(1.0f,(_prec+_rec));
        printf("\n[%d]CMAT[+/-: %d,%d][PRF:%f; %f; %f]\n", g, int(tp+fn), int(fp+tn), _prec, _rec, _f1);
        printf("%d\t%d\n%d\t%d\n", tp, fp, fn, tn);
        printf("Accuracy:[%f]\n\n", float(tp+tn)/float(tp+fp+fn+tn));
    }
}
Пример #6
0
static int svdcmp(double **u, int m, int n, double w[], double **v) {
  const int max_its = 30;
  int flag, i, its, j, jj, k, l, nm;
  double anorm, c, f, g, h, s, scale, x, y, z;
  double *rv1 = (double *)aom_malloc(sizeof(*rv1) * (n + 1));
  g = scale = anorm = 0.0;
  for (i = 0; i < n; i++) {
    l = i + 1;
    rv1[i] = scale * g;
    g = s = scale = 0.0;
    if (i < m) {
      for (k = i; k < m; k++) scale += fabs(u[k][i]);
      if (scale != 0.) {
        for (k = i; k < m; k++) {
          u[k][i] /= scale;
          s += u[k][i] * u[k][i];
        }
        f = u[i][i];
        g = -sign(sqrt(s), f);
        h = f * g - s;
        u[i][i] = f - g;
        for (j = l; j < n; j++) {
          for (s = 0.0, k = i; k < m; k++) s += u[k][i] * u[k][j];
          f = s / h;
          for (k = i; k < m; k++) u[k][j] += f * u[k][i];
        }
        for (k = i; k < m; k++) u[k][i] *= scale;
      }
    }
    w[i] = scale * g;
    g = s = scale = 0.0;
    if (i < m && i != n - 1) {
      for (k = l; k < n; k++) scale += fabs(u[i][k]);
      if (scale != 0.) {
        for (k = l; k < n; k++) {
          u[i][k] /= scale;
          s += u[i][k] * u[i][k];
        }
        f = u[i][l];
        g = -sign(sqrt(s), f);
        h = f * g - s;
        u[i][l] = f - g;
        for (k = l; k < n; k++) rv1[k] = u[i][k] / h;
        for (j = l; j < m; j++) {
          for (s = 0.0, k = l; k < n; k++) s += u[j][k] * u[i][k];
          for (k = l; k < n; k++) u[j][k] += s * rv1[k];
        }
        for (k = l; k < n; k++) u[i][k] *= scale;
      }
    }
    anorm = fmax(anorm, (fabs(w[i]) + fabs(rv1[i])));
  }

  for (i = n - 1; i >= 0; i--) {
    if (i < n - 1) {
      if (g != 0.) {
        for (j = l; j < n; j++) v[j][i] = (u[i][j] / u[i][l]) / g;
        for (j = l; j < n; j++) {
          for (s = 0.0, k = l; k < n; k++) s += u[i][k] * v[k][j];
          for (k = l; k < n; k++) v[k][j] += s * v[k][i];
        }
      }
      for (j = l; j < n; j++) v[i][j] = v[j][i] = 0.0;
    }
    v[i][i] = 1.0;
    g = rv1[i];
    l = i;
  }
  for (i = AOMMIN(m, n) - 1; i >= 0; i--) {
    l = i + 1;
    g = w[i];
    for (j = l; j < n; j++) u[i][j] = 0.0;
    if (g != 0.) {
      g = 1.0 / g;
      for (j = l; j < n; j++) {
        for (s = 0.0, k = l; k < m; k++) s += u[k][i] * u[k][j];
        f = (s / u[i][i]) * g;
        for (k = i; k < m; k++) u[k][j] += f * u[k][i];
      }
      for (j = i; j < m; j++) u[j][i] *= g;
    } else {
      for (j = i; j < m; j++) u[j][i] = 0.0;
    }
    ++u[i][i];
  }
  for (k = n - 1; k >= 0; k--) {
    for (its = 0; its < max_its; its++) {
      flag = 1;
      for (l = k; l >= 0; l--) {
        nm = l - 1;
        if ((double)(fabs(rv1[l]) + anorm) == anorm || nm < 0) {
          flag = 0;
          break;
        }
        if ((double)(fabs(w[nm]) + anorm) == anorm) break;
      }
      if (flag) {
        c = 0.0;
        s = 1.0;
        for (i = l; i <= k; i++) {
          f = s * rv1[i];
          rv1[i] = c * rv1[i];
          if ((double)(fabs(f) + anorm) == anorm) break;
          g = w[i];
          h = pythag(f, g);
          w[i] = h;
          h = 1.0 / h;
          c = g * h;
          s = -f * h;
          for (j = 0; j < m; j++) {
            y = u[j][nm];
            z = u[j][i];
            u[j][nm] = y * c + z * s;
            u[j][i] = z * c - y * s;
          }
        }
      }
      z = w[k];
      if (l == k) {
        if (z < 0.0) {
          w[k] = -z;
          for (j = 0; j < n; j++) v[j][k] = -v[j][k];
        }
        break;
      }
      if (its == max_its - 1) {
        aom_free(rv1);
        return 1;
      }
      assert(k > 0);
      x = w[l];
      nm = k - 1;
      y = w[nm];
      g = rv1[nm];
      h = rv1[k];
      f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
      g = pythag(f, 1.0);
      f = ((x - z) * (x + z) + h * ((y / (f + sign(g, f))) - h)) / x;
      c = s = 1.0;
      for (j = l; j <= nm; j++) {
        i = j + 1;
        g = rv1[i];
        y = w[i];
        h = s * g;
        g = c * g;
        z = pythag(f, h);
        rv1[j] = z;
        c = f / z;
        s = h / z;
        f = x * c + g * s;
        g = g * c - x * s;
        h = y * s;
        y *= c;
        for (jj = 0; jj < n; jj++) {
          x = v[jj][j];
          z = v[jj][i];
          v[jj][j] = x * c + z * s;
          v[jj][i] = z * c - x * s;
        }
        z = pythag(f, h);
        w[j] = z;
        if (z != 0.) {
          z = 1.0 / z;
          c = f * z;
          s = h * z;
        }
        f = c * g + s * y;
        x = c * y - s * g;
        for (jj = 0; jj < m; jj++) {
          y = u[jj][j];
          z = u[jj][i];
          u[jj][j] = y * c + z * s;
          u[jj][i] = z * c - y * s;
        }
      }
      rv1[l] = 0.0;
      rv1[k] = f;
      w[k] = x;
    }
  }
  aom_free(rv1);
  return 0;
}
Пример #7
0
 inline 
 typename stan::return_type<T0,T1>::type
 operator()(const T0& arg1,
            const T1& arg2) const {
   return fmax(arg1,arg2);
 }
Пример #8
0
// Solve numerically for lambda
// Subclasses can override for analytical solution if possible
double SLMaterial::SolveForLambdaBracketed(MPMBase *mptr,int np,double strial,Tensor *stk,double Gred,
								double psKred,double Ptrial,double delTime,HardeningAlpha *a,void *properties,int offset) const
{
	if(np==PLANE_STRESS_MPM)
	{	// not allowed
		return 0.;
	}
	else
	{	// solve - sqrt(2/3)GetYield(alpha+dalpha,dalpha) + strial - 2 GRed sqrt(3/2)dalpha  = 0
		SLProperties *p = (SLProperties *)properties;
		double rho = parent->GetRho(mptr);
		
		// test lower limit
		a->dalpha = delTime*p->epdotmin;
		a->alpint = mptr->GetHistoryDble(0,offset) + a->dalpha;
		p->isConstantYT = true;
		p->constantYT = YTmin;
		double gmin = strial - 2.*Gred*a->dalpha/SQRT_TWOTHIRDS - SQRT_TWOTHIRDS*GetYield(mptr,np,delTime,a,p);
		if(gmin<0.)
		{	// low strain rate answer between 0 and epdotmin
			double lambdak = HardeningLawBase::SolveForLambda(mptr,np,strial,stk,Gred,psKred,1.,delTime,a,p,offset);
			//cout << "# low strain rate condition " << lambdak << " should be below " << delTime*epdotmin/SQRT_TWOTHIRDS << endl;
			p->isConstantYT = false;
			mptr->SetHistoryDble(YT_HISTORY,p->currentYTred*rho*1.e-6,offset);
			mptr->SetHistoryDble(EPDOT_HISTORY,SQRT_TWOTHIRDS*lambdak/delTime,offset);
			return lambdak;
		}
		
		// test upper limit
		a->dalpha = delTime*p->epdotmax;
		a->alpint = mptr->GetHistoryDble(0,offset) + a->dalpha;
		p->constantYT=YPred;
		double gmax =  strial - 2.*Gred*a->dalpha/SQRT_TWOTHIRDS - SQRT_TWOTHIRDS*GetYield(mptr,np,delTime,a,p);
		if(gmax>0.)
		{	// high string rate answer for rate higher than epmax
			double lambdak=HardeningLawBase::SolveForLambda(mptr,np,strial,stk,Gred,psKred,1.,delTime,a,p,offset);
			//cout << "# high strain rate condition " << lambdak << " should be above " << delTime*epdotmax/SQRT_TWOTHIRDS << endl;
			p->isConstantYT = false;
			mptr->SetHistoryDble(YT_HISTORY,p->currentYTred*rho*1.e-6,offset);
			mptr->SetHistoryDble(EPDOT_HISTORY,SQRT_TWOTHIRDS*lambdak/delTime,offset);
			return lambdak;
		}
		p->isConstantYT=false;
		
		// Newton method in ln epdot space
		p->currentYTred=fmax(YTmin,mptr->GetHistoryDble(YT_HISTORY,offset)*1.e6/rho);
		double epdot=GetEpdot(p->currentYTred,p->TwoUkkT);
		double logepdot = log(epdot);
		a->dalpha = epdot*delTime;
		a->alpint = mptr->GetHistoryDble(0,offset) + a->dalpha;
		int step=1;
		while(true)
		{	// update iterative variables (alpha, dalpha)
			double glam = -SQRT_TWOTHIRDS*GetYield(mptr,np,delTime,a,p) + strial - 2.*Gred*a->dalpha/SQRT_TWOTHIRDS;
			double slope = -2.*Gred*a->dalpha/SQRT_TWOTHIRDS - GetKPrime(mptr,np,delTime,a,p);
			double delLogepdot = -glam/slope;
			logepdot += delLogepdot;
			
			// check for convergence
			a->dalpha = exp(logepdot)*delTime;
			a->alpint = mptr->GetHistoryDble(0,offset) + a->dalpha;
			if(step>20 || fabs(delLogepdot)<0.0001) break;
			step++;
		}
	
		// set history when done
		mptr->SetHistoryDble(YT_HISTORY,p->currentYTred*rho*1.e-6,offset);
		mptr->SetHistoryDble(EPDOT_HISTORY,a->dalpha/delTime,offset);
		return a->dalpha/SQRT_TWOTHIRDS;
		
	}
	
}
Пример #9
0
static inline 
rrr_t* compute_new_rrr(cluster_t *cl, int tid, proc_t *procinfo,
		       val_t *Wstruct, vec_t *Zstruct,
		       tol_t *tolstruct, double *work, int *iwork)
{
  /* From inputs */
  int              cl_begin    = cl->begin;
  int              cl_end      = cl->end;
  int              cl_size     = cl_end - cl_begin + 1;
  int              depth       = cl->depth;
  int              bl_begin    = cl->bl_begin;
  int              bl_end      = cl->bl_end;
  int              bl_size     = bl_end - bl_begin + 1;
  double           bl_spdiam   = cl->bl_spdiam;
  rrr_t            *RRR_parent = cl->RRR;

  double *restrict Werr        = Wstruct->Werr;
  double *restrict Wgap        = Wstruct->Wgap;
  int    *restrict Windex      = Wstruct->Windex;
  double *restrict Wshifted    = Wstruct->Wshifted;

  double           pivmin      = tolstruct->pivmin;

  /* New RRR */
  double           *restrict D,         *restrict L;
  double           *restrict DL,        *restrict DLL;
  double           *restrict D_parent,  *restrict L_parent;
  double           *DL_parent,          *DLL_parent;
  double           left_gap, right_gap, tau, fudge;
  rrr_t            *RRR;
  double           RQtol = 2*DBL_EPSILON;
  double           savegap;

  /* Others */
  int              i, k, p, info;
  double           tmp;
  int              offset, IONE=1;

  /* Allocate memory for new representation for cluster */
  D   = (double *) malloc(bl_size * sizeof(double));
  assert(D != NULL);
  
  L   = (double *) malloc(bl_size * sizeof(double));
  assert(L != NULL);
  
  DL  = (double *) malloc(bl_size * sizeof(double));
  assert(DL != NULL);
  
  DLL = (double *) malloc(bl_size * sizeof(double));
  assert(DLL != NULL);

  /* Recompute DL and DLL */
  D_parent = RRR_parent->D;
  L_parent = RRR_parent->L;
  for (i=0; i<bl_size-1; i++) {
    tmp    = D_parent[i]*L_parent[i];
    DL[i]  = tmp;
    DLL[i] = tmp*L_parent[i];
  }
  DL_parent  = DL;
  DLL_parent = DLL;

  /* to shift as close as possible refine extremal eigenvalues */
  for (k=0; k<2; k++) {
    if (k == 0) {
      p              = Windex[cl_begin];
      savegap        = Wgap[cl_begin];
      Wgap[cl_begin] = 0.0;
    } else {
      p              = Windex[cl_end  ];
      savegap        = Wgap[cl_end];
      Wgap[cl_end]   = 0.0;
    }
    
    offset  = Windex[cl_begin] - 1;

    LAPACK(dlarrb)
    (&bl_size, D_parent, DLL_parent, &p, &p, &RQtol, &RQtol, &offset, 
     &Wshifted[cl_begin], &Wgap[cl_begin], &Werr[cl_begin], work, iwork, 
     &pivmin, &bl_spdiam, &bl_size, &info);
    assert( info == 0 );

    if (k == 0) {
      Wgap[cl_begin] = fmax(0, (Wshifted[cl_begin+1]-Werr[cl_begin+1])
			    - (Wshifted[cl_begin]+Werr[cl_begin]) );
    } else {
      Wgap[cl_end]   = savegap;
    }
  } /* end k */

  left_gap  = cl->lgap;
  right_gap = Wgap[cl_end];

  /* Compute new RRR and store it in D and L */
  LAPACK(dlarrf)
  (&bl_size, D_parent, L_parent, DL_parent, &IONE, &cl_size, 
   &Wshifted[cl_begin], &Wgap[cl_begin], &Werr[cl_begin], &bl_spdiam, &left_gap,
   &right_gap, &pivmin, &tau, D, L, work, &info);
  assert(info == 0);

  /* Update shift and store it */
  tmp = L_parent[bl_size-1] + tau;
  L[bl_size-1] = tmp;

  /* Compute D*L and D*L*L */
  for (i=0; i<bl_size-1; i++) {
    tmp    = D[i]*L[i];
    DL[i]  = tmp;
    DLL[i] = tmp*L[i];
  }

  /* New RRR of cluster is usually created at the parent level and
   * initialized to parent RRR, now reset to contain new RRR */
  if (RRR_parent->copied_parent_rrr == true) {
    free(RRR_parent->D);
    free(RRR_parent->L);
  }
  RRR = PMR_reset_rrr(RRR_parent, D, L, DL, DLL, bl_size, depth+1);
  
  /* Update shifted eigenvalues */
  for (k=cl_begin; k<=cl_end; k++) {
    fudge  = THREE * DBL_EPSILON * fabs( Wshifted[k] );
    Wshifted[k] -= tau;
    fudge += FOUR * DBL_EPSILON * fabs( Wshifted[k] );
    Werr[k] += fudge;
  }

  /* Assure that structure is not freed while it is processed */
  PMR_increment_rrr_dependencies(RRR);

  return(RRR);
} /* end compute_new_rrr */
Пример #10
0
void World::OnUpdate(const sf::Time& dt)
{
    // Update the camera.
    cam.Update();

    // Move the camera.
    if (sf::Keyboard::isKeyPressed(sf::Keyboard::W))
    {
        cam.Move(Vector3(0.0f, 0.0f, -5.0f * dt.asSeconds()));
    }
    else if (sf::Keyboard::isKeyPressed(sf::Keyboard::S))
    {
        cam.Move(Vector3(0.0f, 0.0f, 5.0f * dt.asSeconds()));
    }

    if (sf::Keyboard::isKeyPressed(sf::Keyboard::A))
    {
        cam.Move(Vector3(-5.0f * dt.asSeconds()));
    }
    else if (sf::Keyboard::isKeyPressed(sf::Keyboard::D))
    {
        cam.Move(Vector3(5.0f * dt.asSeconds()));
    }

    // Rotate camera via mouse.
    sf::Vector2i mpos = sf::Mouse::getPosition(window);
    if(mouseEnabled) sf::Mouse::setPosition(sf::Vector2i(WINDOW_WIDTH / 2, WINDOW_HEIGHT / 2), window);
    sf::Vector2i deltaPos = sf::Mouse::getPosition(window) - mpos;
    cam.Rotate(deltaPos.x * 1.5f * dt.asSeconds(), deltaPos.y * 1.5f * dt.asSeconds(), 0.0f);

    // Sound Spatialization
    //std::cout << "Pos: " << cam.GetPosition() << "\n";
    sf::Listener::setPosition(cam.GetPosition().ToSFMLVec());
    sf::Listener::setDirection(cam.GetDirection().ToSFMLVec());
    sf::Listener::setUpVector(cam.GetUp().ToSFMLVec());

   // if (sndExplode.getStatus() == sf::Sound::Stopped)
        //sndExplode.play();

    // Move light up and down.
    Vector3 lcpos = pointLights[0].GetPosition();

    if (sf::Keyboard::isKeyPressed(sf::Keyboard::Left))
        exposure -= 1.0f * dt.asSeconds();
    else if (sf::Keyboard::isKeyPressed(sf::Keyboard::Right))
        exposure += 1.0f * dt.asSeconds();

    if (sf::Keyboard::isKeyPressed(sf::Keyboard::Up))
    {
        const Color& col = pointLights[0].GetColor();
        pointLights[0].SetColor(Color(col.R(), col.G(), col.B() + 1.0f * dt.asSeconds()));
        std::cout << col.B() << "\n";
    }
    else if (sf::Keyboard::isKeyPressed(sf::Keyboard::Down))
    {
        const Color& col = pointLights[0].GetColor();
        pointLights[0].SetColor(Color(col.R(), col.G(), col.B() - 1.0f * dt.asSeconds()));
        std::cout << col.B() << "\n";
    }

    // Randomize Attenuations.
    if (sf::Keyboard::isKeyPressed(sf::Keyboard::I))
    {
        for (auto& l : pointLights)
        {
            float catt = ((float)rand() / (float)RAND_MAX);
            float latt = ((float)rand() / (float)RAND_MAX);
            float eatt = ((float)rand() / (float)RAND_MAX);

            l.SetAttenuationConstant(catt);
            l.SetAttenuationLinear(latt);
            l.SetAttenuationExponential(eatt);

            const Color& c = l.GetColor();
            float mc = fmax(fmax((float)c.R(), (float)c.G()), (float)c.B());
            float ret = (-latt + sqrtf(latt * latt - 4 * eatt * (eatt - 256 * mc * l.GetDiffuseIntensity()))) / 2 * eatt;

            l.SetScale(Vector3(ret, ret, ret));
        }
    }

    // Randomize intensity.
    if (sf::Keyboard::isKeyPressed(sf::Keyboard::K))
    {
        for (auto& l : pointLights)
        {
            float ambient = ((float)rand() / (float)RAND_MAX);
            float diff = ((float)rand() / (float)RAND_MAX);

            l.SetAmbientIntensity(ambient);
            l.SetDiffuseIntensity(diff);

            // Generate Color
            float r = float_rand(0.0f, maxCol);
            float g = float_rand(0.0f, maxCol);
            float b = float_rand(0.0f, maxCol);
            l.SetColor(Color(r, g, b, 1.0f));
        }
    }

    // Uniforms
    mv += 0.2f * dt.asSeconds();
    pointLights[0].SetPosition(Matrix::GetVectorTransform(Matrix::CreateYRotation(mv), spherePos));

    if (terrainMove)
        tv += 0.1f * dt.asSeconds();

    /*if (exposure >= 10.0f)
        exposureVar = -1.0f;
    else if (exposureVar <= 0.0f)
        exposureVar = 1.0f;

    exposure += exposureVar * dt.asSeconds();*/
}
Пример #11
0
/*
 * All dimensions are in SI units and relative
 */
void traject_delta_on_all_axes( traject5D* traject)
{
  static unsigned long int serno = 0;
  static time_t t0;
  if (traject == NULL) {
    return;
  }
  if (serno++ == 0) {
    time( &t0);
  }
  if (DEBUG_TRAJECT && (debug_flags & DEBUG_TRAJECT)) {
    printf( "\nMOVE[ #%lu %ds] traject_delta_on_all_axes( traject( %0.9lf, %1.9lf, %1.9lf, %1.9lf, F=%u) [m])\n",
	    serno, (int)time( NULL)-(int)t0,
	    traject->dx, traject->dy, traject->dz, traject->de, traject->feed);
  }

  double dx = traject->dx;
  double dy = traject->dy;
  double dz = traject->dz;
  double de = traject->de;

  int reverse_x = 0;
  if (dx < 0.0) {
    dx = -dx;
    reverse_x = 1;
  }
  int reverse_y = 0;
  if (dy < 0.0) {
    dy = -dy;
    reverse_y = 1;
  }
  int reverse_z = 0;
  if (dz < 0.0) {
    dz = -dz;
    reverse_z = 1;
  }
  int reverse_e = 0;
  if (de < 0.0) {
    de = -de;
    reverse_e = 1;
  }
  // We're only moving in 3D space, e-axis isn't part of this!
  double distance = sqrt( dx * dx + dy * dy + dz * dz);
  if (distance < 2.0E-9) {
    if (de == 0.0) {
      if (DEBUG_TRAJECT && (debug_flags & DEBUG_TRAJECT)) {
        printf( "*** Null move, distance = %1.9lf\n", distance);
      }
      return;	// TODO: will this suffice ?
    }
    // If E is only moving axis, set distance from E
    distance = de;
  }
 /*
  * Travel distance and requested velocity are now known.
  * Determine the velocities for the individual axes
  * using the distances and total duration of the move.
  * If a calculated velocity is higher than the maximum
  * allowed, slow down the entire move.
  */
  double recipr_dt = traject->feed / ( 60000.0 * distance);	/* [m/s] / [m] */
  if (DEBUG_TRAJECT && (debug_flags & DEBUG_TRAJECT)) {
    printf( "Request: total distance = %1.6lf [mm], vector velocity = %1.3lf [mm/s] => est. time = %1.3lf [ms]\n",
	    SI2MM( distance), SI2MS( traject->feed / 60000.0), SI2MS( RECIPR( recipr_dt)));
  }
  int v_change = 0;
  double vx = dx * recipr_dt;
  if (vx > vx_max) {	  // clip feed !
    if (DEBUG_TRAJECT && (debug_flags & DEBUG_TRAJECT)) {
      printf( "*** clipping vx (%1.6lf) to vx_max (%1.6lf)\n", vx, vx_max);
    }
    recipr_dt = vx_max / dx;
    v_change = 1;
  }
  double vy = dy * recipr_dt;
  if (vy > vy_max) {	  // clip feed !
    if (DEBUG_TRAJECT && (debug_flags & DEBUG_TRAJECT)) {
      printf( "*** clipping vy (%1.6lf) to vy_max (%1.6lf)\n", vy, vy_max);
    }
    recipr_dt = vy_max / dy;
    v_change = 1;
  }
  double vz = dz * recipr_dt;
  if (vz > vz_max) {	  // clip feed !
    if (DEBUG_TRAJECT && (debug_flags & DEBUG_TRAJECT)) {
      printf( "*** clipping vz (%1.6lf) to vz_max (%1.6lf)\n", vz, vz_max);
    }
    recipr_dt = vz_max / dz;
    v_change = 1;
  }
  double ve = de * recipr_dt;
  if (ve > ve_max) {	  // clip feed !
    if (DEBUG_TRAJECT && (debug_flags & DEBUG_TRAJECT)) {
      printf( "*** clipping ve (%1.6lf) to ve_max (%1.6lf)\n", ve, ve_max);
    }
    recipr_dt = ve_max / de;
    v_change = 1;
  }
 /*
  * If one or more velocity were limited by its maximum,
  * some of the other values may be incorrect. Recalculate all.
  */
  if (v_change) {
    if (DEBUG_TRAJECT && (debug_flags & DEBUG_TRAJECT)) {
      printf( "Velocity changed to %1.3lf [mm/s] and duration to %1.3lf [ms] due to this clipping\n",
	      SI2MM( distance * recipr_dt), SI2MS( RECIPR( recipr_dt)));
    }
    vx = dx * recipr_dt;
    vy = dy * recipr_dt;
    vz = dz * recipr_dt;
    ve = de * recipr_dt;
  }
  if (DEBUG_TRAJECT && (debug_flags & DEBUG_TRAJECT)) {
    printf( "Velocities - X: %1.3lf, Y: %1.3lf, Z %1.3lf, E: %1.3lf [mm/s]\n",
	    SI2MM( vx), SI2MM( vy), SI2MM( vz), SI2MM( ve));
  }
 /*
  * For a neat linear move, all ramps must start and end at the same moment
  * and have constant (or synchronized) accelation.
  * Now that the targeted velocity is now known for each axis, determine
  * how long it takes for that axis to reach its target speed using maximum
  * acceleration. The slowest axis then scales the acceleration used for all axes.
  */
  double tx_acc = vx * recipr_a_max_x;
  double ty_acc = vy * recipr_a_max_y;
  double tz_acc = vz * recipr_a_max_z;
  double te_acc = ve * recipr_a_max_e;
 /*
  * determine the largest period and scale the acceleration for all axes.
  */
  double t_acc = fmax( fmax( tx_acc, ty_acc), fmax( tz_acc, te_acc));
  if (DEBUG_TRAJECT && (debug_flags & DEBUG_TRAJECT)) {
    printf( "Time needed to reach velocity: X= %1.3lf, Y= %1.3lf, Z= %1.3lf, E= %1.3lf => MAX= %1.3lf [ms]\n",
	   SI2MS( tx_acc), SI2MS( ty_acc), SI2MS( tz_acc), SI2MS( te_acc), SI2MS( t_acc));
  }
  double recipr_t_acc = 1.0 / t_acc;
  double ax = vx * recipr_t_acc;
  double ay = vy * recipr_t_acc;
  double az = vz * recipr_t_acc;
  double ae = ve * recipr_t_acc;
  if (DEBUG_TRAJECT && (debug_flags & DEBUG_TRAJECT)) {
    printf( "Synchronized acceleration constants: X= %1.3lf, Y= %1.3lf, Z= %1.3lf, E= %1.3lf [m/s^2]\n", 
	    ax, ay, az, ae);
  }
 /*
  *  Length of acceleration/deceleration traject:
  *    s = v^2/2a or s = a.t^2/2
  */
  double t_square  = t_acc * t_acc;
  double double_sx = ax * t_square;
  double double_sy = ay * t_square;
  double double_sz = az * t_square;
  double double_se = ae * t_square;
  if (DEBUG_TRAJECT && (debug_flags & DEBUG_TRAJECT)) {
    printf( "Distance to reach full speed: X= %1.6lf Y= %1.6lf Z= %1.6lf E= %1.6lf [mm]\n",
	    SI2MM( 0.5 * double_sx), SI2MM( 0.5 * double_sy), SI2MM( 0.5 * double_sz), SI2MM( 0.5 * double_se));
  }
  double ramp_dx, ramp_dy, ramp_dz, ramp_de;
  double dwell_dx, dwell_dy, dwell_dz, dwell_de;

  uint32_t c0x, c0y, c0z, c0e;
  uint32_t cminx, cminy, cminz, cmine;
  double recipr_t_move = 0.0;	// means: not set
 /*
  * Calculate the timing for all axes
  */
  AXIS_CALC( x);
  AXIS_CALC( y);
  AXIS_CALC( z);
  AXIS_CALC( e);
 /*
  * Put the sign back into the deltas
  */
  if (reverse_x) {
    ramp_dx = -ramp_dx;
    dwell_dx = -dwell_dx;
  }
  if (reverse_y) {
    ramp_dy = -ramp_dy;
    dwell_dy = -dwell_dy;
  }
  if (reverse_z) {
    ramp_dz = -ramp_dz;
    dwell_dz = -dwell_dz;
  }
  if (reverse_e) {
    ramp_de = -ramp_de;
    dwell_de = -dwell_de;
  }
  if (DEBUG_TRAJECT && (debug_flags & DEBUG_TRAJECT)) {
    printf( "Ramps: X= %1.6lf, Y= %1.6lf, Z= %1.6lf, E= %1.6lf [mm], ramp duration= %1.3lf [ms]\n",
	    SI2MM( ramp_dx), SI2MM( ramp_dy), SI2MM( ramp_dz), SI2MM( ramp_de), SI2MS( RECIPR( recipr_t_acc)));
  }

 /*
  * Up from version v3.0 of the stepper firmware, the stepper driver does acceleration
  * and deceleration timing and switching all by itself. Only one command needs to be
  * queued to accelerate from zero speed to max speed, dwell at max speed and decelerate
  * back to zero speed.
  */
  int any_move = 0;

  any_move += QUEUE_MOVE( x);
  any_move += QUEUE_MOVE( y);
  any_move += QUEUE_MOVE( z);
  any_move += QUEUE_MOVE( e);

  if (any_move) {
    pruss_queue_execute();
    any_move = 0;
  }
  if (ramp_dx != 0.0) {
    pruss_queue_adjust_for_ramp( 1, (int32_t)(1.0E9 * ramp_dx));
  }
  if (ramp_dy != 0.0) {
    pruss_queue_adjust_for_ramp( 2, (int32_t)(1.0E9 * ramp_dy));
  }
  if (ramp_dz != 0.0) {
    pruss_queue_adjust_for_ramp( 3, (int32_t)(1.0E9 * ramp_dz));
  }
  if (ramp_de != 0.0) {
    pruss_queue_adjust_for_ramp( 4, (int32_t)(1.0E9 * ramp_de));
  }
  if (config_e_axis_is_always_relative()) {
    pruss_queue_adjust_origin( 4);
  }
}
Пример #12
0
World::World(sf::RenderWindow& inWindow)
: window(inWindow), gbuffer(WINDOW_WIDTH, WINDOW_HEIGHT), hdrDownsampleTexture(WINDOW_WIDTH / 2, WINDOW_HEIGHT / 2, true), hdrBlurTexture(WINDOW_WIDTH / 2, WINDOW_HEIGHT / 2, true), hdrDownsample2Texture(WINDOW_WIDTH / 4, WINDOW_HEIGHT / 4, true), hdrBlur2Texture(WINDOW_WIDTH / 4, WINDOW_HEIGHT / 4, true), fullscreenQuad(0, 0, WINDOW_WIDTH, WINDOW_HEIGHT), aaTexture(WINDOW_WIDTH, WINDOW_HEIGHT)
{
    // OpenGL State
    glBlendEquation(GL_FUNC_ADD);
    glBlendFunc(GL_ONE, GL_ONE);
    glStencilOpSeparate(GL_BACK, GL_KEEP, GL_INCR_WRAP, GL_KEEP);
    glStencilOpSeparate(GL_FRONT, GL_KEEP, GL_DECR_WRAP, GL_KEEP);

    // Seed the PRNG.
    srand(static_cast<unsigned int>(time(0)));

    // Load light meshes.
    pointLightMesh.LoadFromFile("Models/pointlightMesh.obj");
    spotLightMesh.LoadFromFile("Models/spotlightMesh.obj");
    directionalLightMesh.LoadFromFile("Models/directionalMesh.obj");

    // Lighting Shaders.
    pointLightShader.LoadFromFile("Shaders/lightShader.vert", "Shaders/pointlightShader.frag");
    spotLightShader.LoadFromFile("Shaders/lightShader.vert", "Shaders/spotlightShader.frag");
    directionalLightShader.LoadFromFile("Shaders/lightShader.vert", "Shaders/directionalShader.frag");

    // HDR Shaders.
    hdrDownsample.LoadFromFile("Shaders/fsquad.vert", "Shaders/hdrDownsample.frag");
    hdrTonemap.LoadFromFile("Shaders/fsquad.vert", "Shaders/hdrTonemap.frag");
    hdrBlur.LoadFromFile("Shaders/fsquad.vert", "Shaders/hdrBlur.frag");
    hdrBrightness.LoadFromFile("Shaders/fsquad.vert", "Shaders/hdrBrightness.frag");

    // Load Models.
    mdl2.LoadFromFile("Models/sphere.obj");
    mdl.LoadFromFile("Models/floor.obj");
    mdl3.LoadFromFile("Models/sphere.obj");
    quadMesh.LoadFromFile("Models/quad.obj");

    // Load Shaders
    basicShader.LoadFromFile("Shaders/basic.vert", "Shaders/basic.frag");
    pass1Shader.LoadFromFile("Shaders/drpass1.vert", "Shaders/drpass1.frag");
    pass2Shader.LoadFromFile("Shaders/drpass2.vert", "Shaders/drpass2.frag");
    pass2ShaderDirectional.LoadFromFile("Shaders/empty.vert", "Shaders/drpass2directional.frag");
    quadShader.LoadFromFile("Shaders/fsquad.vert", "Shaders/hdrTonemap.frag");
    terrainShader.LoadFromFile("Shaders/terrainshader.vert", "Shaders/terrainshader.frag");
    flatShader.LoadFromFile("Shaders/basic.vert", "Shaders/flatshade.frag");
    emptyShader.LoadFromFile("Shaders/empty.vert", "Shaders/empty.frag");
    sphereShader.LoadFromFile("Shaders/sphere.vert", "Shaders/sphere.frag");

    // Anti-Aliasing Shaders
#if USE_FXAA == 1
    fxaaShader.LoadFromFile("Shaders/fxaa.vert", "Shaders/fxaa.frag");
#endif
    baseAAShader.LoadFromFile("Shaders/baseaa.vert", "Shaders/baseaa.frag");

    // Load textures.
    mdlTex.loadFromFile("Textures/test.png");
    terrainTexture.loadFromFile("Textures/terraintex.png");
    terrainWater.loadFromFile("Textures/terrainwater.png");

    // Main world directional light.
    DirectionalLight worldLight;
    worldLight.SetDirection(Vector3(-1.0f, 0.0f, 0.0f));
    worldLight.SetColor(Color(0.8f, 0.8f, 0.8f));
    worldLight.SetDiffuseIntensity(0.9f);
    worldLight.SetAmbientIntensity(0.4f);
    directionalLights.push_back(worldLight);

    // Lights
    const unsigned int numLights = 40;
    const float lightRadius = 2.0f;
    PointLight l;
    l.SetPosition(Vector3(10.0f, 2.0f, 10.0f));
    l.SetScale(Vector3(3.0f, 3.0f, 3.0f));
    l.SetColor(Color(0.0f, 0.0f, 1.4f, 1.0f));
    l.SetAttenuationConstant(0.4f);
    l.SetDiffuseIntensity(0.4f);
    l.SetAmbientIntensity(0.2f);
    pointLights.push_back(l);

    const int maxXZ = 15;
    const int minXZ = -15;
    const int maxY = 5;
    const float minRad = 0.3f;
    const float maxRad = 2.0f;
    for (int i = 0; i < numLights; i++)
    {
        PointLight l;
        
        // Generate position.
        int x = (rand() % (maxXZ - minXZ)) + minXZ;
        int y = (rand() % maxY);
        int z = (rand() % (maxXZ - minXZ)) + minXZ;
        l.SetPosition(Vector3(x, y, z));

        // Generate Scale
        float frand = ((float)rand() / (float)RAND_MAX);
        float fdiff = maxRad - minRad;

        //l.SetScale(Vector3(scale, scale, scale));

        // Generate Color
        float r = float_rand(0.0f, maxCol);
        float g = float_rand(0.0f, maxCol);
        float b = float_rand(0.0f, maxCol);
        l.SetColor(Color(r, g, b, 1.0f));

        float ambient = ((float)rand() / (float)RAND_MAX);
        float diff = ((float)rand() / (float)RAND_MAX);

        l.SetAmbientIntensity(ambient);
        l.SetDiffuseIntensity(diff);

        float catt = ((float)rand() / (float)RAND_MAX);
        float latt = ((float)rand() / (float)RAND_MAX);
        float eatt = ((float)rand() / (float)RAND_MAX);

        float mc = fmax(fmax((float)r, (float)g), (float)b);
        float ret = (-latt + sqrtf(latt * latt - 4 * eatt * (eatt - 256 * mc * diff))) / 2 * eatt;

        l.SetScale(Vector3(ret, ret, ret));

        l.SetAttenuationConstant(catt);
        l.SetAttenuationLinear(latt);
        l.SetAttenuationExponential(eatt);

        pointLights.push_back(l);
    }

    // Set the initial position of the sphere
    spherePos = Vector3(10.0f, 3.0f, 10.0f);

    // Setup the help text.
    defaultFont.loadFromFile("arial.ttf");
    helpText.setFont(defaultFont);
    helpText.setString("Controls:\n" \
                       "Mouse: Camera direction.\n" \
                       "WASD: Camera movement.\n" \
                       "I: Randomize light attenuations.\n" \
                       "K: Randomize light color and intensity.\n" \
                       "Insert: Disable mouse.\n" \
                       "Delete: Switch shown render target.\n" \
                       "Home: Disable/Enable terrain movement.\n"
                       );
    helpText.setCharacterSize(14);
    helpText.setColor(sf::Color(255, 0, 255, 255));

    targetText.setFont(defaultFont);
    targetText.setCharacterSize(14);
    targetText.setPosition(sf::Vector2f(0.0, window.getSize().y - 25));
    targetText.setString("Current Buffer: Final Output");
    targetText.setColor(sf::Color(255, 0, 255, 255));

    // Sounds
    sndExplodeBuffer.loadFromFile("explode.wav");
    sndExplode.setBuffer(sndExplodeBuffer);
    sndExplode.setAttenuation(10.0f);
    sndExplode.setMinDistance(60.0f);
    sndExplode.setPosition(sf::Vector3f(0.0f, 1.0f, 0.0f));
}
Пример #13
0
TwoDoubles f9(double* x) {
    /* Rosenbrock, rotated*/
    int i, j, rseed; /*Loop over dim*/
    static unsigned int funcId = 9;
    double scales, tmp, Fadd, Fval, Ftrue = 0.;
    TwoDoubles res;

    if (!isInitDone)
    {
        rseed = funcId + 10000 * trialid;
        /*INITIALIZATION*/
        Fopt = computeFopt(funcId, trialid);
        /* computeXopt(rseed, DIM);*/
        computeRotation(rotation, rseed, DIM);
        scales = fmax(1., sqrt(DIM) / 8.);
        for (i = 0; i < DIM; i ++)
        {
            for (j = 0; j < DIM; j++)
                linearTF[i][j] = scales * rotation[i][j];
        }
        // Jakob Bossek
        // This section is hidden by comments in the original implementation,
        // but it is reactivated here for smoof.
        for (i = 0; i < DIM; i++)
        {
              Xopt[i] = 0.;
              for (j = 0; j < DIM; j++)
              {
                Xopt[i] += linearTF[j][i] * 0.5/scales/scales;
                //computed only if Xopt is returned which is not the case at this point.
              }
        }
        isInitDone = 1;
    }
    Fadd = Fopt;

    /* BOUNDARY HANDLING*/

    /* TRANSFORMATION IN SEARCH SPACE*/
    for (i = 0; i < DIM; i++) {
        tmx[i] = 0.5;
        for (j = 0; j < DIM; j++) {
            tmx[i] += linearTF[i][j] * x[j];
        }
    }

    /* COMPUTATION core*/
    for (i = 0; i < DIM - 1; i++)
    {
        tmp = (tmx[i] * tmx[i] - tmx[i+1]);
        Ftrue += tmp * tmp;
    }
    Ftrue *= 1e2;
    for (i = 0; i < DIM - 1; i ++)
    {
       tmp = (tmx[i] - 1.);
        Ftrue += tmp * tmp;
    }

    Ftrue += Fadd;
    Fval = Ftrue; /* without noise*/

    res.Fval = Fval;
    res.Ftrue = Ftrue;
    return res;
}
Пример #14
0
TwoDoubles f7(double* x) {
    /* step-ellipsoid, condition 100*/

    int i, j, rseed; /*Loop over dim*/
    static unsigned int funcId = 7;
    static double condition = 100.;
    static double alpha = 10.;
    double x1, tmp, Fadd, Fval, Fpen = 0., Ftrue = 0.;
    TwoDoubles res;

    if (!isInitDone)
    {
        rseed = funcId + 10000 * trialid;
        /*INITIALIZATION*/
        Fopt = computeFopt(funcId, trialid);
        computeXopt(rseed, DIM);
        computeRotation(rotation, rseed + 1000000, DIM);
        computeRotation(rot2, rseed, DIM);
        isInitDone = 1;
    }
    Fadd = Fopt;

    /* BOUNDARY HANDLING*/
    for (i = 0; i < DIM; i++)
    {
        tmp = fabs(x[i]) - 5.;
        if (tmp > 0.)
        {
            Fpen += tmp * tmp;
        }
    }
    Fadd += Fpen;

    /* TRANSFORMATION IN SEARCH SPACE*/
    for (i = 0; i < DIM; i++) {

        tmpvect[i] = 0.;
        tmp = sqrt(pow(condition/10., ((double)i)/((double)(DIM-1))));
        for (j = 0; j < DIM; j++) {
            tmpvect[i] += tmp * rot2[i][j] * (x[j] - Xopt[j]);
        }

    }
    x1 = tmpvect[0];

    for (i = 0; i < DIM; i++) {
        if (fabs(tmpvect[i]) > 0.5)
            tmpvect[i] = round(tmpvect[i]);
        else
            tmpvect[i] = round(alpha * tmpvect[i])/alpha;
    }

    for (i = 0; i < DIM; i++) {
        tmx[i] = 0.;
        for (j = 0; j < DIM; j++) {
            tmx[i] += rotation[i][j] * tmpvect[j];
        }
    }

    /* COMPUTATION core*/
    for (i = 0; i < DIM; i++)
    {
        Ftrue += pow(condition, ((double)i)/((double)(DIM-1))) * tmx[i] * tmx[i];
    }
    Ftrue = 0.1 * fmax(1e-4 * fabs(x1), Ftrue);

    Ftrue += Fadd;
    Fval = Ftrue; /* without noise*/

    res.Fval = Fval;
    res.Ftrue = Ftrue;
    return res;
}
Пример #15
0
/*! \fn solve non-linear system with hybrd method
 *
 *  \param [in]  [data]
 *               [sysNumber] index of the corresponing non-linear system
 *
 *  \author wbraun
 */
int solveHybrd(DATA *data, int sysNumber)
{
  NONLINEAR_SYSTEM_DATA* systemData = &(data->simulationInfo.nonlinearSystemData[sysNumber]);
  DATA_HYBRD* solverData = (DATA_HYBRD*)systemData->solverData;
  /*
   * We are given the number of the non-linear system.
   * We want to look it up among all equations.
   */
  int eqSystemNumber = systemData->equationIndex;
  threadData_t *threadData = data->threadData;

  int i, j;
  integer iflag = 1;
  double xerror, xerror_scaled;
  int success = 0;
  double local_tol = 1e-12;
  double initial_factor = solverData->factor;
  int nfunc_evals = 0;
  int continuous = 1;
  int nonContinuousCase = 0;

  int giveUp = 0;
  int retries = 0;
  int retries2 = 0;
  int retries3 = 0;
  int assertCalled = 0;
  int assertRetries = 0;
  int assertMessage = 0;

  modelica_boolean* relationsPreBackup;

  struct dataAndSys dataAndSysNumber = {data, sysNumber};

  relationsPreBackup = (modelica_boolean*) malloc(data->modelData.nRelations*sizeof(modelica_boolean));

  solverData->numberOfFunctionEvaluations = 0;
  /* debug output */
  if(ACTIVE_STREAM(LOG_NLS_V))
  {
    int indexes[2] = {1,eqSystemNumber};
    infoStreamPrintWithEquationIndexes(LOG_NLS_V, 1, indexes, "start solving non-linear system >>%d<< at time %g", eqSystemNumber, data->localData[0]->timeValue);
    for(i=0; i<solverData->n; i++)
    {
      infoStreamPrint(LOG_NLS_V, 1, "%d. %s = %f", i+1, modelInfoGetEquation(&data->modelData.modelDataXml,eqSystemNumber).vars[i], systemData->nlsx[i]);
      infoStreamPrint(LOG_NLS_V, 0, "    nominal = %f\nold = %f\nextrapolated = %f",
          systemData->nominal[i], systemData->nlsxOld[i], systemData->nlsxExtrapolation[i]);
      messageClose(LOG_NLS_V);
    }
    messageClose(LOG_NLS_V);
  }

  /* set x vector */
  if(data->simulationInfo.discreteCall)
    memcpy(solverData->x, systemData->nlsx, solverData->n*(sizeof(double)));
  else
    memcpy(solverData->x, systemData->nlsxExtrapolation, solverData->n*(sizeof(double)));

  for(i=0; i<solverData->n; i++){
    solverData->xScalefactors[i] = fmax(fabs(solverData->x[i]), systemData->nominal[i]);
  }

  /* start solving loop */
  while(!giveUp && !success)
  {
    for(i=0; i<solverData->n; i++)
      solverData->xScalefactors[i] = fmax(fabs(solverData->x[i]), systemData->nominal[i]);

    /* debug output */
    if(ACTIVE_STREAM(LOG_NLS_V)) {
      printVector(solverData->xScalefactors, &(solverData->n), LOG_NLS_V, "scaling factors x vector");
      printVector(solverData->x, &(solverData->n), LOG_NLS_V, "Iteration variable values");
    }

    /* Scaling x vector */
    if(solverData->useXScaling) {
      for(i=0; i<solverData->n; i++) {
        solverData->x[i] = (1.0/solverData->xScalefactors[i]) * solverData->x[i];
      }
    }

    /* debug output */
    if(ACTIVE_STREAM(LOG_NLS_V))
    {
      printVector(solverData->x, &solverData->n, LOG_NLS_V, "Iteration variable values (scaled)");
    }

    /* set residual function continuous
     */
    if(continuous) {
      ((DATA*)data)->simulationInfo.solveContinuous = 1;
    } else {
      ((DATA*)data)->simulationInfo.solveContinuous = 0;
    }

    giveUp = 1;

    /* try */
    {
      int success = 0;
#ifndef OMC_EMCC
      MMC_TRY_INTERNAL(simulationJumpBuffer)
#endif
      hybrj_(wrapper_fvec_hybrj, &solverData->n, solverData->x,
          solverData->fvec, solverData->fjac, &solverData->ldfjac, &solverData->xtol,
          &solverData->maxfev, solverData->diag, &solverData->mode, &solverData->factor,
          &solverData->nprint, &solverData->info, &solverData->nfev, &solverData->njev, solverData->r__,
          &solverData->lr, solverData->qtf, solverData->wa1, solverData->wa2,
          solverData->wa3, solverData->wa4, (void*) &dataAndSysNumber);

      success = 1;
      if(assertCalled)
      {
        infoStreamPrint(LOG_NLS, 0, "After assertions failed, found a solution for which assertions did not fail.");
        /* re-scaling x vector */
        for(i=0; i<solverData->n; i++){
          if(solverData->useXScaling)
            systemData->nlsxOld[i] = solverData->x[i]*solverData->xScalefactors[i];
          else
            systemData->nlsxOld[i] = solverData->x[i];
        }
      }
      assertRetries = 0;
      assertCalled = 0;
      success = 1;
#ifndef OMC_EMCC
      MMC_CATCH_INTERNAL(simulationJumpBuffer)
#endif
      /* catch */
      if (!success)
      {
        if (!assertMessage)
        {
          if (ACTIVE_WARNING_STREAM(LOG_STDOUT))
          {
            if(data->simulationInfo.initial)
              warningStreamPrint(LOG_STDOUT, 1, "While solving non-linear system an assertion failed during initialization.");
            else
              warningStreamPrint(LOG_STDOUT, 1, "While solving non-linear system an assertion failed at time %g.", data->localData[0]->timeValue);
            warningStreamPrint(LOG_STDOUT, 0, "The non-linear solver tries to solve the problem that could take some time.");
            warningStreamPrint(LOG_STDOUT, 0, "It could help to provide better start-values for the iteration variables.");
            if (!ACTIVE_STREAM(LOG_NLS))
              warningStreamPrint(LOG_STDOUT, 0, "For more information simulate with -lv LOG_NLS");
            messageClose(LOG_STDOUT);
          }
          assertMessage = 1;
        }

        solverData->info = -1;
        xerror_scaled = 1;
        xerror = 1;
        assertCalled = 1;
      }
    }

    /* set residual function continuous */
    if(continuous)
    {
      ((DATA*)data)->simulationInfo.solveContinuous = 0;
    }
    else
    {
      ((DATA*)data)->simulationInfo.solveContinuous = 1;
    }

    /* re-scaling x vector */
    if(solverData->useXScaling)
      for(i=0; i<solverData->n; i++)
        solverData->x[i] = solverData->x[i]*solverData->xScalefactors[i];

    /* check for proper inputs */
    if(solverData->info == 0) {
      printErrorEqSyst(IMPROPER_INPUT, modelInfoGetEquation(&data->modelData.modelDataXml, eqSystemNumber),
          data->localData[0]->timeValue);
    }

    if(solverData->info != -1)
    {
      /* evaluate with discontinuities */
      if(data->simulationInfo.discreteCall){
        int scaling = solverData->useXScaling;
        int success = 0;
        if(scaling)
          solverData->useXScaling = 0;

        ((DATA*)data)->simulationInfo.solveContinuous = 0;

        /* try */
#ifndef OMC_EMCC
        MMC_TRY_INTERNAL(simulationJumpBuffer)
#endif
        wrapper_fvec_hybrj(&solverData->n, solverData->x, solverData->fvec, solverData->fjac, &solverData->ldfjac, &iflag, (void*) &dataAndSysNumber);
        success = 1;
#ifndef OMC_EMCC
        MMC_CATCH_INTERNAL(simulationJumpBuffer)
#endif
        /* catch */
        if (!success)
        {
          warningStreamPrint(LOG_STDOUT, 0, "Non-Linear Solver try to handle a problem with a called assert.");

          solverData->info = -1;
          xerror_scaled = 1;
          xerror = 1;
          assertCalled = 1;
        }

        if(scaling)
          solverData->useXScaling = 1;

        updateRelationsPre(data);
      }
    }

    if(solverData->info != -1)
    {
      /* scaling residual vector */
      {
        int l=0;
        for(i=0; i<solverData->n; i++){
          solverData->resScaling[i] = 1e-16;
          for(j=0; j<solverData->n; j++){
            solverData->resScaling[i] = (fabs(solverData->fjacobian[l]) > solverData->resScaling[i])
                    ? fabs(solverData->fjacobian[l]) : solverData->resScaling[i];
            l++;
          }
          solverData->fvecScaled[i] = solverData->fvec[i] * (1 / solverData->resScaling[i]);
        }
        /* debug output */
        if(ACTIVE_STREAM(LOG_NLS_V))
        {
          infoStreamPrint(LOG_NLS_V, 1, "scaling factors for residual vector");
          for(i=0; i<solverData->n; i++)
          {
            infoStreamPrint(LOG_NLS_V, 1, "scaled residual [%d] : %.20e", i, solverData->fvecScaled[i]);
            infoStreamPrint(LOG_NLS_V, 0, "scaling factor [%d] : %.20e", i, solverData->resScaling[i]);
            messageClose(LOG_NLS_V);
          }
          messageClose(LOG_NLS_V);
        }

        /* debug output */
        if(ACTIVE_STREAM(LOG_NLS_JAC))
        {
          char buffer[4096];

          infoStreamPrint(LOG_NLS_JAC, 1, "jacobian matrix [%dx%d]", (int)solverData->n, (int)solverData->n);
          for(i=0; i<solverData->n; i++)
          {
            buffer[0] = 0;
            for(j=0; j<solverData->n; j++)
              sprintf(buffer, "%s%10g ", buffer, solverData->fjacobian[i*solverData->n+j]);
            infoStreamPrint(LOG_NLS_JAC, 0, "%s", buffer);
          }
          messageClose(LOG_NLS_JAC);
        }

        /* check for error  */
        xerror_scaled = enorm_(&solverData->n, solverData->fvecScaled);
        xerror = enorm_(&solverData->n, solverData->fvec);
      }
    }

    /* reset non-contunuousCase */
    if(nonContinuousCase && xerror > local_tol && xerror_scaled > local_tol)
    {
      memcpy(data->simulationInfo.relationsPre, relationsPreBackup, sizeof(modelica_boolean)*data->modelData.nRelations);
      nonContinuousCase = 0;
    }

    if(solverData->info < 4 && xerror > local_tol && xerror_scaled > local_tol)
      solverData->info = 4;

    /* solution found */
    if(solverData->info == 1 || xerror <= local_tol || xerror_scaled <= local_tol)
    {
      int scaling;

      success = 1;
      nfunc_evals += solverData->nfev;
      if(ACTIVE_STREAM(LOG_NLS))
      {
        int indexes[2] = {1,eqSystemNumber};
        /* output solution */
        infoStreamPrintWithEquationIndexes(LOG_NLS, 1, indexes, "solution for NLS %d at t=%g", eqSystemNumber, data->localData[0]->timeValue);
        for(i=0; i<solverData->n; ++i)
        {
          infoStreamPrint(LOG_NLS, 0, "[%d] %s = %g", i+1, modelInfoGetEquation(&data->modelData.modelDataXml,eqSystemNumber).vars[i],  solverData->x[i]);
        }
        messageClose(LOG_NLS);
      }else if (ACTIVE_STREAM(LOG_NLS_V)){
        infoStreamPrint(LOG_NLS_V, 1, "system solved");
        infoStreamPrint(LOG_NLS_V, 0, "%d retries\n%d restarts", retries, retries2+retries3);
        messageClose(LOG_NLS_V);
        printStatus(data, solverData, eqSystemNumber, &nfunc_evals, &xerror, &xerror_scaled, LOG_NLS_V);
      }
      scaling = solverData->useXScaling;
      if(scaling)
        solverData->useXScaling = 0;

      /* take the solution */
      memcpy(systemData->nlsx, solverData->x, solverData->n*(sizeof(double)));

      /* try */
      {
        int success = 0;
#ifndef OMC_EMCC
        MMC_TRY_INTERNAL(simulationJumpBuffer)
#endif
        wrapper_fvec_hybrj(&solverData->n, solverData->x, solverData->fvec, solverData->fjac, &solverData->ldfjac, &iflag, (void*) &dataAndSysNumber);
        success = 1;
#ifndef OMC_EMCC
        MMC_CATCH_INTERNAL(simulationJumpBuffer)
#endif
        /* catch */
        if (!success) {
          warningStreamPrint(LOG_STDOUT, 0, "Non-Linear Solver try to handle a problem with a called assert.");

          solverData->info = 4;
          xerror_scaled = 1;
          xerror = 1;
          assertCalled = 1;
          success = 0;
          giveUp = 0;
        }
      }
      if(scaling)
        solverData->useXScaling = 1;
    }
Пример #16
0
/* 
 * Refine eigenvalues with respect to new rrr 
 */
static inline 
int refine_eigvals(cluster_t *cl, int rf_begin, int rf_end,
		   int tid, proc_t *procinfo, rrr_t *RRR, 
		   val_t *Wstruct, vec_t *Zstruct,
		   tol_t *tolstruct, counter_t *num_left,
		   workQ_t *workQ, double *work,
		   int *iwork)
{
  /* From inputs */
  int              rf_size   = rf_end-rf_begin+1;
  int              bl_begin  = cl->bl_begin;
  int              bl_end    = cl->bl_end;
  int              bl_size   = bl_end - bl_begin + 1;
  double           bl_spdiam = cl->bl_spdiam;

  int              nthreads  = procinfo->nthreads;

  double *restrict D         = RRR->D;
  double *restrict L         = RRR->L;
  double *restrict DLL       = RRR->DLL;

  double *restrict W         = Wstruct->W;
  double *restrict Werr      = Wstruct->Werr;
  double *restrict Wgap      = Wstruct->Wgap;
  int    *restrict Windex    = Wstruct->Windex;
  double *restrict Wshifted  = Wstruct->Wshifted;

  int              nz        = Zstruct->nz;

  double           pivmin    = tolstruct->pivmin;
  double           rtol1     = tolstruct->rtol1;
  double           rtol2     = tolstruct->rtol2;

  /* Others */
  int              info, i, p, q, offset;
  double           sigma, savegap;
  int              MIN_REFINE_CHUNK = fmax(2,nz/(4*nthreads));
  int              left, own_part, others_part, num_tasks;
  int              ts_begin, ts_end, chunk, count;
  task_t           *task;
  sem_t            sem;
  int              num_iter;

  /* Determine if refinement should be split into tasks */
  left = PMR_get_counter_value(num_left);
  own_part = (int) fmax( ceil( (double) left / nthreads ),
			 MIN_REFINE_CHUNK);

  if (own_part < rf_size) {

    others_part = rf_size - own_part;
    num_tasks   = iceil(rf_size, own_part) - 1; /* >1 */
    chunk       = others_part/num_tasks;        /* floor */

    sem_init(&sem, 0, 0);
    ts_begin = rf_begin;
    p        = Windex[rf_begin];
    for (i=0; i<num_tasks; i++) {
      ts_end = ts_begin + chunk - 1;
      q      = p        + chunk - 1;

      task = PMR_create_r_task(ts_begin, ts_end, D, DLL, p, q, 
			       bl_size, bl_spdiam, tid, &sem);
     
      if (ts_begin <= ts_end)
	PMR_insert_task_at_back(workQ->r_queue, task);
      else
	sem_post(&sem); /* case chunk=0 */

      ts_begin = ts_end + 1;
      p        = q      + 1;
    }
    ts_end = rf_end;
    q      = Windex[rf_end];
    offset = Windex[ts_begin] - 1;

    /* Call bisection routine to refine the values */
    if (ts_begin <= ts_end) {
      LAPACK(dlarrb)
      (&bl_size, D, DLL, &p, &q, &rtol1, &rtol2, &offset, &Wshifted[ts_begin], 
       &Wgap[ts_begin], &Werr[ts_begin], work, iwork, &pivmin, &bl_spdiam, 
       &bl_size, &info);
      assert( info == 0 );
    }

    /* Empty "all" r-queue refine tasks before waiting */
    num_iter = PMR_get_num_tasks(workQ->r_queue);
    for (i=0; i<num_iter; i++) {
      task = PMR_remove_task_at_front(workQ->r_queue);
      if (task != NULL) {
	if (task->flag == REFINE_TASK_FLAG) {
	  PMR_process_r_task((refine_t *) task->data, procinfo, 
			     Wstruct, tolstruct, work, iwork);
	  free(task);
	} else {
	  PMR_insert_task_at_back(workQ->r_queue, task);
	}
      } /* if task */
    } /* end for i */
    
    /* Barrier: wait until all created tasks finished */
    count = num_tasks;
    while (count > 0) {
      while (sem_wait(&sem) != 0) { };
      count--;
    }
    sem_destroy(&sem);

    /* Edit right gap at splitting point */
    ts_begin = rf_begin;
    for (i=0; i<num_tasks; i++) {
      ts_end = ts_begin + chunk - 1;
      
      Wgap[ts_end] = Wshifted[ts_end + 1] - Werr[ts_end + 1]
	             - Wshifted[ts_end] - Werr[ts_end];
      
      ts_begin = ts_end + 1;
    }

  } else {
    /* Refinement of cluster without creating tasks */
    
    /* 'p' and 'q' are local (within block) indices of
     * the first/last eigenvalue of the cluster */
    p = Windex[rf_begin];
    q = Windex[rf_end];
    
    offset = Windex[rf_begin] - 1;    /* = p - 1 */
    
    if (p == q) {
      savegap = Wgap[rf_begin];
      Wgap[rf_begin] = 0.0;
    }  
    
    /* Bisection routine to refine the values */
    LAPACK(dlarrb)
    (&bl_size, D, DLL, &p, &q, &rtol1, &rtol2, &offset, &Wshifted[rf_begin], 
     &Wgap[rf_begin], &Werr[rf_begin], work, iwork, &pivmin, &bl_spdiam, 
     &bl_size, &info);
    assert( info == 0 );
    
    if (p == q) {
      Wgap[rf_begin] = savegap;
    }  
  
  } /* end refine with or without creating tasks */

  sigma     = L[bl_size-1];
  
  /* refined eigenvalues with all shifts applied in W */
  for (i=rf_begin; i<=rf_end; i++) {
    W[i] = Wshifted[i] + sigma;
  }

  return(0);
} /* end refine_eigvals */
Пример #17
0
void _solveFP(const double * abgth,
				const double * phis, const int N_phi,
				const double * ts, const int num_steps, //num_steps is not the best way to call this variable it is actually num_epochs or num_tks
				const double * xs, const int num_nodes,
				double *** Fs) {

//	printf("a,b,g,t = %.2f %.2f %.2f %.2f", abgth[0],abgth[1],abgth[2],abgth[3]);
//	double *** Fs;
//	= new double[N_phi][num_steps][num_nodes];
//	double * contiguousAllocationPtr = malloc(N_phi*num_steps*num_nodes * sizeof(double));
//	Fs = malloc(N_phi * sizeof(double**));

//	for (int phi_idx = 0; phi_idx < N_phi; ++phi_idx) {
//		for (int t_idx = 0; t_idx < num_steps; ++t_idx) {
//			for (int x_idx = 0; x_idx < num_nodes; ++x_idx) {
//				Fs[phi_idx][t_idx][x_idx] = phis[phi_idx]*ts[t_idx]*xs[x_idx];
//			}
//		}
//	}
	//RIP PARAMS:
	double alpha, beta, gamma, theta;
	double dx, dt;
	alpha = abgth[0]; beta= abgth[1]; gamma =abgth[2]; theta = abgth[3];
	dt = ts[1]-ts[0]; //or pass explicitly??? (nein!)
	dx = xs[1]-xs[0]; //or pass explicitly??? (nein!)

	printf("FP: alpha=%.2f, beta=%.2f,gamma=%.2f,theta=%.2f\n",
			alpha, beta, gamma, theta);

	double D =  beta * beta / 2.; //the diffusion coeff
	double dx_sqrd = dx * dx;

	//Set BCs, ICs:
	setICs(Fs, phis, N_phi, xs, num_nodes);
	setBCs(Fs, phis, N_phi, ts, num_steps);

//  e = ones(self._num_nodes() - 1);
//	d_on = D * dt / dx_sqrd;
//	centre_diag = e + d_on;
//	centre_diag[-1] = 1.0;
//	M.setdiag(centre_diag)

	double t_prev, t_next;
	double * U_prev; double * U_next;	double * F_prev; double * F_next;
	double * L_prev; double * RHS;
	int num_active_nodes = num_nodes -1; //Recall that the first node (F[0] is identically zero!!!) only from x_k = 2...N_x are they active
	//TODO: create an array loop for the following:
	U_prev = (double *)malloc(num_active_nodes*sizeof(double));
	U_next = (double *)malloc(num_active_nodes*sizeof(double));
	F_prev = (double *)malloc(num_nodes*sizeof(double)); //!!!
	F_next = (double *)malloc(num_active_nodes*sizeof(double));
	L_prev = (double *)malloc(num_active_nodes*sizeof(double));
	RHS    = (double *)malloc(num_active_nodes*sizeof(double));

    //AllOCATE MASS MTX (diagonals):
	double (* M_l), (*M_c), (*M_u); //lower, central and upper diagonal respectively
	M_l = (double *)malloc(num_active_nodes*sizeof(double));
	M_c = (double *)malloc(num_active_nodes*sizeof(double));
	M_u = (double *)malloc(num_active_nodes*sizeof(double));

	double max_F_next = .0;
	double phi, x_cur; //Uninitialized!
	for (int tk = 1; tk < num_steps; ++tk) {
           t_prev = ts[tk-1];
           t_next = ts[tk];
           for (int phi_idx = 0; phi_idx < N_phi; ++phi_idx) {
        	   phi = phis[phi_idx];

        	   //reload the variables for this time step:
        	   //Rip the previous time solution:
        	   for (int x_idx = 0; x_idx < num_nodes; ++x_idx)
        		   F_prev[x_idx] = Fs[phi_idx][tk-1][x_idx];

			   //Form the advection coefficient:
			   for (int x_idx = 0; x_idx < num_active_nodes; ++x_idx) {
        		   x_cur = xs[x_idx+1];
				   U_prev[x_idx] = -(alpha - x_cur + gamma * sin(theta * (t_prev + phi) ));
				   U_next[x_idx] = -(alpha - x_cur + gamma * sin(theta * (t_next + phi) ));
        	   }

			   //Form operator term at the previous step:
        	   for (int x_idx = 0; x_idx < num_active_nodes-1; ++x_idx) {
        		   L_prev[x_idx] =  U_prev[x_idx] * (F_prev[x_idx+2] - F_prev[x_idx]) / 2. / dx 			//advection
                                  + D * (F_prev[x_idx+2] -  2*F_prev[x_idx+1] + F_prev[x_idx]) / dx_sqrd;   //diffusion

        	   }
        	   //Specifically handle right side conditions:
        	   L_prev[num_active_nodes-1] =   U_prev[num_active_nodes-1]*(F_prev[num_nodes-1] - F_prev[num_nodes-2]) / dx
        			   	   	   	   	   	  + D * (-F_prev[num_nodes-1] + F_prev[num_nodes-2]) / dx_sqrd; //the form of the diffusion term comes from extrapolating the zero Neumann BCs:

        	   //Finally form the RHS:
        	   for (int x_idx = 0; x_idx < num_active_nodes-1; ++x_idx)
        		   RHS[x_idx]= F_prev[x_idx+1] + .5 * dt * L_prev[x_idx];

        	   //and impose the right BCs:
               RHS[num_active_nodes-1] = 0.;

               //Set the MASS MTX:
               //Main diagonal: Carefully read the Thomas Algo index specs!!!
               double d_on  = -D * dt / dx_sqrd;
               double d_off = .5 * D * dt / dx_sqrd;
               double u_factor = .5 * dt / dx / 2.;

               for (int x_idx = 0; x_idx < num_active_nodes-1; ++x_idx) {
            	   //Central diagonal
               		M_c[x_idx] = 1.0 - d_on;
               	   //Lower diagonal: Carefully read the THomas Algo index specs (it only uses the values from M_l[1] onwards !!!
               	   M_l[x_idx+1] = -(d_off - u_factor * U_next[x_idx+1]);
               	   //Upper diagonal: Carefully read the THomas Algo index specs!!!
				   M_u[x_idx]   = -(d_off + u_factor * U_next[x_idx]);
               }
//			  Set the Neumann BCs:
			  M_l[num_active_nodes-1] = -1.0; //
			  M_c[num_active_nodes-1] = 1.0;


               //Thomas Solve it:
               thomasSolve(num_active_nodes, M_l, M_c, M_u, RHS, F_next);

               //Store solution:
               for (int x_idx = 1; x_idx < num_nodes; ++x_idx) {
            	   double lF = F_next[x_idx-1];
            	   max_F_next = fmax(max_F_next, lF);
            	   Fs[phi_idx][tk][x_idx] = lF;
               }
		}//end per-phi loop
//      TODO: Break out of time-loop?
//		if max_F_next< 1e-4:
//			break // from time-loop (remember to set the rest of F to zero!!!
	}//end time loop
}//_solveFP(...)
Пример #18
0
static inline 
int communicate_refined_eigvals(cluster_t *cl, proc_t *procinfo,
				int tid, val_t *Wstruct, rrr_t *RRR)
{
  /* From inputs */
  int              cl_begin     = cl->begin;
  int              cl_end       = cl->end;
  int              bl_begin     = cl->bl_begin;
  int              bl_end       = cl->bl_end;
  int              proc_W_begin = cl->proc_W_begin;
  int              proc_W_end   = cl->proc_W_end;
  int              left_pid     = cl->left_pid;
  int              right_pid    = cl->right_pid;
  int              num_messages = 4*(right_pid - left_pid);

  int              pid          = procinfo->pid;

  double *restrict W            = Wstruct->W;
  double *restrict Werr         = Wstruct->Werr;
  double *restrict Wgap         = Wstruct->Wgap;
  double *restrict Wshifted     = Wstruct->Wshifted;
  int    *restrict iproc        = Wstruct->iproc;

  /* Others */
  int              p, i_msg, u, k;
  int              my_begin, my_end, my_size;
  int              other_begin, other_end, other_size;
  double           sigma;
  int              status, communication_done;
  MPI_Request      *requests;
  MPI_Status       *stats;
  comm_t           *comm;
  bool             proc_involved;

  my_begin = imax(cl_begin, proc_W_begin);
  my_end   = imin(cl_end,   proc_W_end);
  if (pid == left_pid ) my_begin = cl_begin;
  if (pid == right_pid) my_end   = cl_end;
  my_size  = my_end - my_begin + 1;

  requests = (MPI_Request *) malloc( num_messages *
					  sizeof(MPI_Request) );
  stats    = (MPI_Status  *) malloc( num_messages * 
					  sizeof(MPI_Status) );

  i_msg = 0;
  for (p=left_pid; p<=right_pid; p++) {

    proc_involved = false;
    for (k=cl_begin; k<=cl_end; k++) {
      if (iproc[k] == p) {
	proc_involved = true;
	break;
      }
    }

    if (p != pid && proc_involved == true) {

      /* send message to process p (non-blocking) */
      MPI_Isend(&Wshifted[my_begin], my_size, MPI_DOUBLE, p,
		my_begin, procinfo->comm, &requests[4*i_msg]);

      MPI_Isend(&Werr[my_begin], my_size, MPI_DOUBLE, p,
		my_begin, procinfo->comm, &requests[4*i_msg+1]);

      /* Find eigenvalues in of process p */
      other_size = 0;
      for (k=cl_begin; k<=cl_end; k++) {
	if (other_size == 0 && iproc[k] == p) {
	  other_begin = k;
	  other_end   = k;
	  other_size++;
	  u = k+1;
	  while (u <=cl_end && iproc[u] == p) {
	    other_end++;
	    other_size++;
	    u++;
	  }
	}
      }
      if (p == left_pid) {
	other_begin = cl_begin;
	u = cl_begin;
	while (iproc[u] == -1) {
	  other_size++;
	  u++;
	}
      }
      if (p == right_pid) {
	other_end = cl_end;
	u = cl_end;
	while (iproc[u] == -1) {
	  other_size++;
	  u--;
	}
      }

      /* receive message from process p (non-blocking) */
      MPI_Irecv(&Wshifted[other_begin], other_size, MPI_DOUBLE,	p,
		other_begin, procinfo->comm, &requests[4*i_msg+2]);

      MPI_Irecv(&Werr[other_begin], other_size, MPI_DOUBLE, p,
		other_begin, procinfo->comm, &requests[4*i_msg+3]);
     
      i_msg++;
    }

  } /* end for p */
  num_messages = 4*i_msg; /* messages actually send */

  status = MPI_Testall(num_messages, requests, 
		       &communication_done, stats);
  assert(status == MPI_SUCCESS);

  if (communication_done == true) {

    sigma     = RRR->L[bl_end-bl_begin];
    for (k=cl_begin; k<cl_end; k++) {
      W[k]    = Wshifted[k] + sigma;
      Wgap[k] = fmax(0, Wshifted[k+1]-Werr[k+1] 
                        - (Wshifted[k]-Werr[k]));
    }
    W[cl_end] = Wshifted[cl_end] + sigma;

    free(requests);
    free(stats);
    status = COMM_COMPLETE;
  } else {
   
    comm = (comm_t *) malloc( sizeof(comm_t) );
    assert(comm != NULL);

    comm->num_messages      = num_messages;
    comm->requests          = requests;
    comm->stats             = stats;
    cl->wait_until_refined  = true;
    cl->messages            = comm;
    
    status = COMM_INCOMPLETE;
  }
  
  return(status);
} /* end communicate_refined_eigvals */
Пример #19
0
static int ransac(const int *matched_points, int npoints,
                  int *num_inliers_by_motion, double *params_by_motion,
                  int num_desired_motions, const int minpts,
                  IsDegenerateFunc is_degenerate,
                  FindTransformationFunc find_transformation,
                  ProjectPointsDoubleFunc projectpoints) {
  static const double PROBABILITY_REQUIRED = 0.9;
  static const double EPS = 1e-12;

  int N = 10000, trial_count = 0;
  int i = 0;
  int ret_val = 0;

  unsigned int seed = (unsigned int)npoints;

  int indices[MAX_MINPTS] = { 0 };

  double *points1, *points2;
  double *corners1, *corners2;
  double *image1_coord;

  // Store information for the num_desired_motions best transformations found
  // and the worst motion among them, as well as the motion currently under
  // consideration.
  RANSAC_MOTION *motions, *worst_kept_motion = NULL;
  RANSAC_MOTION current_motion;

  // Store the parameters and the indices of the inlier points for the motion
  // currently under consideration.
  double params_this_motion[MAX_PARAMDIM];

  double *cnp1, *cnp2;

  if (npoints < minpts * MINPTS_MULTIPLIER || npoints == 0) {
    return 1;
  }

  points1 = (double *)aom_malloc(sizeof(*points1) * npoints * 2);
  points2 = (double *)aom_malloc(sizeof(*points2) * npoints * 2);
  corners1 = (double *)aom_malloc(sizeof(*corners1) * npoints * 2);
  corners2 = (double *)aom_malloc(sizeof(*corners2) * npoints * 2);
  image1_coord = (double *)aom_malloc(sizeof(*image1_coord) * npoints * 2);

  motions =
      (RANSAC_MOTION *)aom_malloc(sizeof(RANSAC_MOTION) * num_desired_motions);
  for (i = 0; i < num_desired_motions; ++i) {
    motions[i].inlier_indices =
        (int *)aom_malloc(sizeof(*motions->inlier_indices) * npoints);
    clear_motion(motions + i, npoints);
  }
  current_motion.inlier_indices =
      (int *)aom_malloc(sizeof(*current_motion.inlier_indices) * npoints);
  clear_motion(&current_motion, npoints);

  worst_kept_motion = motions;

  if (!(points1 && points2 && corners1 && corners2 && image1_coord && motions &&
        current_motion.inlier_indices)) {
    ret_val = 1;
    goto finish_ransac;
  }

  cnp1 = corners1;
  cnp2 = corners2;
  for (i = 0; i < npoints; ++i) {
    *(cnp1++) = *(matched_points++);
    *(cnp1++) = *(matched_points++);
    *(cnp2++) = *(matched_points++);
    *(cnp2++) = *(matched_points++);
  }

  while (N > trial_count) {
    double sum_distance = 0.0;
    double sum_distance_squared = 0.0;

    clear_motion(&current_motion, npoints);

    int degenerate = 1;
    int num_degenerate_iter = 0;

    while (degenerate) {
      num_degenerate_iter++;
      if (!get_rand_indices(npoints, minpts, indices, &seed)) {
        ret_val = 1;
        goto finish_ransac;
      }

      copy_points_at_indices(points1, corners1, indices, minpts);
      copy_points_at_indices(points2, corners2, indices, minpts);

      degenerate = is_degenerate(points1);
      if (num_degenerate_iter > MAX_DEGENERATE_ITER) {
        ret_val = 1;
        goto finish_ransac;
      }
    }

    if (find_transformation(minpts, points1, points2, params_this_motion)) {
      trial_count++;
      continue;
    }

    projectpoints(params_this_motion, corners1, image1_coord, npoints, 2, 2);

    for (i = 0; i < npoints; ++i) {
      double dx = image1_coord[i * 2] - corners2[i * 2];
      double dy = image1_coord[i * 2 + 1] - corners2[i * 2 + 1];
      double distance = sqrt(dx * dx + dy * dy);

      if (distance < INLIER_THRESHOLD) {
        current_motion.inlier_indices[current_motion.num_inliers++] = i;
        sum_distance += distance;
        sum_distance_squared += distance * distance;
      }
    }

    if (current_motion.num_inliers >= worst_kept_motion->num_inliers &&
        current_motion.num_inliers > 1) {
      int temp;
      double fracinliers, pNoOutliers, mean_distance;
      mean_distance = sum_distance / ((double)current_motion.num_inliers);
      current_motion.variance =
          sum_distance_squared / ((double)current_motion.num_inliers - 1.0) -
          mean_distance * mean_distance * ((double)current_motion.num_inliers) /
              ((double)current_motion.num_inliers - 1.0);
      if (is_better_motion(&current_motion, worst_kept_motion)) {
        // This motion is better than the worst currently kept motion. Remember
        // the inlier points and variance. The parameters for each kept motion
        // will be recomputed later using only the inliers.
        worst_kept_motion->num_inliers = current_motion.num_inliers;
        worst_kept_motion->variance = current_motion.variance;
        memcpy(worst_kept_motion->inlier_indices, current_motion.inlier_indices,
               sizeof(*current_motion.inlier_indices) * npoints);

        assert(npoints > 0);
        fracinliers = (double)current_motion.num_inliers / (double)npoints;
        pNoOutliers = 1 - pow(fracinliers, minpts);
        pNoOutliers = fmax(EPS, pNoOutliers);
        pNoOutliers = fmin(1 - EPS, pNoOutliers);
        temp = (int)(log(1.0 - PROBABILITY_REQUIRED) / log(pNoOutliers));

        if (temp > 0 && temp < N) {
          N = AOMMAX(temp, MIN_TRIALS);
        }

        // Determine the new worst kept motion and its num_inliers and variance.
        for (i = 0; i < num_desired_motions; ++i) {
          if (is_better_motion(worst_kept_motion, &motions[i])) {
            worst_kept_motion = &motions[i];
          }
        }
      }
    }
    trial_count++;
  }

  // Sort the motions, best first.
  qsort(motions, num_desired_motions, sizeof(RANSAC_MOTION), compare_motions);

  // Recompute the motions using only the inliers.
  for (i = 0; i < num_desired_motions; ++i) {
    copy_points_at_indices(points1, corners1, motions[i].inlier_indices,
                           motions[i].num_inliers);
    copy_points_at_indices(points2, corners2, motions[i].inlier_indices,
                           motions[i].num_inliers);

    find_transformation(motions[i].num_inliers, points1, points2,
                        params_by_motion + (MAX_PARAMDIM - 1) * i);
    num_inliers_by_motion[i] = motions[i].num_inliers;
  }

finish_ransac:
  aom_free(points1);
  aom_free(points2);
  aom_free(corners1);
  aom_free(corners2);
  aom_free(image1_coord);
  aom_free(current_motion.inlier_indices);
  for (i = 0; i < num_desired_motions; ++i) {
    aom_free(motions[i].inlier_indices);
  }
  aom_free(motions);

  return ret_val;
}
Пример #20
0
static inline 
int create_subtasks(cluster_t *cl, int tid, proc_t *procinfo, 
		    rrr_t *RRR, val_t *Wstruct, vec_t *Zstruct,
		    workQ_t *workQ, counter_t *num_left)
{
  /* From inputs */
  int              cl_begin  = cl->begin;
  int              cl_end    = cl->end;
  int              depth     = cl->depth;
  int              bl_begin  = cl->bl_begin;
  int              bl_end    = cl->bl_end;
  int              bl_size   = bl_end - bl_begin + 1;
  double           bl_spdiam = cl->bl_spdiam;
  double           lgap;

  int              pid       = procinfo->pid;
  int              nproc     = procinfo->nproc;
  int              nthreads  = procinfo->nthreads;

  double *restrict Wgap      = Wstruct->Wgap;
  double *restrict Wshifted  = Wstruct->Wshifted;
  int    *restrict iproc     = Wstruct->iproc;

  int              ldz       = Zstruct->ldz;
  double *restrict Z         = Zstruct->Z;
  int    *restrict Zindex    = Zstruct->Zindex;

  /* others */
  int    i, l;
  int    max_size;
  task_t *task;
  bool   task_inserted;
  int    new_first, new_last, new_size, new_ftt1, new_ftt2;
  int    sn_first, sn_last, sn_size;
  rrr_t  *RRR_parent;
  int    new_lpid, new_rpid;
  double *restrict D_parent;
  double *restrict L_parent;
  int    my_first, my_last;
  bool   copy_parent_rrr;


  max_size = fmax(1, PMR_get_counter_value(num_left) /
		     (fmin(depth+1,4)*nthreads) );
  task_inserted = true;
  new_first = cl_begin;
  for (i=cl_begin; i<=cl_end; i++) {    

    if ( i == cl_end )
      new_last = i;
    else if ( Wgap[i] >= MIN_RELGAP*fabs(Wshifted[i]) )
      new_last = i;
    else
      continue;

    new_size = new_last - new_first + 1;

    if (new_size == 1) {
      /* singleton was found */
      
      if (new_first==cl_begin || task_inserted==true) {
	/* initialize new singleton task */
	sn_first = new_first;
	sn_last  = new_first;
	sn_size  = 1;
      } else {
	/* extend singleton task by one */
	sn_last++;
	sn_size++;
      }
      
      /* insert task if ... */
      if (i==cl_end || sn_size>=max_size ||
	    Wgap[i+1] < MIN_RELGAP*fabs(Wshifted[i+1])) {

	if (sn_first == cl_begin) {
	  lgap = cl->lgap;
	} else {
	  lgap = Wgap[sn_first-1];
	}
	
	PMR_increment_rrr_dependencies(RRR);
	
	task = PMR_create_s_task(sn_first, sn_last, depth+1, bl_begin,
				 bl_end, bl_spdiam, lgap, RRR);
	
	PMR_insert_task_at_back(workQ->s_queue, task);
	  
	task_inserted = true;
      } else {
	task_inserted = false;
      }
      
    } else {
      /* cluster was found */

      /* check if process involved in processing the new cluster */
      new_lpid = nproc-1;
      new_rpid = 0;
      for (l=new_first; l<=new_last; l++) {
	if (iproc[l] != -1) {
	  new_lpid = imin(new_lpid, iproc[l]);
	  new_rpid = imax(new_rpid, iproc[l]);
	  }
      }
      if (new_lpid > pid || new_rpid < pid) {
	task_inserted = true;
	new_first = i + 1;
	continue;
      }

      /* find gap to the left */
      if (new_first == cl_begin) {
	lgap = cl->lgap;
      } else {
	lgap = Wgap[new_first - 1];
      }
    
      /* determine where to store the parent rrr needed by the
       * cluster to find its new rrr */
      my_first = imax(new_first, cl->proc_W_begin);
      my_last  = imin(new_last,  cl->proc_W_end);
      if ( my_first == my_last ) {
	/* only one eigenvalue of cluster belongs to process */
	copy_parent_rrr = true;
      } else {
	/* store parent rrr in Z at column new_ftt */
	copy_parent_rrr = false;
      }
      new_ftt1 = Zindex[my_first    ];
      new_ftt2 = Zindex[my_first + 1];

      if (copy_parent_rrr == true) {
	/* Copy parent RRR into alloceted arrays and mark them
	 * for freeing later */
	D_parent = (double *) malloc(bl_size * sizeof(double));
	assert(D_parent != NULL);
	
	L_parent = (double *) malloc(bl_size * sizeof(double));
	assert(L_parent != NULL);

	memcpy(D_parent, RRR->D, bl_size*sizeof(double));
	memcpy(L_parent, RRR->L, bl_size*sizeof(double));

	RRR_parent = PMR_create_rrr(D_parent, L_parent, NULL, 
				    NULL, bl_size, depth);
	PMR_set_copied_parent_rrr_flag(RRR_parent, true);

      } else {
	/* copy parent RRR into Z to make cluster task independent */
	memcpy(&Z[new_ftt1*ldz+bl_begin], RRR->D, 
	       bl_size*sizeof(double));
	memcpy(&Z[new_ftt2*ldz+bl_begin], RRR->L, 
	       bl_size*sizeof(double));

	RRR_parent = PMR_create_rrr(&Z[new_ftt1*ldz + bl_begin],
				    &Z[new_ftt2*ldz + bl_begin],
				    NULL, NULL, bl_size, depth);
      }
      
      /* Create the task for the cluster and put it in the queue */ 
      task = PMR_create_c_task(new_first, new_last, depth+1, 
			       bl_begin, bl_end, bl_spdiam, lgap, 
			       cl->proc_W_begin, cl->proc_W_end, 
			       new_lpid, new_rpid, RRR_parent);

      if (new_lpid != new_rpid)
	PMR_insert_task_at_back(workQ->r_queue, task);
      else
	PMR_insert_task_at_back(workQ->c_queue, task);

      task_inserted = true;
      
    } /* if singleton or cluster found */

    new_first = i + 1;
  } /* end i */
  
  /* set flag in RRR that last singleton is created */
  PMR_set_parent_processed_flag(RRR);
  
  /* clean up */
  PMR_try_destroy_rrr(RRR);
  free(cl);

  return(0);
} /* end create_subtasks */
Пример #21
0
static double find_inverse_gamma(double a, double p, double q)
{
    /*
     * In order to understand what's going on here, you will
     * need to refer to:
     *
     * Computation of the Incomplete Gamma Function Ratios and their Inverse
     * ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR.
     * ACM Transactions on Mathematical Software, Vol. 12, No. 4,
     * December 1986, Pages 377-393.
     */
    double result;

    if (a == 1) {
        if (q > 0.9) {
            result = -log1p(-p);
        }
        else {
            result = -log(q);
        }
    }
    else if (a < 1) {
        double g = Gamma(a);
        double b = q * g;

        if ((b > 0.6) || ((b >= 0.45) && (a >= 0.3))) {
            /* DiDonato & Morris Eq 21:
             *
             * There is a slight variation from DiDonato and Morris here:
             * the first form given here is unstable when p is close to 1,
             * making it impossible to compute the inverse of Q(a,x) for small
             * q. Fortunately the second form works perfectly well in this case.
             */
            double u;
            if((b * q > 1e-8) && (q > 1e-5)) {
                u = pow(p * g * a, 1 / a);
            }
            else {
                u = exp((-q / a) - NPY_EULER);
            }
            result = u / (1 - (u / (a + 1)));
        }
        else if ((a < 0.3) && (b >= 0.35)) {
            /* DiDonato & Morris Eq 22: */
            double t = exp(-NPY_EULER - b);
            double u = t * exp(t);
            result = t * exp(u);
        }
        else if ((b > 0.15) || (a >= 0.3)) {
            /* DiDonato & Morris Eq 23: */
            double y = -log(b);
            double u = y - (1 - a) * log(y);
            result = y - (1 - a) * log(u) - log(1 + (1 - a) / (1 + u));
        }
        else if (b > 0.1) {
            /* DiDonato & Morris Eq 24: */
            double y = -log(b);
            double u = y - (1 - a) * log(y);
            result = y - (1 - a) * log(u)
                - log((u * u + 2 * (3 - a) * u + (2 - a) * (3 - a))
                      / (u * u + (5 - a) * u + 2));
        }
        else {
            /* DiDonato & Morris Eq 25: */
            double y = -log(b);
            double c1 = (a - 1) * log(y);
            double c1_2 = c1 * c1;
            double c1_3 = c1_2 * c1;
            double c1_4 = c1_2 * c1_2;
            double a_2 = a * a;
            double a_3 = a_2 * a;

            double c2 = (a - 1) * (1 + c1);
            double c3 = (a - 1) * (-(c1_2 / 2)
                                   + (a - 2) * c1
                                   + (3 * a - 5) / 2);
            double c4 = (a - 1) * ((c1_3 / 3) - (3 * a - 5) * c1_2 / 2
                                   + (a_2 - 6 * a + 7) * c1
                                   + (11 * a_2 - 46 * a + 47) / 6);
            double c5 = (a - 1) * (-(c1_4 / 4)
                                   + (11 * a - 17) * c1_3 / 6
                                   + (-3 * a_2 + 13 * a -13) * c1_2
                                   + (2 * a_3 - 25 * a_2 + 72 * a - 61) * c1 / 2
                                   + (25 * a_3 - 195 * a_2 + 477 * a - 379) / 12);

            double y_2 = y * y;
            double y_3 = y_2 * y;
            double y_4 = y_2 * y_2;
            result = y + c1 + (c2 / y) + (c3 / y_2) + (c4 / y_3) + (c5 / y_4);
        }
    }
    else {
        /* DiDonato and Morris Eq 31: */
        double s = find_inverse_s(p, q);

        double s_2 = s * s;
        double s_3 = s_2 * s;
        double s_4 = s_2 * s_2;
        double s_5 = s_4 * s;
        double ra = sqrt(a);

        double w = a + s * ra + (s_2 - 1) / 3;
        w += (s_3 - 7 * s) / (36 * ra);
        w -= (3 * s_4 + 7 * s_2 - 16) / (810 * a);
        w += (9 * s_5 + 256 * s_3 - 433 * s) / (38880 * a * ra);

        if ((a >= 500) && (fabs(1 - w / a) < 1e-6)) {
            result = w;
        }
        else if (p > 0.5) {
            if (w < 3 * a) {
                result = w;
            }
            else {
                double D = fmax(2, a * (a - 1));
                double lg = lgam(a);
                double lb = log(q) + lg;
                if (lb < -D * 2.3) {
                    /* DiDonato and Morris Eq 25: */
                    double y = -lb;
                    double c1 = (a - 1) * log(y);
                    double c1_2 = c1 * c1;
                    double c1_3 = c1_2 * c1;
        	    double c1_4 = c1_2 * c1_2;
		    double a_2 = a * a;
		    double a_3 = a_2 * a;

		    double c2 = (a - 1) * (1 + c1);
		    double c3 = (a - 1) * (-(c1_2 / 2)
					   + (a - 2) * c1
					   + (3 * a - 5) / 2);
		    double c4 = (a - 1) * ((c1_3 / 3)
					   - (3 * a - 5) * c1_2 / 2
					   + (a_2 - 6 * a + 7) * c1
					   + (11 * a_2 - 46 * a + 47) / 6);
		    double c5 = (a - 1) * (-(c1_4 / 4)
					   + (11 * a - 17) * c1_3 / 6
					   + (-3 * a_2 + 13 * a -13) * c1_2
					   + (2 * a_3 - 25 * a_2 + 72 * a - 61) * c1 / 2
					   + (25 * a_3 - 195 * a_2 + 477 * a - 379) / 12);

		    double y_2 = y * y;
		    double y_3 = y_2 * y;
		    double y_4 = y_2 * y_2;
		    result = y + c1 + (c2 / y) + (c3 / y_2) + (c4 / y_3) + (c5 / y_4);
		}
		else {
		    /* DiDonato and Morris Eq 33: */
		    double u = -lb + (a - 1) * log(w) - log(1 + (1 - a) / (1 + w));
		    result = -lb + (a - 1) * log(u) - log(1 + (1 - a) / (1 + u));
		}
	    }
	}
	else {
	    double z = w;
	    double ap1 = a + 1;
	    double ap2 = a + 2;
	    if (w < 0.15 * ap1) {
		/* DiDonato and Morris Eq 35: */
		double v = log(p) + lgam(ap1);
		z = exp((v + w) / a);
		s = log1p(z / ap1 * (1 + z / ap2));
		z = exp((v + z - s) / a);
		s = log1p(z / ap1 * (1 + z / ap2));
		z = exp((v + z - s) / a);
		s = log1p(z / ap1 * (1 + z / ap2 * (1 + z / (a + 3))));
		z = exp((v + z - s) / a);
	    }

	    if ((z <= 0.01 * ap1) || (z > 0.7 * ap1)) {
		result = z;
	    }
	    else {
		/* DiDonato and Morris Eq 36: */
		double ls = log(didonato_SN(a, z, 100, 1e-4));
		double v = log(p) + lgam(ap1);
		z = exp((v + z - ls) / a);
		result = z * (1 - (a * log(z) - z - v + ls) / (a - z));
	    }
	}
    }
    return result;
}
Пример #22
0
int FinalRead::alfaBeta(int alfa, int beta)
{
	if (board.getTurnPlayer() == 1)
	{
		for (int i = 1; i <= board.getAbleNumber(); i++)
		{

			board.put(i);
			switch (board.getSCondition())
			{
			case 0:
				alfa = fmax(alfa, alfaBeta(alfa, beta));
				break;
			case 4:
				board.pass();
				alfa = fmax(alfa, alfaBeta(alfa, beta));
				board.undo();
				break;
			default:
				alfa = board.getNumber(1) - board.getNumber(2);
				count++;
				break;
			}
			board.undo();
			if (alfa >= beta)
			{
				return beta;
			}
			if (alfa > 0)
			{
				return alfa;
			}
		}
		return alfa;
	}
	if (board.getTurnPlayer() == 2)
	{
		for (int i = 1; i <= board.getAbleNumber(); i++)
		{
			board.put(i);
			switch (board.getSCondition())
			{
			case 0:
				beta = fmin(beta, alfaBeta(alfa, beta));
				break;
			case 4:
				board.pass();
				beta = fmin(beta, alfaBeta(alfa, beta));
				board.undo();
				break;
			default:
				beta = board.getNumber(1) - board.getNumber(2);
				count++;
				break;
			}
			board.undo();
			if (alfa >= beta)
			{
				return alfa;
			}
			if (beta < 0)
			{
				return beta;
			}
		}
		return beta;
	}
}int FinalRead::alfaBetaRev(int alfa, int beta)
Пример #23
0
int exec(int op, int a, int b) {
   int result;
const float PI =3.14159265;
if(op<0) {
	op = -op;
	int c;
	c =a;
	a =b;
	b =c;
	}
	switch (op){
	case 0:
		result =-a;
		break;
	case 1:
		result = a+b;
		break;
	case 2:
		result =a-b;
		break;
	case 3:
		result =a*b;
		break;
	case 4:
		result =a/b;
		break;
	case 5:
		result =abs(a);
		break;
	case 6:
		if((b<1)&&(a!=1)&&(a!=-1))
			result =0;
		else
			result = pow(a, b);
		break;
	case 7:
	case 13:
	case 77:
		result =a%b;
		break;
	case 8:
		if(a==b)
			result =0;
		else
			result =fmax(a, b);
		break;
	case 9:
		if(a==b)
			result =0;
		else
			result =fmin(a, b);
		break;
	case 10:
		switch (abs(b)%8){
				case 0:
					result =abs(a)*sizeof(char);
					break;
				case 1:
					result =abs(a)*sizeof(signed char);
					break;
				case 2:
					result =abs(a)*sizeof(short int);
					break;
				case 3:
					result =abs(a)*sizeof(unsigned int);
					break;
				case 4:
					result =abs(a)*sizeof(long int);
					break;
				case 5:
					result =abs(a)*sizeof(unsigned long long int);
					break;
				case 6:
					result =abs(a)*sizeof(float);
					break;
				case 7:
					result =abs(a)*sizeof(double);
					break;
		}
		break;
	case 11:
		if(b=2)
			result =0;
		else
			result =nearbyint(tg(a*PI)/(2-b));
		break;
	default:
		if(op<100){
			if((a == -1) || (b == -1))
				result =0;
			else
			result =((op / abs(a + 1)) + (op / abs(b + 1)));
		}
		else
			result =-1;
	}
   return result;
}
Пример #24
0
    double lr_event_rate (const vector <DIM> &r_, unsigned direction)
    {
        const double L = period[direction];
        const vector <DIM> shift = unit_vector <DIM> (direction) * (L/2);

        // assign polar pair partner
        const double x_prim = r_[direction];
        const double x_pair = x_prim - 2*L * floor (x_prim/L) - L;

        bool first_copy = fabs (x_prim - x_pair) < 2*L;
        vector <DIM> r[2] = { r_, r_ };
        if (x_prim >= 0.)
            r[0][direction] = x_pair;
        else
            r[1][direction] = x_pair;
        double rsq[2] = { norm_sq (r[0]), norm_sq (r[1]) };

        assert (r[0][direction] < 0.);
        assert (r[1][direction] >= 0.);

        // compute the rate for the polar pair
        double rate = 0.;

        // for large distances, the naive formula for the event rate can be
        // numerically problematic.
        if (fmax (rsq[0], rsq[1]) < 1e6*L*L)
        {
            for (int i = 0; i != 2; ++i)
            {
                double x = r[i][direction];

                double u_rp = (i==0 && first_copy)
                    ? 0.
                    : unit_potential (r[i] + shift);
                double u_rm = (i==1 && first_copy)
                    ? 0.
                    : unit_potential (r[i] - shift);

                rate += (u_rp-u_rm) / L;

                // main charge -- parts smaller than sr_lr_split are handled by the SR code
                if (rsq[i] > sq (sr_lr_split))
                    rate += unit_directional_derivative (x, rsq[i]);
            }
        }
        else if (fmax (rsq[0], rsq[1]) < 1e20*L*L)
        {
            for (int i = 0; i != 2; ++i)
            {
                double x = r[i][direction];
                // avoid catastrophic cancellation in the far field
                // (valid also for exponent = 0)
                rate += x * pow (rsq[i], -.5*6. -.5*exponent) * L*L
                    * (2+exponent) * (1./24) * (3.*rsq[i] - (4+exponent)*x*x);
            }
        }
        else
        {
            // rate < numerical precision
        }

        // asymptotic decay of particle event rate is ~ 1 / r^(n+4).
        double w = (r[0][direction] == r_[direction])
            ? norm_sq (r[1]) / norm_sq (r[0])
            : norm_sq (r[0]) / norm_sq (r[1]);

        rate /= 1 + pow (w, -.5 * (exponent+4.));

        if (rate <= 0.)
            return 0.;
        else
            return stren * rate;
    }
Пример #25
0
void ElevationMap::updateBuffers(int paddingIn) // call this function whenever you modify the elevations
{
    unsigned int nx = _nx;
    unsigned int ny = _ny;

    float* h = _h;

    if (paddingIn != 0)
    {
        unsigned int padding = paddingIn;

        if (paddingIn < 0)
        {
            padding = (unsigned int)fmax(1.0f, sqrt(_nx*_ny) / 8);
        }
        nx = _nx + 2 * padding;
        ny = _ny + 2 * padding;

        h = new float[nx*ny];

        int counter = 0;
        for (unsigned int j = 0; j < padding*ny; j++, counter++)
        {
            h[counter] = 0;
        }
        for (unsigned int i = 0; i < _nx; i++)
        {
            for (unsigned int j = 0; j < padding; j++, counter++)
            {
                h[counter] = 0;
            }
            for (unsigned int j = 0; j < _ny; j++, counter++)
            {
                h[counter] = _h[_ny*i + j];
            }
            for (unsigned int j = 0; j < padding; j++, counter++)
            {
                h[counter] = 0;
            }
        }
        for (unsigned int j = 0; j < padding*ny; j++, counter++)
        {
            h[counter] = 0;
        }
		_t += (float)padding * glm::vec3(-_dy, 0, -_dx);
    }

    unsigned int nVerts = nx*ny;
    unsigned int xDivs = nx - 1;
    unsigned int yDivs = ny - 1;
    unsigned int nQuads = xDivs*yDivs;
    _nTriangles = 2 * nQuads;

    glm::vec3* positions = new glm::vec3[nVerts];
    glm::vec3* normals = new glm::vec3[nVerts];
    glm::ivec3* indices = new glm::ivec3[2 * nQuads];

    for (unsigned int i = 0; i < nx; i++) // COMPUTE POSITIONS
    {
        float x = i*_dx;

        for (unsigned int j = 0; j < ny; j++)
        {
            float y = j*_dy;

			positions[ny*i + j] = glm::vec3(y, h[ny*i + j], x);
        }
    }

    if (h != _h)
    {
        delete[] h;
    }

    auto clamp = [](int a, int b, int c)
    {
        if (b < a)
            return a;
        if (b > c)
            return c;
        return b;
    };

    for (unsigned int i = 0; i < nx; i++) // COMPUTE NORMALS
	{
		unsigned int iM = clamp(0, i - 1, nx - 1);
		unsigned int iP = clamp(0, i + 1, nx - 1);

		for (unsigned int j = 0; j < ny; j++)
		{
			unsigned int jM = clamp(0, j - 1, ny - 1);
			unsigned int jP = clamp(0, j + 1, ny - 1);

			glm::vec3 gx = positions[ny*iP + j] - positions[ny*iM + j];
			glm::vec3 gy = positions[ny*i + jP] - positions[ny*i + jM];

            normals[ny*i + j] = glm::normalize(glm::cross(gx, gy));
        }
    }

    for (unsigned int i = 0; i < xDivs; i++) // COMPUTE THE TRIANGLE INDICES
    {
        for (unsigned int j = 0; j < yDivs; j++)
        {
            unsigned int q = yDivs*i + j;

            unsigned int v00 = ny*i + j;
            unsigned int v10 = ny*(i + 1) + j;
            unsigned int v11 = ny*(i + 1) + (j + 1);
            unsigned int v01 = ny*i + (j + 1);

            indices[2 * q + 0] = glm::ivec3(v00, v10, v11);
            indices[2 * q + 1] = glm::ivec3(v11, v01, v00);
        }
    }

    glUseProgram(_shader); // I think you can actually forgo this if you use "layouts" but I prefer grabbing the attrib names instead

    // Tell OpenGL which vertex array object we would like to hold our vertex buffer objects in
    glBindVertexArray(_VAO);

    // Pass in PER-VERTEX attributes as buffer elements of _VAO
    glBindBuffer(GL_ARRAY_BUFFER, _pVBO);
    glBufferData(GL_ARRAY_BUFFER, sizeof(glm::vec3)*nVerts, &positions[0], GL_STATIC_DRAW);
    GLint in_position = glGetAttribLocation(_shader, "in_position");
    glEnableVertexAttribArray(in_position);
    glVertexAttribPointer(in_position, 3, GL_FLOAT, GL_FALSE, sizeof(glm::vec3), (void*)0);

    glBindBuffer(GL_ARRAY_BUFFER, _nVBO);
    glBufferData(GL_ARRAY_BUFFER, sizeof(glm::vec3)*nVerts, &normals[0], GL_STATIC_DRAW);
    GLint in_normal = glGetAttribLocation(_shader, "in_normal");
    glEnableVertexAttribArray(in_normal);
    glVertexAttribPointer(in_normal, 3, GL_FLOAT, GL_FALSE, sizeof(glm::vec3), (void*)0);

    // Pass in triangle indices. Each ivec3 holds the indices for one triangle
    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, _IBO);
    glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(glm::ivec3) * 2 * nQuads, &indices[0], GL_STATIC_DRAW);

    // Unbind everything. Note the order of operations: i.e. unbind the VAO before unbinding the VBOs
    glBindVertexArray(0);
    glBindBuffer(GL_ARRAY_BUFFER, 0);
    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);

    glUseProgram(0);

    delete[] positions;
    delete[] normals;
    delete[] indices;
}
Пример #26
0
static enum req_fsm_nxt
cnt_deliver(struct worker *wrk, struct req *req)
{
	struct busyobj *bo;

	CHECK_OBJ_NOTNULL(wrk, WORKER_MAGIC);
	CHECK_OBJ_NOTNULL(req, REQ_MAGIC);
	CHECK_OBJ_NOTNULL(req->objcore, OBJCORE_MAGIC);
	CHECK_OBJ_NOTNULL(req->objcore->objhead, OBJHEAD_MAGIC);
	CHECK_OBJ_NOTNULL(req->vcl, VCL_CONF_MAGIC);
	assert(WRW_IsReleased(wrk));

	assert(req->objcore->refcnt > 0);

	if (req->objcore->exp_flags & OC_EF_EXP)
		EXP_Touch(req->objcore, req->t_prev);

	HTTP_Setup(req->resp, req->ws, req->vsl, SLT_RespMethod);
	AZ(HTTP_Decode(req->resp,
	    ObjGetattr(req->wrk, req->objcore, OA_HEADERS, NULL)));
	http_ForceField(req->resp, HTTP_HDR_PROTO, "HTTP/1.1");

	if (req->wrk->stats->cache_hit)
		http_PrintfHeader(req->resp,
		    "X-Varnish: %u %u", VXID(req->vsl->wid),
		    ObjGetXID(wrk, req->objcore));
	else
		http_PrintfHeader(req->resp,
		    "X-Varnish: %u", VXID(req->vsl->wid));

	/* We base Age calculation upon the last timestamp taken during
	   client request processing. This gives some inaccuracy, but
	   since Age is only full second resolution that shouldn't
	   matter. (Last request timestamp could be a Start timestamp
	   taken before the object entered into cache leading to negative
	   age. Truncate to zero in that case).
	*/
	http_PrintfHeader(req->resp, "Age: %.0f",
	    fmax(0., req->t_prev - req->objcore->exp.t_origin));

	http_SetHeader(req->resp, "Via: 1.1 varnish-v4");

	if (cache_param->http_gzip_support &&
	    ObjCheckFlag(req->wrk, req->objcore, OF_GZIPED) &&
	    !RFC2616_Req_Gzip(req->http))
		RFC2616_Weaken_Etag(req->resp);

	VCL_deliver_method(req->vcl, wrk, req, NULL, req->http->ws);
	VSLb_ts_req(req, "Process", W_TIM_real(wrk));

	/* Stop the insanity before it turns "Hotel California" on us */
	if (req->restarts >= cache_param->max_restarts)
		wrk->handling = VCL_RET_DELIVER;

	if (wrk->handling != VCL_RET_DELIVER) {
		assert(req->objcore == req->objcore);
		(void)HSH_DerefObjCore(wrk, &req->objcore);
		http_Teardown(req->resp);

		switch (wrk->handling) {
		case VCL_RET_RESTART:
			req->req_step = R_STP_RESTART;
			break;
		case VCL_RET_SYNTH:
			req->req_step = R_STP_SYNTH;
			break;
		default:
			INCOMPL();
		}

		return (REQ_FSM_MORE);
	}

	assert(wrk->handling == VCL_RET_DELIVER);

	if (!(req->objcore->flags & OC_F_PASS)
	    && req->esi_level == 0
	    && http_IsStatus(req->resp, 200)
	    && req->http->conds && RFC2616_Do_Cond(req)) {
		http_PutResponse(req->resp, "HTTP/1.1", 304, NULL);
		req->wantbody = 0;
	}

	/* Grab a ref to the bo if there is one, and hand it down */
	bo = HSH_RefBusy(req->objcore);
	if (bo != NULL && req->esi_level == 0 && bo->state == BOS_FINISHED)
		VBO_DerefBusyObj(wrk, &bo);
	V1D_Deliver(req, bo);
	if (bo != NULL)
		VBO_DerefBusyObj(wrk, &bo);

	VSLb_ts_req(req, "Resp", W_TIM_real(wrk));

	if (http_HdrIs(req->resp, H_Connection, "close"))
		req->doclose = SC_RESP_CLOSE;

	if (req->objcore->flags & OC_F_PASS) {
		/*
		 * No point in saving the body if it is hit-for-pass,
		 * but we can't yank it until the fetching thread has
		 * finished/abandoned also.
		 */
		while (req->objcore->busyobj != NULL)
			(void)usleep(100000);
		ObjSlim(wrk, req->objcore);
	}

	assert(WRW_IsReleased(wrk));
	(void)HSH_DerefObjCore(wrk, &req->objcore);
	http_Teardown(req->resp);
	return (REQ_FSM_DONE);
}
Пример #27
0
void isolate_root_with_cluster(double& low, double& hgh, Polynomial *p)
{
  dprint(low);
  dprint(hgh);
  // ensure starting conditions
  //
  PointVal pv_low(low,p);
  // double p_low = eval(low);
  if(pv_low.v() < 0.)
  {
    Polynomial p_minus = -(*p);
    p_minus.isolate_root(low,hgh);
    return;
  }

  // initialize algorithm
  //
  // initialize root bracket.
  //
  PointVal pv_hgh(hgh,p);
  RootBracketClusterState rb(pv_low,pv_hgh);
  dprint(rb.low().x());
  dprint(rb.low().v());
  dprint(rb.hgh().x());
  dprint(rb.hgh().v());
  //
  Polynomial Dp = p->get_derivative();

  // requirements of algorithm:
  // - second-order convergence in all cases
  // - high performance in generic cases
  // - handles repeated polynomial roots
  // - converges to a root between low and hgh
  // - residual of answer is less than machine precision

  // conditions maintained by algorithm:
  // * low < hgh
  // * p(low) > EPSILON
  // * p(hgh) < -EPSILON
  // using_hgh = fabs(p(hgh)) > fabs(p(low))

  // state of algorithm is specified by:
  // * low
  // * hgh
  // * using_hgh
  // * num_roots_in_cluster = assumed odd number of roots in cluster

  // iteration of algorithm:
  // use newton iteration on smaller side to look for root
  // - check that guess is between low and hgh
  //   and that new value has smaller residual
  //   else switch to secant method
  // - if newton does not change the sign
  //   increment num_roots_in_cluster until you leave interval,
  //   the sign changes, or the residual increases.
  //  - if the sign does change
  //    decrement num_roots_in_cluster until num_roots_in_cluster=1
  //    or the sign does not change (if residual does not shrink
  //    sufficiently modify num_roots_in_cluster).
  //  - do one iteration of bisection if residual did not
  //    shrink by factor of e.g. .5

  // assumed number of roots in root cluster we are converging towards
  goto start_newton_iteration;

 start_newton_iteration:
  // initialize newton iteration
  // choose smaller endpoint as starting value.
  goto newton_iteration;

 done:
   dprint(rb.num_evals());
   low = rb.low().x();
   hgh = rb.hgh().x();
   //return rb.x();
   return;

 // newton's method begins with a point which is one end of a root bracket.
 // it determines a displacement based on the value of the function
 // and its derivative at the point.  To handle root clusters
 // it assumes that the function is approximated by a shifted
 // and scaled monomial.
 newton_iteration:
  {
    if(rb.done()) goto done;
    dprintf("using newton: x0=%24.16e, p(x0)=%24.16e",rb.pv().x(),rb.pv().v());

    const PointVal start_pv0(rb.pv());
    //
    const double Dpx0 = Dp.eval(rb.pv().x());
    // bail if the derivative is too small
    if(fabs(Dpx0) < 1e-6)
    {
      dprintf("bad derivative: %f",Dpx0);
      goto newton_bailed;
    }
    double displacement = -rb.pv().v()/Dpx0;
   compute_x0:
    double x0 = rb.pv().x() + rb.num_roots_in_cluster()*displacement;
    // if Newton prediction is outside interval
    if(rb.not_in_interval(x0))
    {
      // reset num_roots_in_cluster and try again.
      if(rb.num_roots_in_cluster()!=1)
      {
        rb.reset_num_roots_in_cluster();
        goto compute_x0;
      }
      dprintf("Newton %24.16e outside interval [%24.16e,%24.16e]",
        x0, rb.low().x(), rb.hgh().x());
      dprintf("with values [%24.16e,%24.16e]", rb.low().v(), rb.hgh().v());
      goto newton_bailed;
    }
    // evaluate the polynomial at the new guess
    if(rb.eval(x0,p)) goto done;
    // bail if the new residual is not sufficiently smaller
    if(rb.pv().a() > .5*start_pv0.a()) // exp(-1) < .5 < 1
    {
      dprintf("apx0=%24.16e and start_apx0=%24.16e",rb.pv().a(),start_pv0.a());
      dprintf("so Newton didn't help enough, maybe because Dpx0=%24.16e.",Dpx0);
      goto newton_bailed;
    }
    // did the sign change?
    bool sign_changed = ((start_pv0.v() < 0. && rb.pv().v() > 0.)
                      || (start_pv0.v() > 0. && rb.pv().v() < 0.)) ? true : false;
    // does changing number of roots in root cluster help?
    const PointVal old_pv0 = rb.pv();
    if(sign_changed)
    {
      // keep decrementing presumed number
      // of roots in cluster as long as you can
      // until it does not help the residual
      if(rb.num_roots_in_cluster()>=3)
      {
        //double old_x0 = x0;
       decrement:
        double x0 = old_pv0.x() - 2*displacement;
        // displacement stayed in root bracket, so smaller one will too
        if(rb.eval(x0,p)) goto done;
        if(rb.pv().a() < old_pv0.a())
        {
            // accept decrement
            rb.decrement_num_roots_in_cluster();
            goto decrement;
        }
        else
        {
          // reject decrement
          rb.set_to_using_smaller();
          goto newton_iteration;
        }
      }
      // only one root so just iterate
      goto newton_iteration;
    }
    else // !sign_changed
    {
      // if the residual did not shrink by trigger_ratio then
      // we will try incrementing num_roots_in_cluster
      //
      // compute trigger_ratio
      //
      const double trigger_ratio
        = get_trigger_ratio_for_newton(rb.num_roots_in_cluster());
      // if residual shrunk by more than trigger_ratio we're fine
      if(rb.pv().a() < trigger_ratio*old_pv0.a())
      {
        // evidently not near a root cluster
        // with a wrong assumption about the number
        // of roots, so just iterate
        goto newton_iteration;
      }
      else
      {
        // keep incrementing presumed number of roots
        // in cluster until we are outside the interval
        // or until doing so does not improve
        // the residual by better than multiplying
        // it by a factor of .5
        // (can use any number between 1/e and 1).
        {
         increment:
          x0 += 2*displacement;
          if(rb.not_in_interval(x0))
          {
            // reject increment
            goto newton_iteration;
          }
          const PointVal curr_pv0(rb.pv());
          if(rb.eval(x0,p)) goto done;
          if(rb.pv().a() > .5*curr_pv0.a())
          {
            // reject increment
            //
            // the constraint that we always
            // tighten the root bracket whenever
            // we evaluate the polynomial means
            // we cannot completely reject the evaluation
            // although we can still refrain from
            // incrementing num_roots_in_cluster.
            rb.set_to_using_smaller();
            goto newton_iteration;
          }
          else
          {
            // accept increment
            rb.increment_num_roots_in_cluster();
            goto increment;
          }
        }
      }
      goto newton_iteration;
    }
  }

 newton_bailed:
  {
    // ideally we should try the secant method
    // before reverting to bisection
    //
    // the secant method should handle repeated roots better
    goto secant_bracket_iteration;
  }

 // the secant rule needs to maintain a list of two
 // points that we are "using".
 // the root bracket contains the most recently evaluated point.
 // The secant method is initialized with a root bracket.
 secant_bracket_iteration:
  {
    dprintf("using secant: low=%24.16e, hgh=%24.16e, p_low=%24.16e, p_hgh=%24.16e",
      rb.low().x(),rb.hgh().x(),rb.low().v(),rb.hgh().v());
    // rb.set_to_using_smaller();
    PointVal pv1(rb.pv_old());
    const double start_pva = fmax(pv1.a(),rb.pv().a());

   secant_iteration:
    // double accepted_num_roots_in_cluster = num_roots_in_cluster;
   compute_x0_secant: // for try_num_roots_in_cluster
    const double ratio = rb.pv().v()/pv1.v();
   compute_secant_rule:
      // problem: these numbers can be equal.
      //dprint(rb.pv().v());
      //dprint(pv1.v());
    double x0 = secant_rule(rb.pv().x(),pv1.x(),ratio,
      rb.num_roots_in_cluster());
    // make sure that x0 is in root bracket
    if(x0<rb.low().x() || x0>rb.hgh().x())
    {
      // different signs would force x0 in root bracket
      //assert_gt(ratio, 0.)
      // increasing num roots would only make it worse
      if(rb.num_roots_in_cluster()>1)
      {
        rb.decrement_num_roots_in_cluster();
        goto compute_secant_rule;
      }
      //else
      //{
      //  goto bisect;
      //}
    }
    // evaluate at the candidate
    dprint(x0);
    pv1 = rb.pv(); if(rb.eval(x0,p)) goto done;
    if(rb.pv().a() >= pv1.a())
    {
      dprintf("bisect");
      goto bisect;
    }

    // did residual change signs?

    bool sign_changed = ((pv1.v() < 0. && rb.pv().v() > 0.)
                      || (pv1.v() > 0. && rb.pv().v() < 0.)) ? true : false;
    // if residual changed signs then try decrementing num_roots_in_cluster
    // if residual did not change signs then try incrementing.
    if(!sign_changed)
    {
      // if the residual did not shrink by trigger_ratio then
      // we will try incrementing num_roots_in_cluster
      double secant_trigger = get_trigger_ratio_for_secant(
        rb.num_roots_in_cluster());
      if(pv1.a() > secant_trigger * rb.pv().a())
      {
        // try incrementing num_roots_in_cluster
        const double ratio = rb.pv().v()/pv1.v();
        const double x0 = secant_rule(rb.pv().x(),pv1.x(),ratio,
          rb.num_roots_in_cluster()+2);
        // make sure root is in interval
        if(x0<rb.low().x() || x0>rb.hgh().x())
        {
          dprintf("bisect");
          goto bisect;
        }
        pv1 = rb.pv(); if(rb.eval(x0,p)) goto done;
        if(rb.pv().a() < pv1.a())
        {
          rb.increment_num_roots_in_cluster();
          goto secant_iteration;
        }
      }
    }
    else
    {
      // try decrementing num_roots_in_cluster
      if(rb.num_roots_in_cluster()>=3)
      {
        const double ratio = rb.pv().v()/pv1.v();
        const double x0 = secant_rule(rb.pv().x(),pv1.x(),ratio,
          rb.num_roots_in_cluster()-2);
        // make sure root is in interval
        if(x0<rb.low().x() || x0>rb.hgh().x()) goto bisect;
        pv1 = rb.pv(); if(rb.eval(x0,p)) goto done;
        if(rb.pv().a() < pv1.a())
        {
          rb.decrement_num_roots_in_cluster();
          goto secant_iteration;
        }
      }
    }
   secant_iter_done:
    {
      if(rb.pv().a() > .5*start_pva)
      {
        dprintf("secant did not help enough: apx=%24.16e, old_apx=%24.16e",
          rb.pv().a(), start_pva);
        goto bisect;
      }
      goto secant_bracket_iteration;
    }
  }
  
 bisect:
  {
    dprintf("bisecting: x=%24.16e, px=%24.16e",rb.pv().x(),rb.pv().v());
    const double x0 = (rb.low().x() + rb.hgh().x())*.5;
    if(rb.eval(x0,p)) goto done;
    //goto newton_iteration;
    goto secant_bracket_iteration;
  }
  eprintf("should not get here");
  return;
}
Пример #28
0
int main(int argc, char **argv){
  int i,j,k,n; 
  int nsteps=150;
  double dt =0.01;
  
  /* Computational (2D polar) grid coordinates */
  int nx1 = 400;
  int nx2 = 400;

  //number of ghost cells on both sides of each dimension
  //only need 1 for piecewise constant method
  //need 2 for piecewise linear reconstruction
  int num_ghost = 2;
  int nx1_r = nx1;
  int nx2_r = nx2;
  nx1 += 2*num_ghost; 
  nx2 += 2*num_ghost; 

  /*non-ghost indices */
  int is = num_ghost;
  int ie= is+nx1_r; 
  int js = num_ghost;
  int je = js+nx2_r; 

  //convention: these ranges refer to the non-ghost cells
  //however, the ghost cells have real coordinate interpretations
  //this means that we must be sure that the number of ghost cells makes sense with the range of coordinates
  //this is a big problem if the phi polar coordinate runs the whole range [0,2pi) for example

  //further, all mesh structures (zonal and nodal) will be nx1 x nx2, although we may not fill the ghost entries with anything meaningful
  //this is to standardize the loop indexing from is:ie

  /*another convention: when the phi coordinate is periodic, do we repeat the boundary mesh points? yes for now */

  double lx1 = 2.0;//these values are inclusive [x1_i, x1_f]
  double lx2 = M_PI;
  double x1_i = 0.5;
  double x2_i = 0.0;

  double dx2 = lx2/(nx2_r-1);   
  double x2_f = x2_i + lx2;

  if(X2_PERIODIC){
    dx2 = 2*M_PI/(nx2_r);   
    lx2 = dx2*(nx2_r-1); 
    x2_i = 0.0;
  }

  double dx1 = lx1/(nx1_r-1);   
  double x1_f = x1_i + lx1;

  printf("dx1=%lf dx2=%lf \n",dx1,dx2); 
  
  /*Cell centered (zonal) values of computational coordinate position */
  double *x1 = (double *) malloc(sizeof(double)*nx1); 
  double *x2 = (double *) malloc(sizeof(double)*nx2);
  x1[is] = x1_i;
  x2[js] = x2_i;
  for(i=is+1; i<ie; i++){ 
    x1[i] = x1[i-1] + dx1;
    //    printf("%lf\n",x1[i]);
  }
  for(i=js+1; i<je; i++){
    x2[i] = x2[i-1] + dx2;
    //printf("%lf\n",x2[i]);
  } 

  /*Mesh edge (nodal) values of computational coordinate position */
  double *x1_b = (double *) malloc(sizeof(double)*(nx1+1)); 
  double *x2_b = (double *) malloc(sizeof(double)*(nx2+1));
  x1_b[is] = x1_i - dx1/2;
  x2_b[js] = x2_i - dx2/2;
  for(i=is+1; i<=ie; i++){ 
    x1_b[i] = x1_b[i-1] + dx1;
    //    printf("%lf\n",x1_b[i]);
  }
  for(i=js+1; i<=je; i++){
    x2_b[i] = x2_b[i-1] + dx2;
    //printf("%lf\n",x2_b[i]);
  } 

  /*Cell centered (zonal) values of physical coordinate position */
  //These must be 2D arrays since coordinate transformation is not diagonal
  //indexed by x1 rows and x2 columns
  double **x = (double **) malloc(sizeof(double *)*nx1); 
  double **y = (double **) malloc(sizeof(double *)*nx1);
  double *dataX = (double *) malloc(sizeof(double)*nx1*nx2);
  double *dataY = (double *) malloc(sizeof(double)*nx1*nx2);
  for(i=0; i<nx1; i++){
    x[i] = &(dataX[nx2*i]);
    y[i] = &(dataY[nx2*i]);
  }

  /*precompute coordinate mappings */
  for(i=is; i<ie; i++){
    for(j=js; j<je; j++){
      x[i][j] = X_physical(x1[i],x2[j]); 
      y[i][j] = Y_physical(x1[i],x2[j]); 
    }
  }
  /*Mesh edge (nodal) values of physical coordinate position */
  double **x_b = (double **) malloc(sizeof(double *)*(nx1+1)); 
  double **y_b = (double **) malloc(sizeof(double *)*(nx1+1));
  double *dataXb = (double *) malloc(sizeof(double)*(nx1+1)*(nx2+1));
  double *dataYb = (double *) malloc(sizeof(double)*(nx1+1)*(nx2+1));
  for(i=0; i<=nx1; i++){
    x_b[i] = &(dataXb[(nx2+1)*i]);
    y_b[i] = &(dataYb[(nx2+1)*i]);
  }
  for(i=is; i<=ie; i++){
    for(j=js; j<=je; j++){
      x_b[i][j] = X_physical(x1_b[i],x2_b[j]); 
      y_b[i][j] = Y_physical(x1_b[i],x2_b[j]); 
    }
  }
  /*Edge normal vectors in Cartesian coordinates */
  // point radially outward and +\phi
  

  /*Coordinate cell capacity */
  double **kappa;
  double *datakappa;
  kappa = (double **) malloc(sizeof(double *)*nx1);
  datakappa = (double *) malloc(sizeof(double)*nx1*nx2);
  for(i=0; i<nx1; i++){
    kappa[i] = &(datakappa[nx2*i]);
  }
  
  for(i=is; i<ie; i++){
    for(j=js; j<je; j++){
      kappa[i][j] = x1[i]*dx1*dx2/(dx1*dx2); // C_ij/(dx1*dx2)
      //capacity in ghost cells
      kappa[ie][j] = (x1[ie-1]+dx1)*dx1*dx2/(dx1*dx2); 
    }
    kappa[i][je] = (x1[i]+dx1)*dx1*dx2/(dx1*dx2); 
  }
 
  /*Average normal edge velocities */
  //now we move from cell centered quantities to edge quantities 
  //the convention in this code is that index i refers to i-1/2 edge
  double **U, **V;
  double *dataU, *dataV; 
  U = (double **) malloc(sizeof(double *)*nx1);
  V = (double **) malloc(sizeof(double *)*nx1);
  dataU = (double *) malloc(sizeof(double)*nx1*nx2);
  dataV = (double *) malloc(sizeof(double)*nx1*nx2);
  for(i=0; i<nx1; i++){
    U[i] = &(dataU[nx2*i]);
    V[i] = &(dataV[nx2*i]);
  }
  
  /*Option #1 for computing edge velocities: path integral of Cartesian stream function */
  /* Stream function */ //probably dont need this array
  //this variable is cell centered stream 
  /* double **stream;
  double *datastream;
  stream = (double **) malloc(sizeof(double *)*nx1);
  datastream = (double *) malloc(sizeof(double)*nx1*nx2);
  for(i=0; i<nx1; i++){
    stream[i] = &(datastream[nx2*i]);
  }
  for(i=is; i<ie; i++){
    for(j=js; j<je; j++){
      stream[i][j] = stream_function(x[i][j],y[i][j]);
    }
  }
 
   for(i=is; i<ie; i++){
    for(j=js; j<je; j++){ //go an additional step to capture nx2_r+1/2
      //average the corner stream functions
      U[i][j]= (stream_function(X_physical(x1[i]-dx1/2,x2[j]+dx2/2),Y_physical(x1[i]-dx1/2,x2[j]+dx2/2)) - 
		stream_function(X_physical(x1[i]-dx1/2,x2[j]-dx2/2),Y_physical(x1[i]-dx1/2,x2[j]-dx2/2)))/(dx2);
      V[i][j]= -(stream_function(X_physical(x1[i]+dx1/2,x2[j]-dx2/2),Y_physical(x1[i]+dx1/2,x2[j]-dx2/2)) - 
      	 stream_function(X_physical(x1[i]-dx1/2,x2[j]-dx2/2),Y_physical(x1[i]-dx1/2,x2[j]-dx2/2)))/dx1;
      if(j==10)
	printf("i=%d, u,v=%lf,%lf\n",i,U[i][j],V[i][j]); 
    }
    j=je;
    U[i][j]= (stream_function(X_physical(x1[i]-dx1/2,x2[j-1]+3*dx2/2),Y_physical(x1[i]-dx1/2,x2[j-1]+3*dx2/2)) - 
	      stream_function(X_physical(x1[i]-dx1/2,x2[j-1]+dx2/2),Y_physical(x1[i]-dx1/2,x2[j-1]+dx2/2)))/(dx2);
    V[i][j]= -(stream_function(X_physical(x1[i]+dx1/2,x2[j-1]+dx2/2),Y_physical(x1[i]+dx1/2,x2[j-1]+dx2/2)) - 
	       stream_function(X_physical(x1[i]-dx1/2,x2[j-1]+dx2/2),Y_physical(x1[i]-dx1/2,x2[j-1]+dx2/2)))/dx1;
  }
  i=ie;
  for(j=js; j<je; j++){
    U[i][j]= (stream_function(X_physical(x1[i-1]+dx1/2,x2[j]+dx2/2),Y_physical(x1[i-1]+dx1/2,x2[j]+dx2/2)) - 
	      stream_function(X_physical(x1[i-1]+dx1/2,x2[j]-dx2/2),Y_physical(x1[i-1]+dx1/2,x2[j]-dx2/2)))/(dx2);
    V[i][j]= -(stream_function(X_physical(x1[i-1]+3*dx1/2,x2[j]-dx2/2),Y_physical(x1[i-1]+3*dx1/2,x2[j]-dx2/2)) - 
	      stream_function(X_physical(x1[i-1]+dx1/2,x2[j]-dx2/2),Y_physical(x1[i-1]+dx1/2,x2[j]-dx2/2)))/dx1;
  } 
  */
  double ux,vy,temp; 
  for(i=is; i<=ie; i++){
    for(j=js; j<=je; j++){ 
      /*Option #2: directly specify velocity in Cartesian coordinate basis as a function of cartesian position*/   
      
      //radial face i-1/2
      velocity_physical(X_physical(x1_b[i],x2_b[j]+dx2/2),Y_physical(x1_b[i],x2_b[j]+dx2/2),&ux,&vy);
      // Average normal edge velocity: just transform face center velocity to local orthonormal basis? 
      vector_physical_to_coordinate(ux,vy,x2_b[j]+dx2/2,&U[i][j],&temp); 
      
#ifdef HORIZONTAL
      //EXACT SOLUTION FOR EDGE VELOCITY FOR HORIZONTAL FLOW
      //      printf("U_before = %lf\n",U[i][j]);
      U[i][j] = (sin(x2_b[j]+dx2) - sin(x2_b[j]))/(dx2); 
      //      printf("U_after = %lf\n",U[i][j]);
#endif

      //phi face j-1/2
      velocity_physical(X_physical(x1_b[i]+dx1/2,x2_b[j]),Y_physical(x1_b[i]+dx1/2,x2_b[j]),&ux,&vy);
      vector_physical_to_coordinate(ux,vy,x2_b[j],&temp,&V[i][j]); 
#if defined(SEMI_CLOCK) || defined(GAUSS_CLOCK)
      velocity_coordinate(x1_b[i],x2_b[j],&U[i][j],&V[i][j]);
#endif
      //      printf("U,V = %lf,%lf\n",U[i][j],V[i][j]);
    }
  } 

  /* check normalization of velocities  */
  /* these are naturally not normalized because the cell has finite volume so the edge velocities arent the velocity of the same point */
  double norm; 
  double max_deviation =0.0; 
  for(i=is; i<=ie; i++){
    for(j=js; j<=je; j++){ 
      norm = sqrt(U[i][j]*U[i][j] + V[i][j]*V[i][j]); 
      if (fabs(norm-1.0) > max_deviation)
	max_deviation = fabs(norm-1.0); 
      //  printf("%0.12lf\n",norm);
    }
  }
  //  printf("maximum deviation from 1.0 = %0.12lf\n",max_deviation); 



  /*Option #3: specify velocity in polar coordinates */


  /*Check CFL condition, reset timestep */
  float **cfl_array;
  float *dataCFL;
  cfl_array = (float **) malloc(sizeof(float *)*nx1);
  dataCFL = (float *) malloc(sizeof(float)*nx1*nx2);
  for(i=0; i<nx1; i++){
    cfl_array[i] = &(dataCFL[nx2*i]);
  }
  for(i=1; i<nx1; i++){
    for(j=1; j<nx2; j++){ //based on edge velocities or cell centered u,v?
      if (i >=is && i< ie && j >=js && j <je)
	cfl_array[i][j] = fabs(U[i][j])*dt/dx1 + fabs(V[i][j])*dt/(x1_b[i]*dx2); //use boundary radius
      else
	cfl_array[i][j] =0.0; 
    }
  }
  //find maximum CFL value in domain
  float  max_cfl = find_max(dataCFL,nx1*nx2); 
  printf("Largest CFL number = %lf\n",max_cfl); 
  if (max_cfl > CFL || AUTO_TIMESTEP){//reset timestep if needed
    dt = CFL*dt/max_cfl; 
    for(i=1; i<nx1; i++){
      for(j=1; j<nx2; j++){ 
	if (i >=is && i< ie && j >=js && j <je)
	  cfl_array[i][j] = fabs(U[i][j])*dt/dx1 + fabs(V[i][j])*dt/(x1_b[i]*dx2);
	else
	  cfl_array[i][j] =0.0; 
      }
    } 
  }
  max_cfl = find_max(dataCFL,nx1*nx2); 
  printf("Largest CFL number = %lf\n",max_cfl); 
  
#ifdef SEMI_CLOCK
  nsteps = M_PI/dt;
  printf("nsteps = %d, dt = %lf, t_final = %lf\n",nsteps,dt,nsteps*dt);
#endif
  
#ifdef GAUSS_CLOCK
  nsteps = 2*M_PI/dt;
  //turn dt down until mod(2*MPI,nsteps) = 0
  double remainder = 2*M_PI -nsteps*dt;
  double extra = remainder/nsteps; 
  double dt_new = dt +extra; 
  printf("nsteps = %d, dt = %lf, dt_new =%lf, remainder = %lf, t_final = %lf t_final_new =%lf\n",nsteps,dt,dt_new,remainder,nsteps*dt,nsteps*dt_new);
  max_cfl *= dt_new/dt; 
  dt = dt_new; 
  printf("Largest CFL number = %lf\n",max_cfl); 
#endif

  /*Conserved variable on the computational coordinate mesh*/
  double **Q;
  double *dataQ;
  //make contiguous multiarray                                                                                                
  Q = (double **) malloc(sizeof(double *)*nx1);
  dataQ = (double *) malloc(sizeof(double)*nx1*nx2);
  for(i=0; i<nx1; i++){
    Q[i] = &(dataQ[nx2*i]);
  }
  /*Initial condition */
  //specified as a function of cartesian physical cooridnates, as is the stream function
  for(i=is; i<ie; i++){
    for(j=js; j<je; j++){
      Q[i][j] = initial_condition(x[i][j],y[i][j]); 
    }
  }

  //net fluctuations/fluxes
  double U_plus,U_minus,V_plus,V_minus;
  double **net_fluctuation;
  double *dataFlux;
  net_fluctuation = (double **) malloc(sizeof(double *)*nx1);
  dataFlux = (double *) malloc(sizeof(double)*nx1*nx2);
  for(i=0; i<nx1; i++){
    net_fluctuation[i] = &(dataFlux[nx2*i]);
  }

  /* Using Visit VTK writer */
  char filename[20];
  int dims[] = {nx1_r+1, nx2_r+1, 1}; //dont output ghost cells. //nodal variables have extra edge point
  int nvars = 2;
  int vardims[] = {1, 3}; //Q is a scalar, velocity is a 3-vector 
  int centering[] = {0, 1}; // Q is cell centered, velocity is defined at edges
  const char *varnames[] = {"Q", "edge_velocity"};
  /* Curvilinear mesh points stored x0,y0,z0,x1,y1,z1,...*/
  //An array of size nI*nJ*nK*3 . These points are nodal, not zonal... unfortunatley
  float *pts = (float *) malloc(sizeof(float)*(nx1_r+1)*(nx2_r+1)*3); 
  //The array should be layed out as (pt(i=0,j=0,k=0), pt(i=1,j=0,k=0), ...
  //pt(i=nI-1,j=0,k=0), pt(i=0,j=1,k=0), ...).
  int index=0; 
  for(k=0; k<1; k++){
    for(j=js; j<=je; j++){
      for(i=is; i<=ie; i++){
	pts[index] = x_b[i][j];
	pts[++index] = y_b[i][j];
	pts[++index] = 0.0;
	index++;
      }
    }
  }

  /* pack U and V into a vector */
  float *edge_vel = (float *) malloc(sizeof(float)*(nx1_r+1)*(nx2_r+1)*3); //An array of size nI*nJ*nK*3 
  index=0; 
  for(k=0; k<1; k++){
    for(j=js; j<=je; j++){
      for(i=is; i<=ie; i++){
	vector_coordinate_to_physical(U[i][j],V[i][j], x2[j], &ux,&vy);
	//edge_vel[index] = U[i][j];
	//edge_vel[++index] = V[i][j]; 
	edge_vel[index] = ux;
	edge_vel[++index] = vy;
	edge_vel[++index] = 0.0;
	index++;
      }
    }
  } 

  //  vars       An array of variables.  The size of vars should be nvars.
  //                 The size of vars[i] should be npts*vardim[i].
  float *realQ; 
  realQ =(float *) malloc(sizeof(double)*nx1_r*nx2_r);
  float *vars[] = {(float *) realQ, (float *)edge_vel};

  /*-----------------------*/
  /* Main timestepping loop */
  /*-----------------------*/
  for (n=0; n<nsteps; n++){
    /*Boundary conditions */
    //bcs are specified along a computational coord direction, but are a function of the physical coordinate of adjacent "real cells"
    for(k=0;k<num_ghost; k++){
      for (j=js; j<je; j++){
	Q[k][j] = bc_x1i(x[is][j],y[is][j]);
	Q[nx1-1-k][j] = bc_x1f(x[ie-1][j],y[ie-1][j],(n+1)*dt);
      }
      for (i=is; i<ie; i++){
	if(X2_PERIODIC){
	  Q[i][k] = Q[i][je-1-k];
	  Q[i][nx2-1-k] = Q[i][js+k];
	}
	else{
	Q[i][k] = bc_x2i(x[i][js],y[i][js]);
	Q[i][nx2-1-k] = bc_x2f(x[i][je-1],y[i][je-1]);
	}
      }
    }

    double flux_limiter =0.0; 
    double *qmu = (double *) malloc(sizeof(double)*3); //manually copy array for computing slope limiters
    /* Donor cell upwinding */
    for (i=is; i<ie; i++){
      for (j=js; j<je; j++){
	/* First coordinate */
	U_plus = fmax(U[i][j],0.0); // max{U_{i-1/2,j},0.0} LHS boundary
	U_minus = fmin(U[i+1][j],0.0); // min{U_{i+1/2,j},0.0} RHS boundary
	/*Fluctuations: A^+ \Delta Q_{i-1/2,j} + A^- \Delta Q_{i+1/2,j} */
	//	net_fluctuation[i][j] = dt/(kappa[i][j]*dx1)*(U_plus*(Q[i][j] - Q[i-1][j]) + U_minus*(Q[i+1][j] - Q[i][j]));
	/* First order fluxes: F_i+1/2 - F_i-1/2 */
	net_fluctuation[i][j] = dt/(kappa[i][j]*dx1)*(x1_b[i+1]*(fmax(U[i+1][j],0.0)*Q[i][j] + U_minus*Q[i+1][j])-x1_b[i]*(U_plus*Q[i-1][j] + fmin(U[i][j],0.0)*Q[i][j]));
#ifdef SECOND_ORDER
	/* Second order fluxes */
	if (U[i+1][j] > 0.0){ //middle element is always the upwind element
	  qmu[0] = Q[i-1][j];
	  qmu[1] = Q[i][j];  
	  qmu[2] = Q[i+1][j];  
	  flux_limiter= flux_PLM(dx1,qmu);
	}
	else{
	  qmu[0] = Q[i+2][j];
	  qmu[1] = Q[i+1][j];  
	  qmu[2] = Q[i][j];  
	  flux_limiter= flux_PLM(dx1,qmu);
	  /*	  if (flux_limiter != 0.0){
	    printf("i,j: %d,%d F_{i+1/2} flux limiter: %lf\n",i,j,flux_limiter);
	    printf("Q0 = %lf Q1= %lf Q2 = %lf\n",qmu[0],qmu[1],qmu[2]); } */
	}
	//F^H_{i+1/2,j}
	net_fluctuation[i][j] -= dt/(kappa[i][j]*dx1)*(x1_b[i+1]*(1-dt*fabs(U[i+1][j])/(dx1))*fabs(U[i+1][j])*flux_limiter/2);
	//	net_fluctuation[i][j] -= dt/(kappa[i][j]*dx1)*((kappa[i][j]/x1_b[i+1]-dt*fabs(U[i+1][j])/(x1_b[i+1]*dx1))*fabs(U[i+1][j])*flux_limiter/2);
	//	net_fluctuation[i][j] += dt/(kappa[i][j]*dx1)*(x1_b[i+1]*(kappa[i][j]/x1_b[i+1]-dt*fabs(U[i+1][j])/(x1_b[i+1]*dx1))*fabs(U[i+1][j])*flux_limiter/2);
	if (U[i][j] > 0.0){
	  qmu[0] = Q[i-2][j];  //points to the two preceeding bins; 
	  qmu[1] = Q[i-1][j];  
	  qmu[2] = Q[i][j];  
	  flux_limiter= flux_PLM(dx1,qmu);
	}
	else{
	  qmu[0] = Q[i+1][j]; //centered around current bin
	  qmu[1] = Q[i][j];  
	  qmu[2] = Q[i-1][j];  
	  flux_limiter= flux_PLM(dx1,qmu);
	}
	//F^H_{i-1/2,j}
	net_fluctuation[i][j] += dt/(kappa[i][j]*dx1)*(x1_b[i]*(1-dt*fabs(U[i][j])/(dx1))*fabs(U[i][j])*flux_limiter/2);
	//	net_fluctuation[i][j] += dt/(kappa[i][j]*dx1)*((kappa[i-1][j]/x1_b[i]-dt*fabs(U[i][j])/(x1_b[i]*dx1))*fabs(U[i][j])*flux_limiter/2);
	//net_fluctuation[i][j] -= dt/(kappa[i][j]*dx1)*(x1_b[i]*(kappa[i-1][j]/x1_b[i]-dt*fabs(U[i][j])/(x1_b[i]*dx1))*fabs(U[i][j])*flux_limiter/2);
#endif
	/* Second coordinate */
	V_plus = fmax(V[i][j],0.0); // max{V_{i,j-1/2},0.0} LHS boundary
	V_minus = fmin(V[i][j+1],0.0); // min{V_{i,j+1/2},0.0} RHS boundary
	/*Fluctuations: B^+ \Delta Q_{i,j-1/2} + B^- \Delta Q_{i,j+1/2} */
	//net_fluctuation[i][j] += dt/(kappa[i][j]*dx2)*(V_plus*(Q[i][j] - Q[i][j-1]) + V_minus*(Q[i][j+1] - Q[i][j]));	
	/* Fluxes: G_i,j+1/2 - G_i,j-1/2 */
	net_fluctuation[i][j] += dt/(kappa[i][j]*dx2)*((fmax(V[i][j+1],0.0)*Q[i][j] + V_minus*Q[i][j+1])-(V_plus*Q[i][j-1] + fmin(V[i][j],0.0)*Q[i][j]));
#ifdef SECOND_ORDER
	/* Second order fluxes */
	if (V[i][j+1] > 0.0){
	  qmu[0] = Q[i][j-1];  //points to the two preceeding bins; 
	  qmu[1] = Q[i][j];  
	  qmu[2] = Q[i][j+1];  
	  flux_limiter= flux_PLM(dx2,qmu);
	}
	else{
	  qmu[0] = Q[i][j+2]; //centered around current bin
	  qmu[1] = Q[i][j+1];  
	  qmu[2] = Q[i][j];  
	  flux_limiter= flux_PLM(dx2,qmu);
	  /*	  if (flux_limiter != 0.0){
	    printf("i,j: %d,%d F_{i+1/2} flux limiter: %lf\n",i,j,flux_limiter);
	    printf("Q0 = %lf Q1= %lf Q2 = %lf\n",qmu[0],qmu[1],qmu[2]); } */
	}
	//G^H_{i,j+1/2}
	net_fluctuation[i][j] -= dt/(kappa[i][j]*dx2)*((1-dt*fabs(V[i][j+1])/(kappa[i][j]*dx2))*fabs(V[i][j+1])*flux_limiter/2);
	//net_fluctuation[i][j] -= dt/(kappa[i][j]*dx2)*((1-dt*fabs(V[i][j+1])/(dx2))*fabs(V[i][j+1])*flux_limiter/2);
	if (V[i][j] > 0.0){
	  qmu[0] = Q[i][j-2];  //points to the two preceeding bins; 
	  qmu[1] = Q[i][j-1];  
	  qmu[2] = Q[i][j];  
	  flux_limiter = flux_PLM(dx2,qmu);
	}
	else{
	  qmu[0] = Q[i][j+1]; //centered around current bin
	  qmu[1] = Q[i][j];  
	  qmu[2] = Q[i][j-1];  
	  flux_limiter= flux_PLM(dx2,qmu);
	}
	//G^H_{i,j-1/2}
	net_fluctuation[i][j] += dt/(kappa[i][j]*dx2)*((1-dt*fabs(V[i][j])/(kappa[i][j]*dx2))*fabs(V[i][j])*flux_limiter/2);
	//net_fluctuation[i][j] += dt/(kappa[i][j]*dx2)*((1-dt*fabs(V[i][j])/(dx2))*fabs(V[i][j])*flux_limiter/2);
#endif
      }
    }

    /*Apply fluctuations */
    for (i=is; i<ie; i++)
      for (j=js; j<je; j++){
	Q[i][j] -= net_fluctuation[i][j];
      }
    
    /*Source terms */
    for (i=is; i<ie; i++)
      for (j=js; j<je; j++){
      }

    /*Output */
    //for now, explicitly copy subarray corresponding to real zonal info:
    index=0; 
    for (k=0; k<1; k++){
      for (j=js; j<je; j++){
	for (i=is; i<ie; i++){
	  //index =(j-num_ghost)*nx2_r + (i-num_ghost); 
	  realQ[index] = (float) Q[i][j];//*kappa[i][j]; //\bar{q}=qk density in computational space
	  index++;
	}
      }
    }
    //debug only horizontal flow
    /*    if (find_max(realQ,nx1_r*nx2_r) > 1.1){
      printf("Q greater than 1.0!\n"); 
      for (i=1;i<nx1; i++){
	for (j=1; j<nx2; j++){
	  if (Q[i][j] > 1.0){
	    printf("i=%d j=%d Q[i][j] = %0.10lf\n",i,j,Q[i][j]); 
	    return(0); 
	  }
	}
      }
      }*/

    sprintf(filename,"advect-%.3d.vtk",n); 
    if(!OUTPUT_INTERVAL){
      if (n==nsteps-1) //for only the final result
	write_curvilinear_mesh(filename,3,dims, pts, nvars,vardims, centering, varnames, vars);}
    else{
      if (!(n%OUTPUT_INTERVAL)) //HAVENT CHECKED THIS
	write_curvilinear_mesh(filename,3,dims, pts, nvars,vardims, centering, varnames, vars);}
      
      printf("step: %d time: %lf max{Q} = %0.7lf min{Q} = %0.7lf sum{Q} = %0.7lf \n",
	     n+1,(n+1)*dt,find_max(realQ,nx1_r*nx2_r),find_min(realQ,nx1_r*nx2_r),sum(realQ,nx1_r*nx2_r));
  }
  return(0); 
}
Пример #29
0
void Project::MatchSubCamera(SubDomain *targetSub)
{
	// Pick two nodes from the subdomain
	Node aSub, bSub, aFull, bFull;
	aSub.nodeNumber = 0;
	bSub.nodeNumber = 0;
	aFull.nodeNumber = 0;
	bFull.nodeNumber = 0;
	if (targetSub)
	{
		Fort14 *subFort = targetSub->GetFort14();
		if (subFort)
		{
			Node nodeA = subFort->GetNode(1);
			Node nodeB = subFort->GetNode(2);

			if (nodeA.nodeNumber)
				aSub = nodeA;
			if (nodeB.nodeNumber)
				bSub = nodeB;
		}
	}

	// Find their node numbers in the full domain
	if (aSub.nodeNumber && bSub.nodeNumber)
	{
		Fort14 *fullFort = fullDomain->GetFort14();
		Py140 *subPy = targetSub->GetPy140();
		if (fullFort && subPy)
		{
			Node nodeA = fullFort->GetNode(subPy->ConvertNewToOld(aSub.nodeNumber));
			Node nodeB = fullFort->GetNode(subPy->ConvertNewToOld(bSub.nodeNumber));

			if (nodeA.nodeNumber)
				aFull = nodeA;
			if (nodeB.nodeNumber)
				bFull = nodeB;
		}
	}

	// Create the scaling factor
	float subDist = aSub.normX - bSub.normX;
	float fullDist = aFull.normX - bFull.normX;
	float scaling = fullDist/subDist;

	// Create the offset factors
	float subMaxX = targetSub->GetFort14()->GetMaxX();
	float subMinX = targetSub->GetFort14()->GetMinX();
	float subMidX = subMinX + (subMaxX - subMinX) / 2.0;
	float subMaxY = targetSub->GetFort14()->GetMaxY();
	float subMinY = targetSub->GetFort14()->GetMinY();
	float subMidY = subMinY + (subMaxY - subMinY) / 2.0;

	float fullMaxX = fullDomain->GetFort14()->GetMaxX();
	float fullMinX = fullDomain->GetFort14()->GetMinX();
	float fullMidX = fullMinX + (fullMaxX - fullMinX) / 2.0;
	float fullMaxY = fullDomain->GetFort14()->GetMaxY();
	float fullMinY = fullDomain->GetFort14()->GetMinY();
	float fullMidY = fullMinY + (fullMaxY - fullMinY) / 2.0;
	float fullMax = fmax(fullMaxX-fullMinX, fullMaxY-fullMinY);

	float xOffset = (subMidX - fullMidX) / fullMax;
	float yOffset = (subMidY - fullMidY) / fullMax;

	// Apply the new settings
	CameraSettings selectedSettings = targetSub->GetCameraSettings();
	selectedSettings.zoomLevel /= scaling;
	selectedSettings.panX = selectedSettings.panX*scaling - xOffset;
	selectedSettings.panY = selectedSettings.panY*scaling - yOffset;
	fullDomain->SetCameraSettings(selectedSettings);
}
Пример #30
0
TwoDoubles f19(double* x) {
    /* F8F2 sum of Griewank-Rosenbrock 2-D blocks*/
    int i, j, rseed; /*Loop over dim*/
    static unsigned int funcId = 19;
    double scales, F2, tmp = 0., tmp2, Fadd, Fval, Ftrue = 0.;
    TwoDoubles res;

    if (!isInitDone)
    {
        rseed = funcId + 10000 * trialid;
        /*INITIALIZATION*/
        Fopt = computeFopt(funcId, trialid);
        /* computeXopt(rseed, DIM); Xopt is not used.*/
        scales = fmax(1., sqrt(DIM) / 8.);
        computeRotation(rotation, rseed, DIM);
        for (i = 0; i < DIM; i ++)
        {
            for (j = 0; j < DIM; j++)
            {
                linearTF[i][j] = scales * rotation[i][j];
            }
        }
        for (i = 0; i < DIM; i++)
        {
            Xopt[i] = 0.;
            for (j = 0; j < DIM; j++)
            {
                Xopt[i] += linearTF[j][i] * 0.5/scales/scales;
            }
        }
        isInitDone = 1;
    }
    Fadd = Fopt;
    /* BOUNDARY HANDLING*/

    /* TRANSFORMATION IN SEARCH SPACE*/
    for (i = 0; i < DIM; i++) {
        tmx[i] = 0.5;
        for (j = 0; j < DIM; j++) {
            tmx[i] += linearTF[i][j] * x[j];
        }
    }

    /* COMPUTATION core*/
    for (i = 0; i < DIM - 1; i++)
    {
        tmp2 = tmx[i] * tmx[i] -tmx[i+1];
        F2 = 100. * tmp2 * tmp2;
        tmp2 = 1 - tmx[i];
        F2 += tmp2 * tmp2;
        tmp += F2 / 4000. - cos(F2);
    }
    Ftrue = 10. + 10. * tmp / (double)(DIM - 1);

    Ftrue += Fadd;
    Fval = Ftrue; /* without noise*/

    res.Fval = Fval;
    res.Ftrue = Ftrue;
    return res;
}