Exemplo n.º 1
0
static inline void calc_coeffs4 (double *bcoeff, double *acoeff, double w0, double Dw, double gain)
{
  double G = exp10f(gain/20.f);
  // bandwidth gain is set to half gain
  double GB = exp10f(gain/40.f);
  double c0 = cosf(w0);
  double WB = tanf(Dw/2);
  double e = sqrtf((G*G-GB*GB)/(GB*GB-1.f));
  double g = powf(G, 0.25f);
  double a = powf(e, 0.25f);
#define  si1  0.382683432365089781779f;
#define  si2  0.923879532511286738483f;
  //Ba(1+i,:) = [g^2*WB^2*v, 2*g*b*si*WB, b^2*v]
  //Aa(1+i,:) = [WB^2*v, 2*a*si*WB, a^2*v]
  double Aa0 = WB*WB;
  double Aatmp = 2*a*WB;
  double Aa10 = Aatmp * si1;
  double Aa11 = Aatmp * si2;
  double as = a*a;
  double Ba0 = Aa0 * g*g;
  double Batmp = 2*g * a * WB;
  double Ba10 = Batmp * si1;
  double Ba11 = Batmp * si2;
  //  fprintf(stdout, "%f %f %f %f %f %f %f\n", Aa0, Aa10, Aa11, Ba0, Ba10, Ba11, as);
  //  double Bhat0 = Ba0/Aa0;
  //  D = A0(i)+A1(i)+A2(i)
  double D1 = Aa0+Aa11+as;
  double D0 = Aa0+Aa10+as;
  //Bhat(i,1) = (B0(i)+B1(i)+B2(i))./D
  //(B0(i)+B1(i)+B2(i))./D
  double tmp = Ba0+as;
  double x;
  double Bhat00 = (Ba10+tmp)/D0;
  double Bhat01 = (Ba11+tmp)/D1;
  x=2.f*(Ba0-as);
  double Bhat10 = x/D0;
  double Bhat11 = x/D1;
  double Bhat20 = (tmp-Ba10)/D0;
  double Bhat21 = (tmp-Ba11)/D1;
  //Ahat(i,1) = 1
  //Ahat(i,2) = 2*(A0(i)-A2(i))./D
  //Ahat(i,3) = (A0(i)-A1(i)+A2(i))./D
  x = 2.f*(Aa0-as);
  double Ahat10 = x/D0;
  double Ahat11 = x/D1;
  double Ahat20 = (Aa0-Aa10+as)/D0;
  double Ahat21 = (Aa0-Aa11+as)/D1;

  //  fprintf(stdout, "%f %f %f %f %f\n %f %f %f %f\n\n",Bhat00, Bhat01, Bhat10, Bhat11, Bhat21, Ahat11, Ahat10, Ahat20, Ahat21);


  // B(i,2) = c0*(Bhat(i,2)-2*Bhat(i,1))
  bcoeff[0] = Bhat00;
  bcoeff[5] = Bhat01;
  bcoeff[1] = c0*(Bhat10-2.f*Bhat00);
  bcoeff[6] = c0*(Bhat11-2.f*Bhat01);
  bcoeff[2] = (Bhat00-Bhat10+Bhat20)*c0*c0- Bhat10;
  bcoeff[7] = (Bhat01-Bhat11+Bhat21)*c0*c0 - Bhat11;
  bcoeff[3] = c0*(Bhat10-2.f*Bhat20);
  bcoeff[8] = c0*(Bhat11-2.f*Bhat21);
  bcoeff[4] = Bhat20;
  bcoeff[9] = Bhat21;

  //  fprintf(stdout, "%f %f %f %f %f\n %f %f %f %f %f\n", Bres00, Bres10, Bres20, Bres30, Bres40, Bres01, Bres11, Bres21, Bres31, Bres41);

  acoeff[0]= c0*(Ahat10-2.f);
  acoeff[4]= c0*(Ahat11-2.f);
  acoeff[1]= (1.f-Ahat10+Ahat20)*c0*c0 - Ahat10;
  acoeff[5]= (1.f-Ahat11+Ahat21)*c0*c0- Ahat11;
  acoeff[2]= c0*(Ahat10-2.f*Ahat20);
  acoeff[6]= c0*(Ahat11-2.f*Ahat21);
  acoeff[3]= Ahat20;
  acoeff[7]= Ahat21;

  //  fprintf(stdout, "%f %f %f %f %f\n %f %f %f %f %f\n", Ares00, Ares10, Ares20, Ares30, Ares40, Ares01, Ares11, Ares21, Ares31, Ares41);
}
Exemplo n.º 2
0
static void sf_cram_point3_fill_abins (sf_cram_point3 cram_point, float smp, 
                                       float ss, float sr, int ies, int ier,
                                       float oac, float ozc, float dac, float dzc,
                                       bool zoffset) {
    float dtw = DAM_TP*SF_PI/180.0,
          otw = OAM_TP*SF_PI/180.0; /* Tapering width in degrees */
    int dnw = 2*(int)(dtw/cram_point->db + 0.5), onw = (int)(otw/cram_point->db + 0.5); /* Taper width in samples */
    int isxy, jsxy, ihxy, jhxy, ia, iz, ida, ioa, ioz, idz;
    int ioaf, ioal, iozf, iozl, idaf, idal, idzf, idzl;
    float doa = 0.0, doz = 0.0, dda = 0.0, ddz = 0.0;
    float oa, da, oz, dz;
    float ds, dh, sa, sb, ha, hb, dw, tw = 1.0;
    bool flip = false;

    /* Integral weight */
    if (cram_point->amp) {
        /* See Bleistein et al (2003), equation (21) for details */
        if (cram_point->erefl) {
            dw = sqrtf (ss);
            dw /= sqrtf (fabsf (cram_point->src_exits[ies].j));
            dw *= cram_point->src_exits[ies].cs;
            sb = sinf (cram_point->b0 + cram_point->src_exits[ies].ib*cram_point->db);
            dw *= sqrtf (fabsf (sb));
            smp *= dw;
        } else {
            dw = sqrtf (ss*sr);
            dw *= cosf (0.5*oac);
            dw /= sqrtf (fabsf (cram_point->src_exits[ies].j*cram_point->rcv_exits[ier].j));
            dw *= cram_point->src_exits[ies].cs*cram_point->rcv_exits[ier].cs;
            sb = sinf (cram_point->b0 + cram_point->src_exits[ies].ib*cram_point->db);
            hb = zoffset ? sb : sinf (cram_point->b0 + cram_point->rcv_exits[ier].ib*cram_point->db);
            dw *= sqrtf (fabsf (sb*hb));
            smp *= dw;
        }
    }

    /* Compute tapeting weights at the edges of the mute zone */
    dw = cram_point->oam - fabsf (oac);
    if (dw < 2.0*otw) {
        if (dw < otw)
            tw = 0.0;
        else
            tw *= (dw - otw)/otw;
    }
    dw = cram_point->dam - fabsf (dac);
    if (dw < 2.0*dtw) {
        if (dw < dtw)
            tw = 0.0;
        else
            tw *= (dw - dtw)/dtw;
    }

    /* Contribute sample to the stacked image */
    cram_point->img += tw*smp;
    cram_point->hits += 1.0;

    if (!cram_point->agath && !cram_point->dipgath)
        return;

    if (cram_point->extrap && !zoffset) {
        /* Compute change in inclination and azimuth angles for the source and receiver branches
           using db/dx,db/dy and da/dx,da/dy with respect to a displacement away from the
           source and receiver on the surface; then find dip and scattering angle deviation,
           which correspond to this displacement */
        for (isxy = 0; isxy < 2; isxy++) { /* d/dx, d/dy, source side */
            for (jsxy = 0; jsxy < 2; jsxy++) { /* +/- shift in x/y on the source side */
                ds = jsxy != 0 ? cram_point->ds : -cram_point->ds;
                sb = cram_point->b0 +
                     (cram_point->src_exits[ies].ib + cram_point->src_exits[ies].ibxy[isxy]*ds)*
                     cram_point->db;
                sa = cram_point->a0 +
                     (cram_point->src_exits[ies].ia + cram_point->src_exits[ies].iaxy[isxy]*ds)*
                     cram_point->da;
                for (ihxy = 0; ihxy < 2; ihxy++) { /* d/dx, d/dy, receiver side */
                    for (jhxy = 0; jhxy < 2; jhxy++) { /* +/- shift in x/y on the receiver side */
                        dh = jhxy != 0 ? cram_point->ds : -cram_point->ds;
                        hb = cram_point->b0 +
                             (cram_point->rcv_exits[ier].ib + cram_point->rcv_exits[ier].ibxy[ihxy]*dh)*
                             cram_point->db;
                        ha = cram_point->a0 +
                             (cram_point->rcv_exits[ier].ia + cram_point->rcv_exits[ier].iaxy[ihxy]*dh)*
                             cram_point->da;
                        /* New system of subsurface angles for the surface displacment */
                        sf_cram_point3_angles (sb, sa, hb, ha, &oa, &oz, &da, &dz);
                        /* Scattering angle spread with respect to the initial values */
                        sf_cram_point3_aaz_spread (oac, ozc, oa, oz, &doa, &doz);
                        /* Dip angle spread with respect to the initial values */
                        sf_cram_point3_aaz_spread (dac, dzc, da, dz, &dda, &ddz);
                    }
                }
            }
        }
        /* Average changes in dip and scattering angles and their azimuths */
        doa /= 16.0;
        doz /= 16.0;
        dda /= 16.0;
        ddz /= 16.0;
        /* Extrapolate azimuth further away */
        doa *= 0.75;
        /* Extrapolate dip only in the vicinity of a receiver */
        dw = cram_point->dh/cram_point->ds;
        dda *= 2.0*dw;
        ddz *= 2.0*dw;
    } else {
        doa = 0.5*cram_point->db;
        doz = cram_point->da;
        dda = 0.25*cram_point->db;
        ddz = cram_point->da;
    }

    /* Contribute sample to the scattering angle gather */
    if (cram_point->agath) {
        /* Scattering angle first index */
        ioaf = sf_cram_point3_angle_idx (oac - doa, -SF_PI, cram_point->db, NULL);
        if (ioaf < (cram_point->nb - cram_point->mioz))
            ioaf = cram_point->nb - cram_point->mioz;
        /* Scattering angle last index */
        ioal = sf_cram_point3_angle_idx (oac + doa, -SF_PI, cram_point->db, NULL);
        ioal += 1;
        if (ioal > (cram_point->nb + cram_point->mioz))
            ioal = cram_point->nb + cram_point->mioz;
        if (ioal >= 2*cram_point->nb)
            ioal = 2*cram_point->nb - 1;
        /* Scattering angle azimuth first index */
        iozf = sf_cram_point3_angle_idx (ozc - doz, 0.0, 2.0*cram_point->da, NULL);
        /* Scattering angle azimuth last index */
        iozl = sf_cram_point3_angle_idx (ozc + doz, 0.0, 2.0*cram_point->da, NULL);
        iozl += 1;
        dw = 1.0/*((ioal - ioaf + 1)*(iozl - iozf + 1))*/;
        for (ioz = iozf; ioz <= iozl; ioz++) { /* Scattering angle azimuths */
            iz = ioz;
            if (iz < 0) {
                while (iz < 0)
                    iz += cram_point->na/4;
                flip = true;
            } else if (iz >= cram_point->na/4) {
                while (iz >= cram_point->na/4)
                    iz -= cram_point->na/4;
                flip = true;
            }
            for (ioa = ioaf; ioa <= ioal; ioa++) { /* Scattering angles */
                tw = dw;
                ia = ioa;
                if (flip && ia != 0) {
                    ia = 2*cram_point->nb - ia;
                }
                /* Tapering at the edges */
                if ((cram_point->mioz - abs (ia - cram_point->nb)) < onw) {
                    tw *= (cram_point->mioz - abs (ia - cram_point->nb))/(float)onw;
                }
                cram_point->oimage[iz][ia] += tw*smp;
                cram_point->osqimg[iz][ia] += (tw*smp)*(tw*smp);
                cram_point->ohits[iz][ia] += 1.0;
            }
            flip = false;
        }
    }

    /* Contribute sample to the dip angle gather */
    if (cram_point->dipgath) {
        /* Dip angle first index */
        idaf = sf_cram_point3_angle_idx (dac - dda, -SF_PI, 0.5*cram_point->db, NULL);
        if (idaf < (2*cram_point->nb - cram_point->midz))
            idaf = 2*cram_point->nb - cram_point->midz;
        /* Dip angle last index */
        idal = sf_cram_point3_angle_idx (dac + dda, -SF_PI, 0.5*cram_point->db, NULL);
        idal += 1;
        if (idal > (2*cram_point->nb + cram_point->midz))
            idal = 2*cram_point->nb + cram_point->midz;
        if (idal >= 4*cram_point->nb)
            idal = 4*cram_point->nb - 1;
        /* Dip angle azimuth first index */
        idzf = sf_cram_point3_angle_idx (dzc - ddz, 0.0, 2.0*cram_point->da, NULL);
        /* Dip angle azimuth last index */
        idzl = sf_cram_point3_angle_idx (dzc + ddz, 0.0, 2.0*cram_point->da, NULL);
        idzl += 1;
        if (cram_point->amp)
            dw = 1.0/(sinf (0.5*cram_point->oam)*sinf (0.5*cram_point->oam));
        else
            dw = 1.0;
        for (idz = idzf; idz <= idzl; idz++) { /* Dip angle azimuths */
            iz = idz;
            if (iz < 0) {
                while (iz < 0)
                    iz += cram_point->na/4;
                flip = true;
            } else if (iz >= cram_point->na/4) {
                while (iz >= cram_point->na/4)
                    iz -= cram_point->na/4;
                flip = true;
            }
            for (ida = idaf; ida <= idal; ida++) { /* Dip angles */
                tw = dw;
                ia = ida;
                if (flip && ia != 0) {
                    ia = 4*cram_point->nb - ia;
                }
                /* Tapering at the edges */
                if ((cram_point->midz - abs (ia - 2*cram_point->nb)) < dnw) {
                    tw *= (cram_point->midz - abs (ia - 2*cram_point->nb))/(float)dnw;
                }
                cram_point->dimage[iz][ia] += tw*smp;
                cram_point->dsqimg[iz][ia] += (tw*smp)*(tw*smp);
                cram_point->dhits[iz][ia] += 1.0;
            }
            flip = false;
        }
    }
}
Exemplo n.º 3
0
Arquivo: ex14.c Projeto: spetz911/vog
GLUSboolean update(GLUSfloat time)
{
    static GLfloat angle = 0.0f;

    GLuint primitivesWritten;

    // Field of view

    GLfloat rotationMatrix[16];

    GLfloat positionTextureSpace[4];
    GLfloat directionTextureSpace[3];
    GLfloat leftNormalTextureSpace[3];
    GLfloat rightNormalTextureSpace[3];
    GLfloat backNormalTextureSpace[3];

    GLfloat xzPosition2D[4];

    //

    GLfloat tmvpMatrix[16];

    // Animation update

    g_personView.cameraPosition[0] = -cosf(2.0f * PIf * angle / TURN_DURATION) * TURN_RADIUS * METERS_TO_VIRTUAL_WORLD_SCALE;
    g_personView.cameraPosition[2] = -sinf(2.0f * PIf * angle / TURN_DURATION) * TURN_RADIUS * METERS_TO_VIRTUAL_WORLD_SCALE;

    g_personView.cameraDirection[0] = sinf(2.0f * PIf * angle / TURN_DURATION);
    g_personView.cameraDirection[2] = -cosf(2.0f * PIf * angle / TURN_DURATION);

    if (g_animationOn)
    {
        angle += time;
    }

    glusLookAtf(g_viewMatrix, g_activeView->cameraPosition[0], g_activeView->cameraPosition[1], g_activeView->cameraPosition[2], g_activeView->cameraPosition[0] + g_activeView->cameraDirection[0], g_activeView->cameraPosition[1] + g_activeView->cameraDirection[1],
            g_activeView->cameraPosition[2] + g_activeView->cameraDirection[2], g_activeView->cameraUp[0], g_activeView->cameraUp[1], g_activeView->cameraUp[2]);

    glusMatrix4x4Identityf(tmvpMatrix);
    glusMatrix4x4Multiplyf(tmvpMatrix, tmvpMatrix, g_projectionMatrix);
    glusMatrix4x4Multiplyf(tmvpMatrix, tmvpMatrix, g_viewMatrix);
    glusMatrix4x4Multiplyf(tmvpMatrix, tmvpMatrix, g_textureToWorldMatrix);

    // Position

    xzPosition2D[0] = g_personView.cameraPosition[0];
    xzPosition2D[1] = 0.0f;
    xzPosition2D[2] = g_personView.cameraPosition[2];
    xzPosition2D[3] = g_personView.cameraPosition[3];

    glusMatrix4x4MultiplyPoint4f(positionTextureSpace, g_worldToTextureMatrix, xzPosition2D);

    // Direction

    glusMatrix4x4MultiplyVector3f(directionTextureSpace, g_worldToTextureMatrix, g_personView.cameraDirection);

    // Left normal of field of view

    glusMatrix4x4Identityf(rotationMatrix);
    glusMatrix4x4RotateRyf(rotationMatrix, g_personView.fov * (g_width / g_height) / 2.0f + 90.0f);
    glusMatrix4x4MultiplyVector3f(leftNormalTextureSpace, rotationMatrix, g_personView.cameraDirection);
    glusMatrix4x4MultiplyVector3f(leftNormalTextureSpace, g_worldToTextureNormalMatrix, leftNormalTextureSpace);

    // Right normal of field of view

    glusMatrix4x4Identityf(rotationMatrix);
    glusMatrix4x4RotateRyf(rotationMatrix, -g_personView.fov * (g_width / g_height) / 2.0f - 90.0f);
    glusMatrix4x4MultiplyVector3f(rightNormalTextureSpace, rotationMatrix, g_personView.cameraDirection);
    glusMatrix4x4MultiplyVector3f(rightNormalTextureSpace, g_worldToTextureNormalMatrix, rightNormalTextureSpace);

    // Back normal of field of view

    glusMatrix4x4Identityf(rotationMatrix);
    glusMatrix4x4RotateRyf(rotationMatrix, 180.0f);
    glusMatrix4x4MultiplyVector3f(backNormalTextureSpace, rotationMatrix, g_personView.cameraDirection);
    glusMatrix4x4MultiplyVector3f(backNormalTextureSpace, g_worldToTextureNormalMatrix, backNormalTextureSpace);

    // OpenGL stuff

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    // Pass one.

    // Disable any rasterization
    glEnable(GL_RASTERIZER_DISCARD);

    glUseProgram(g_programPassOne.program);

    glUniform4fv(g_positionTextureSpacePassOneLocation, 1, positionTextureSpace);
    glUniform3fv(g_leftNormalTextureSpacePassOneLocation, 1, leftNormalTextureSpace);
    glUniform3fv(g_rightNormalTextureSpacePassOneLocation, 1, rightNormalTextureSpace);
    glUniform3fv(g_backNormalTextureSpacePassOneLocation, 1, backNormalTextureSpace);

    glBindVertexArray(g_vaoPassOne);

    // Bind to vertices used in render pass two. To this buffer is written.
    glBindBufferBase(GL_TRANSFORM_FEEDBACK_BUFFER, 0, g_verticesPassTwoVBO);

    // We need to know, how many primitives are written. So start the query.
    glBeginQuery(GL_TRANSFORM_FEEDBACK_PRIMITIVES_WRITTEN, g_transformFeedbackQuery);

    // Start the operation ...
    glBeginTransformFeedback(GL_POINTS);

    // ... render the elements ...
    glDrawElements(GL_POINTS, g_sNumPoints * g_tNumPoints, GL_UNSIGNED_INT, 0);

    // ... and stop the operation.
    glEndTransformFeedback();

    // Now, we can also stop the query.
    glEndQuery(GL_TRANSFORM_FEEDBACK_PRIMITIVES_WRITTEN);

    glDisable(GL_RASTERIZER_DISCARD);

    glBindBufferBase(GL_TRANSFORM_FEEDBACK_BUFFER, 0, 0);

    glBindVertexArray(0);

    // Pass two

    glUseProgram(g_shaderProgramPassTwo.program);

    glUniformMatrix4fv(g_tmvpPassTwoLocation, 1, GL_FALSE, tmvpMatrix);
    glUniform4fv(g_positionTextureSpacePassTwoLocation, 1, positionTextureSpace);

    glBindVertexArray(g_vaoPassTwo);

    // Now get the number of primitives written in the first render pass.
    glGetQueryObjectuiv(g_transformFeedbackQuery, GL_QUERY_RESULT, &primitivesWritten);

    // No draw the final terrain.
    glDrawArrays(GL_PATCHES, 0, primitivesWritten);

    return GLUS_TRUE;
}
Exemplo n.º 4
0
// *****************************************************************
// 
// *****************************************************************
void Run_Algo( TGyro *gyro ) 
{
    float cos_freq, sc_argument, local_var;
    Sint8 iCounter;

    // --- 1 ------ НАСРОЙКА ПРИБОРА -----
    if ( gyro->DebugMode != 0 ) 
    {
        SetupAlgo( gyro );

        gyro->ExcitationPhaseAG = gyro->Base_ExcitationPhaseAG +     gyro->c5;
        gyro->ExcitationPhase_x = gyro->Base_ExcitationPhase_x + 2.0*gyro->c6;
        gyro->ExcitationPhase_y = gyro->Base_ExcitationPhase_y + 2.0*gyro->c7;

        //if ( gyro->ExcitationPhaseAG < (float)0.0 ) gyro->ExcitationPhaseAG = (float)0.0;
        //if ( gyro->ExcitationPhase_x < (float)0.0 ) gyro->ExcitationPhase_x = (float)0.0;
        //if ( gyro->ExcitationPhase_y < (float)0.0 ) gyro->ExcitationPhase_x = (float)0.0;

		// ---- debug --- Для определения полосы пропускания -----
	    Debug.TempFrequency = ALGO_FREQUENCY / (Debug.freq_incr * TWO_PI_INVERT * ALGO_FREQUENCY);
	    if ( Debug.TempFrequency == 0 ) Debug.TempFrequency = 1;
	    Calc_Phase( &Debug.Phase, Debug.TempFrequency, gyro->c2, - gyro->pi_y_2[0] );
    }

   // --- 2 ------ ДАННЫЕ В UART -------
    Setup_UART_data( gyro );

   // --- 3 ------ Фильтрация входных сигналов ------
    Run_BandPassFiltration_new( gyro->fi_input_x, gyro->in_Ux );
    Run_BandPassFiltration_new( gyro->fi_input_y, gyro->in_Uy );

    // ---- Фаза между X, Y ----
    if ( gyro->DebugMode <= 4 ) 
        Calc_Phase( &gyro->Phase, gyro->rmnk_x.NCycles, gyro->fi_input_x[0], gyro->fi_input_y[0] );

    // --- 4 ------ RMNK ------ (R-рекуррентный M-метод N-наименьших K-квадратов)
    Run_Demod_RMNK( &gyro->rmnk_x, gyro->fi_input_x[0] );
    Run_Demod_RMNK( &gyro->rmnk_y, gyro->fi_input_y[0] );
    gyro->DeltaFrequency = gyro->rmnk_x.Frequency - gyro->rmnk_y.Frequency;

  // --- 7 ----- Регулирование амплитуды колебаний Канала Х,Y ----
    Run_PiReg_new( gyro->pi_x_1,  gyro->rmnk_x.fi_a[0] );  
    Run_PiReg_new( gyro->pi_x_2, -gyro->pi_x_1[0]  );
    
    Run_PiReg_new( gyro->pi_y_1,  gyro->rmnk_y.fi_a[0] );  
	Run_PiReg_new( gyro->pi_y_2, -gyro->pi_y_1[0]  );

   // --- 9 -------- Коррекция амплитуд -----
    if ( gyro->pi_x_2[0] > (float) 9.9 ) gyro->pi_x_2[0] = (float) 9.9;
    if ( gyro->pi_x_2[0] < (float)-9.9 ) gyro->pi_x_2[0] = (float)-9.9;
    if ( gyro->pi_y_2[0] > (float) 9.9 ) gyro->pi_y_2[0] = (float) 9.9;
    if ( gyro->pi_y_2[0] < (float)-9.9 ) gyro->pi_y_2[0] = (float)-9.9;

  // --- 8 ------ Если автогениратор отключился 
  // ---   ------ тогда переходим на компенсирование узла
  	
    //if ( gyro->fi_input_y[0] == 0 && gyro->fi_input_x[0] == 0 )
    //     gyro->NULL_value = 0.0;    
    //else gyro->NULL_value = atan2f ( gyro->fi_input_y[0], gyro->fi_input_x[0]) * RAD_TO_DEG; 
    
	if ( gyro->isAlgoCounter >= 3 )     
    { 
        //if ( gyro->isAlgoCounter >= 4 ) gyro->BaseAntinodePhase += ( 360.0 * DISCRET_TIME * 0.05 );
        if ( gyro->BaseAntinodePhase >= 360.0 ) gyro->BaseAntinodePhase -= 360;
        
        Run_LowPass_IRFiltr_1rstOrder ( gyro->fi_Phase, gyro->Phase.Phase_rad );
        
        Run_PiReg_new( gyro->pi_phi_1, - gyro->Phase.Phase_rad + 0.970*ONE_PI*sinf(gyro->BaseAntinodePhase*DEG_TO_RAD));
        Run_PiReg_new( gyro->pi_phi_2, - gyro->pi_phi_1[0] );
    }
        	
    
    
    
/*    
	if ( gyro->isAlgoCounter >= 3 ) 
    { 
        if ( gyro->isAlgoCounter >= 4 ) 
            gyro->BaseAntinodePhase += ( 360.0 * DISCRET_TIME * 0.05 );
    	if ( gyro->BaseAntinodePhase >= 360.0 ) gyro->BaseAntinodePhase -= 360;
    	
        
		if ( gyro->Phase.isNewValuePresent == 1 )
    	{
    	    if ( gyro->Phase.Phase_rad_old > 170 * DEG_TO_RAD && gyro->Phase.Phase_rad < -170 * DEG_TO_RAD)
    	    {
    	       //gyro->pi_phi_1[5] = (float)0.0;
    	       //gyro->pi_phi_1[6] = (float)0.0;
    	       //gyro->pi_phi_1[8] = (float)0.0;

    	       //gyro->pi_phi_2[5] = (float)0.0;
    	       //gyro->pi_phi_2[6] = (float)0.0;
    	       //gyro->pi_phi_2[8] = (float)0.0;
    	    }
    	    
	    	gyro->Phase.isNewValuePresent = 0;
	    	//Run_LowPass_IRFiltr_1rstOrder( gyro->fi_Phase, gyro->Phase.Phase_rad - gyro->BaseAntinodePhase*DEG_TO_RAD);
	    	
	    	Run_LowPass_IRFiltr_1rstOrder ( gyro->fi_Phase, gyro->Phase.Phase_rad - ONE_PI*sinf(gyro->BaseAntinodePhase*DEG_TO_RAD));
	        //Run_PiReg_new( gyro->pi_phi_1, -gyro->fi_Phase[0] );  
		    //Run_PiReg_new( gyro->pi_phi_2, -gyro->pi_phi_1[0] );
		    //gyro->pi_phi_2[0] = gyro->pi_phi_1[0];
    	}
    }
*/
   // --- 11 ----------- Управляющие сигналы --------------------------
    gyro->out_Ux = - (gyro->pi_x_2  [0] + gyro->c1 ) * cosf( gyro->rmnk_x.phi + gyro->ExcitationPhase_x )
                   -  gyro->pi_phi_2[0] * 1.0        * sinf( gyro->rmnk_x.phi + gyro->ExcitationPhase_x );
                   
    gyro->out_Uy = - (gyro->pi_y_2  [0] + gyro->c2 ) * cosf( gyro->rmnk_y.phi + gyro->ExcitationPhase_y )
                   +  gyro->pi_phi_2[0] * 1.0        * sinf( gyro->rmnk_y.phi + gyro->ExcitationPhase_y ); 

                
    //gyro->out_Uy = (float)0.0;
    gyro->GyroOut    = (float)0.5 * (gyro->pi_x_2[0] - gyro->pi_y_2[0]);
    gyro->Quadrature = (float)0.5 * (gyro->pi_x_2[0] + gyro->pi_y_2[0]);

    // ----- Если сканироваение Стартовой фазы, зануляю УЗЕЛ ------
    if ( gyro->DebugMode == 5 ) gyro->out_Uy = 0;
    if ( gyro->DebugMode == 6 ) gyro->out_Uy = 0;
    if ( gyro->DebugMode == 7 ) gyro->out_Ux = 0;

    // --- 12 ----------- Счетчик и флаг запуска 0.4 сек --------
    gyro->AlgoCounter++;
    if ( gyro->AlgoCounter >=  gyro->StartUpTime  ) gyro->isAlgoCounter = 2;
    if ( gyro->AlgoCounter >=  50000              ) gyro->isAlgoCounter = 3;
    if ( gyro->AlgoCounter >= 500000              ) gyro->isAlgoCounter = 4;
    
   // ******************************************
   // *** 13 **** Разгон(Старт) Гирика *********
   // ******************************************
    Calc_Period( &gyro->rmnk_x.ANodePeriod, gyro->fi_input_x[0] );
    if ( gyro->isAlgoCounter < 2 || gyro->DebugMode == 5 ) 
    {
        // --- Расчет простого периода колебаний системы
        // --- и перенос начального значения инкримента фазы в RMNK ---
        gyro->rmnk_x.delta_phi = TWO_PI / gyro->rmnk_x.ANodePeriod.Period;
        gyro->rmnk_y.delta_phi = gyro->rmnk_x.delta_phi;

        // -- ФАЗА (Задержка) АГ (перасчет для режима сканирование) ----
        if ( gyro->DebugMode == 5 ) 
        {
            gyro->Zn_exc.zn_curr = gyro->DefaultPeriod * gyro->ExcitationPhaseAG * TWO_PI_INVERT;
            Setup_Zn(&gyro->Zn_exc);
        }

        // ---- Скользящий буфера -----
    	memmove(&gyro->XBuffer[1], gyro->XBuffer, 100); 
    	gyro->XBuffer[0] = - gyro->fi_input_x[0];
        
        local_var    = Get_SplineValue( gyro->XBuffer, &gyro->Zn_exc);
        gyro->out_Ux = ( local_var >= (float)0.0 ) ? gyro->pi_x_2[0] : -gyro->pi_x_2[0];
        gyro->out_Uy = (float)0.0;
    }


   // ******************************************
   // ***  14  *****   Отладка *****************
   // ******************************************
    if ( gyro->DebugMode == 0 ) 
    {   // ------ Debug -----
        if ( Debug.init_counter++ >= Debug.init_count ) Debug.isInited = 1;
        if ( Debug.isWork == 1 && Debug.isInited == 1 )
        {
            Debug.param1[ Debug.work_counter ] = *Debug.debug_value;

            if ( ++Debug.work_counter >= Debug.work_count )
            {
                Debug.isSleep      = 1;
                Debug.isWork       = 0;
                Debug.work_counter = 0;
            }
        }
    }
} 
Exemplo n.º 5
0
Arquivo: poi.C Projeto: wixor/ewo
Array2D<float> evaluateImage(const Image &src, const std::vector<float> &scales, int steps)
{
    int border = 2.5f * (*std::max_element(scales.begin(), scales.end()));
    int w = src.getWidth(), h = src.getHeight();

    progress(0);

    Array2D<float> globalProfile[steps];
    for(int i=0; i<steps; i++) {
        globalProfile[i] = Array2D<float>(w,h);
        globalProfile[i].fill(1.f);
    }

    {
        Array2D<float> currentProfile[steps];
        for(int i=0; i<steps; i++)
            currentProfile[i] = Array2D<float>(w,h);
        
        PrefixSums ps(src);

        DifferenceJob jobs[steps];
        Completion c;

        float progress_step = 1.f/(steps * scales.size());
        float progress_done = 0.f;

        for(int i=0; i<steps; i++) {
            jobs[i].src = &ps;
            jobs[i].dst = &currentProfile[i];
            jobs[i].x1 = border;
            jobs[i].y1 = border;
            jobs[i].x2 = w-border;
            jobs[i].y2 = h-border;
            jobs[i].progress_step = progress_step;
            jobs[i].progress_done = &progress_done;
            jobs[i].completion = &c;
        }
        
        for(int s=0; s<(int)scales.size(); s++)
        {
            for(int i=0; i<steps; i++)
            {
                float angle = 2.0f*M_PI/steps*i;
                jobs[i].scale = scales[s];
                jobs[i].dx = jobs[i].scale * cosf(angle);
                jobs[i].dy = jobs[i].scale * sinf(angle);
                aq->queue(&jobs[i]);
            }
            
            c.wait();
            
            for(int i=0; i<steps; i++)
                for(int y=0; y<h; y++)
                    for(int x=0; x<w; x++)
                        globalProfile[i][y][x] *= currentProfile[i][y][x];
        }
    }

    Array2D<float> maxs(globalProfile[0]), mins(globalProfile[0]);
    
    for(int i=1; i<steps; i++)
        for(int y=border; y<h-border; y++)
            for(int x=border; x<w-border; x++) {
                maxs[y][x] = std::max(maxs[y][x], globalProfile[i][y][x]);
                mins[y][x] = std::min(mins[y][x], globalProfile[i][y][x]);
            }

    Array2D<float> eval(w,h);
    eval.fill(0.f);
    for(int y=border; y<h-border; y++)
        for(int x=border; x<w-border; x++)
            eval[y][x] = powf(maxs[y][x] - mins[y][x], 1.0f/scales.size());
    return eval;
}
Exemplo n.º 6
0
void FlipX3D::update(float time)
{
    float angle = (float)M_PI * time; // 180 degrees
    float mz = sinf(angle);
    angle = angle / 2.0f; // x calculates degrees from 0 to 90
    float mx = cosf(angle);

    Vec3 v0, v1, v, diff;

    v0 = getOriginalVertex(Vec2(1.0f, 1.0f));
    v1 = getOriginalVertex(Vec2());

    float    x0 = v0.x;
    float    x1 = v1.x;
    float    x;
    Vec2    a, b, c, d;

    if ( x0 > x1 )
    {
        // Normal Grid
        a.setZero();
        b.set(0.0f, 1.0f);
        c.set(1.0f, 0.0f);
        d.set(1.0f, 1.0f);
        x = x0;
    }
    else
    {
        // Reversed Grid
        c.setZero();
        d.set(0.0f, 1.0f);
        a.set(1.0f, 0.0f);
        b.set(1.0f, 1.0f);
        x = x1;
    }
    
    diff.x = ( x - x * mx );
    diff.z = fabsf( floorf( (x * mz) / 4.0f ) );

    // bottom-left
    v = getOriginalVertex(a);
    v.x = diff.x;
    v.z += diff.z;
    setVertex(a, v);
    
    // upper-left
    v = getOriginalVertex(b);
    v.x = diff.x;
    v.z += diff.z;
    setVertex(b, v);
    
    // bottom-right
    v = getOriginalVertex(c);
    v.x -= diff.x;
    v.z -= diff.z;
    setVertex(c, v);
    
    // upper-right
    v = getOriginalVertex(d);
    v.x -= diff.x;
    v.z -= diff.z;
    setVertex(d, v);
}
Exemplo n.º 7
0
/// update - update circle controller
void AC_Circle::update()
{
    // calculate dt
    uint32_t now = hal.scheduler->millis();
    float dt = (now - _last_update) / 1000.0f;

    // update circle position at 10hz
    if (dt > 0.095f) {

        // double check dt is reasonable
        if (dt >= 1.0f) {
            dt = 0.0;
        }
        // capture time since last iteration
        _last_update = now;

        // ramp up angular velocity to maximum
        if (_rate >= 0) {
            if (_angular_vel < _angular_vel_max) {
                _angular_vel += _angular_accel * dt;
                _angular_vel = constrain_float(_angular_vel, 0, _angular_vel_max);
            }
        } else {
            if (_angular_vel > _angular_vel_max) {
                _angular_vel += _angular_accel * dt;
                _angular_vel = constrain_float(_angular_vel, _angular_vel_max, 0);
            }
        }

        // update the target angle and total angle traveled
        float angle_change = _angular_vel * dt;
        _angle += angle_change;
        _angle = wrap_PI(_angle);
        _angle_total += angle_change;

        // if the circle_radius is zero we are doing panorama so no need to update loiter target
        if (_radius != 0.0) {
            // calculate target position
            Vector3f target;
            target.x = _center.x + _radius * cosf(-_angle);
            target.y = _center.y - _radius * sinf(-_angle);
            target.z = _pos_control.get_alt_target();

            // update position controller target
            _pos_control.set_pos_target(target);

            // heading is 180 deg from vehicles target position around circle
            _yaw = wrap_PI(_angle-PI) * AC_CIRCLE_DEGX100;
        } else {
            // set target position to center
            Vector3f target;
            target.x = _center.x;
            target.y = _center.y;
            target.z = _pos_control.get_alt_target();

            // update position controller target
            _pos_control.set_pos_target(target);

            // heading is same as _angle but converted to centi-degrees
            _yaw = _angle * AC_CIRCLE_DEGX100;
        }

        // trigger position controller on next update
        _pos_control.trigger_xy();
    }

    // run loiter's position to velocity step
    _pos_control.update_pos_controller(false);
}
Exemplo n.º 8
0
//
/// \brief Generates geometry for a sphere.  Allocates memory for the vertex data and stores 
///        the results in the arrays.  Generate index list for a TRIANGLE_STRIP
/// \param numSlices The number of slices in the sphere
/// \param vertices If not NULL, will contain array of float3 positions
/// \param normals If not NULL, will contain array of float3 normals
/// \param texCoords If not NULL, will contain array of float2 texCoords
/// \param indices If not NULL, will contain the array of indices for the triangle strip
/// \return The number of indices required for rendering the buffers (the number of indices stored in the indices array
///         if it is not NULL ) as a GL_TRIANGLE_STRIP
//
int ESUTIL_API esGenSphere ( int numSlices, float radius, GLfloat **vertices, GLfloat **normals, 
                             GLfloat **texCoords, GLuint **indices )
{
   int i;
   int j;
   int numParallels = numSlices / 2;
   int numVertices = ( numParallels + 1 ) * ( numSlices + 1 );
   int numIndices = numParallels * numSlices * 6;
   float angleStep = (2.0f * ES_PI) / ((float) numSlices);

   // Allocate memory for buffers
   if ( vertices != NULL )
      *vertices = malloc ( sizeof(GLfloat) * 3 * numVertices );
   
   if ( normals != NULL )
      *normals = malloc ( sizeof(GLfloat) * 3 * numVertices );

   if ( texCoords != NULL )
      *texCoords = malloc ( sizeof(GLfloat) * 2 * numVertices );

   if ( indices != NULL )
      *indices = malloc ( sizeof(GLuint) * numIndices );

   for ( i = 0; i < numParallels + 1; i++ )
   {
      for ( j = 0; j < numSlices + 1; j++ )
      {
         int vertex = ( i * (numSlices + 1) + j ) * 3; 

         if ( vertices )
         {
            (*vertices)[vertex + 0] = radius * sinf ( angleStep * (float)i ) *
                                               sinf ( angleStep * (float)j );
            (*vertices)[vertex + 1] = radius * cosf ( angleStep * (float)i );
            (*vertices)[vertex + 2] = radius * sinf ( angleStep * (float)i ) *
                                               cosf ( angleStep * (float)j );
         }

         if ( normals )
         {
            (*normals)[vertex + 0] = (*vertices)[vertex + 0] / radius;
            (*normals)[vertex + 1] = (*vertices)[vertex + 1] / radius;
            (*normals)[vertex + 2] = (*vertices)[vertex + 2] / radius;
         }

         if ( texCoords )
         {
            int texIndex = ( i * (numSlices + 1) + j ) * 2;
            (*texCoords)[texIndex + 0] = (float) j / (float) numSlices;
            (*texCoords)[texIndex + 1] = ( 1.0f - (float) i ) / (float) (numParallels - 1 );
         }
      }
   }

   // Generate the indices
   if ( indices != NULL )
   {
      GLuint *indexBuf = (*indices);
      for ( i = 0; i < numParallels ; i++ ) 
      {
         for ( j = 0; j < numSlices; j++ )
         {
            *indexBuf++  = i * ( numSlices + 1 ) + j;
            *indexBuf++ = ( i + 1 ) * ( numSlices + 1 ) + j;
            *indexBuf++ = ( i + 1 ) * ( numSlices + 1 ) + ( j + 1 );

            *indexBuf++ = i * ( numSlices + 1 ) + j;
            *indexBuf++ = ( i + 1 ) * ( numSlices + 1 ) + ( j + 1 );
            *indexBuf++ = i * ( numSlices + 1 ) + ( j + 1 );
         }
      }
   }

   return numIndices;
}
Exemplo n.º 9
0
void CShapeRenderer::DrawCone(const CMat4& mat, float fButtomRadius, float fTopRadius, float fHeight, CColor coneColor, CColor bottomColor, CColor topColor, bool bSolid) const
{
    DrawCircle(mat, fButtomRadius, bottomColor, bSolid);
    CMat4 translateMat;
    translateMat.SetTranslate(CVec3(0, fHeight, 0));
    CMat4 topMat = mat * translateMat;
    DrawCircle(topMat, fTopRadius, topColor, bSolid);
    if (bSolid)
    {
        CVec3 center(mat[12], mat[13], mat[14]);
        CVec3 upDirection = mat.GetUpVec3();
        upDirection = upDirection * fHeight;
        CVec3 topCenter = center + upDirection;
        CVertexPC point;
        point.position = topCenter;
        point.color = coneColor;
        std::vector<unsigned short> indicesData;
        std::vector<CVertexPC> vertexData;
        vertexData.push_back(point);
        static const float fStepRadians = DegreesToRadians(15);
        for (float fRadian = 0; fRadian <= MATH_PI_DOUBLE; fRadian += fStepRadians)
        {
            CVec3 pos(fButtomRadius * sinf(fRadian), 0, fButtomRadius * cosf(fRadian));
            pos *= mat;
            point.position = pos;
            vertexData.push_back(point);
            if (vertexData.size() >= 3)
            {
                unsigned short index = (unsigned short)vertexData.size();
                indicesData.push_back(0);
                indicesData.push_back(index - 2);
                indicesData.push_back(index - 1);
            }
        }
        CRenderManager::GetInstance()->RenderTriangle(vertexData, indicesData, true);
    }
    else
    {
        CVertexPC startPos, endPos;
        startPos.color = coneColor;
        endPos.color = coneColor;
        CVec3 topPoint(fTopRadius, fHeight, 0);
        CVec3 buttomPoint(fButtomRadius, 0, 0);
        startPos.position = topPoint * mat;
        endPos.position = buttomPoint * mat;
        CRenderManager::GetInstance()->RenderLine(startPos, endPos, 1.0f, true);
        topPoint.X() *= -1;
        buttomPoint.X() *= -1;
        startPos.position = topPoint * mat;
        endPos.position = buttomPoint * mat;
        CRenderManager::GetInstance()->RenderLine(startPos, endPos, 1.0f, true);
        topPoint = CVec3(0, fHeight, fTopRadius);
        buttomPoint = CVec3(0, 0, fButtomRadius);
        startPos.position = topPoint * mat;
        endPos.position = buttomPoint * mat;
        CRenderManager::GetInstance()->RenderLine(startPos, endPos, 1.0f, true);
        topPoint.Z() *= -1;
        buttomPoint.Z() *= -1;
        startPos.position = topPoint * mat;
        endPos.position = buttomPoint * mat;
        CRenderManager::GetInstance()->RenderLine(startPos, endPos, 1.0f, true);
    }
}
Exemplo n.º 10
0
    static bool HandleWarpCommand(ChatHandler* handler, char const* args)
    {
        if (!*args)
            return false;

        Player* player = handler->GetSession()->GetPlayer();

        char* arg1 = strtok((char*)args, " ");
        char* arg2 = strtok(NULL, " ");

        if (!arg1 || !arg2)
        return false;

        char dir = arg1[0];
        float value = float(atof(arg2));
        float x = player->GetPositionX();
        float y = player->GetPositionY();
        float z = player->GetPositionZ();
        float o = player->GetOrientation();
		 uint32 mapid = player->GetMapId();
        Map const* map = sMapMgr->CreateBaseMap(mapid);
        z = std::max(map->GetHeight(x, y, MAX_HEIGHT), map->GetWaterLevel(x, y));

    switch (dir)
    {
	case 'l': // left
        {
			x = x + cos(o+(M_PI/2))*value;
           y = y + sin(o+(M_PI/2))*value;

           player->TeleportTo(mapid, x, y, z, o);
        }
        break;
    case 'r': // right
        {
			x = x + cos(o-(M_PI/2))*value;
           y = y + sin(o-(M_PI/2))*value;

           player->TeleportTo(mapid, x, y, z, o);
        }
        break;
    case 'f': // forward
        {
			x = x + cosf(o)*value;
           y = y + sinf(o)*value;

           player->TeleportTo(mapid, x, y, z, o);
        }
        break;
    case 'u': // up
		{
           player->TeleportTo(mapid, x, y, z + value, o);
		}
        break;
	 case 'd': // down
		{
           player->TeleportTo(mapid, x, y, z - value, o);
		}
        break;
    case 'o': //orientation
        {
			o = Position::NormalizeOrientation((value * M_PI_F/180.0f)+ o);

           player->TeleportTo(mapid, x, y, z, o);
        }
        break;
    }
    return true;
  };
Exemplo n.º 11
0
/*
// Getting feature map for the selected subimage  
//
// API
// int getFeatureMaps(const IplImage * image, const int k, featureMap **map);
// INPUT
// image             - selected subimage
// k                 - size of cells
// OUTPUT
// map               - feature map
// RESULT
// Error status
*/
int getFeatureMaps_dp(const IplImage * image,const int k, CvLSVMFeatureMap **map)
{
    int sizeX, sizeY;
    int p, px, strsz;
    int height, width, channels;
    int i, j, kk, c, ii, jj, d;
    float  * datadx, * datady;
    float tmp, x, y, tx, ty;
    IplImage * dx, * dy;
    int *nearest_x, *nearest_y;
    float *w, a_x, b_x;

    float kernel[3] = {-1.f, 0.f, 1.f}; 
    CvMat kernel_dx = cvMat(1, 3, CV_32F, kernel);
    CvMat kernel_dy = cvMat(3, 1, CV_32F, kernel);

    float * r;
    int    * alfa;
    
    float boundary_x[CNTPARTION+1];
    float boundary_y[CNTPARTION+1];
    float max, tmp_scal;
    int    maxi;

	height = image->height;
	width  = image->width ;

    channels  = image->nChannels;

	dx    = cvCreateImage(cvSize(image->width , image->height) , IPL_DEPTH_32F , 3);
    dy    = cvCreateImage(cvSize(image->width , image->height) , IPL_DEPTH_32F , 3);

    sizeX = width  / k;
    sizeY = height / k;
    px    = CNTPARTION  + 2 * CNTPARTION; // контрастное и не контрастное изображение
    p     = px;
    strsz = sizeX * p;
    allocFeatureMapObject(map, sizeX, sizeY, p,  px);

	cvFilter2D(image, dx, &kernel_dx, cvPoint(-1, 0));
	cvFilter2D(image, dy, &kernel_dy, cvPoint(0, -1));
	
    for(i = 0; i <= CNTPARTION; i++)
    {
        boundary_x[i] = cosf((((float)i) * (((float)PI) / (float) (CNTPARTION))));
        boundary_y[i] = sinf((((float)i) * (((float)PI) / (float) (CNTPARTION))));
    }/*for(i = 0; i <= CNTPARTION; i++) */

    r    = (float *)malloc( sizeof(float) * (width * height));
    alfa = (int   *)malloc( sizeof(int  ) * (width * height * 2));

    for(j = 1; j < height-1; j++)
    {
        datadx = (float*)(dx->imageData + dx->widthStep *j);
        datady = (float*)(dy->imageData + dy->widthStep *j);
        for(i = 1; i < width-1; i++)
        {
			c = 0;
            x = (datadx[i*channels+c]);
            y = (datady[i*channels+c]);

            r[j * width + i] =sqrtf(x*x + y*y);
            for(kk = 1; kk < channels; kk++)
            {
                tx = (datadx[i*channels+kk]);
                ty = (datady[i*channels+kk]);
                tmp =sqrtf(tx*tx + ty*ty);
                if(tmp > r[j * width + i])
                {
                    r[j * width + i] = tmp;
                    c = kk;
                    x = tx;
                    y = ty;
                }
            }/*for(kk = 1; kk < channels; kk++)*/

            
            
            max  = boundary_x[0]*x + boundary_y[0]*y;
            maxi = 0;
            for (kk = 0; kk < CNTPARTION; kk++) {
                tmp_scal = boundary_x[kk]*x + boundary_y[kk]*y;
                if (tmp_scal> max) {
                    max = tmp_scal;
                    maxi = kk;
                }else if (-tmp_scal> max) {
                    max = -tmp_scal;
                    maxi = kk + CNTPARTION;
                }
            }
            alfa[j * width * 2 + i * 2    ] = maxi % CNTPARTION;
            alfa[j * width * 2 + i * 2 + 1] = maxi;  
        }/*for(i = 0; i < width; i++)*/
    }/*for(j = 0; j < height; j++)*/

    //подсчет весов и смещений
    nearest_x = (int *)malloc(sizeof(int) * k);
    nearest_y = (int *)malloc(sizeof(int) * k);
    w         = (float*)malloc(sizeof(float) * (k * 2));
    
    for(i = 0; i < k / 2; i++)
    {
        nearest_x[i] = -1;
        nearest_y[i] = -1;
    }/*for(i = 0; i < k / 2; i++)*/
    for(i = k / 2; i < k; i++)
    {
        nearest_x[i] = 1;
        nearest_y[i] = 1;
    }/*for(i = k / 2; i < k; i++)*/

    for(j = 0; j < k / 2; j++)
    {
        b_x = k / 2 + j + 0.5f;
        a_x = k / 2 - j - 0.5f;
        w[j * 2    ] = 1.0f/a_x * ((a_x * b_x) / ( a_x + b_x)); 
        w[j * 2 + 1] = 1.0f/b_x * ((a_x * b_x) / ( a_x + b_x));  
    }/*for(j = 0; j < k / 2; j++)*/
    for(j = k / 2; j < k; j++)
    {
        a_x = j - k / 2 + 0.5f;
        b_x =-j + k / 2 - 0.5f + k;
        w[j * 2    ] = 1.0f/a_x * ((a_x * b_x) / ( a_x + b_x)); 
        w[j * 2 + 1] = 1.0f/b_x * ((a_x * b_x) / ( a_x + b_x));  
    }/*for(j = k / 2; j < k; j++)*/


    //интерпол¤ци¤
    for(i = 0; i < sizeY; i++)
    {
        for(j = 0; j < sizeX; j++)
        {
            for(ii = 0; ii < k; ii++)
            {
                for(jj = 0; jj < k; jj++)
                {
					if ((i * k + ii > 0) && (i * k + ii < height - 1) && (j * k + jj > 0) && (j * k + jj < width - 1))
					{
						d    =  (k*i + ii)* width + (j*k + jj);
						(*map)->Map[(i                ) * strsz + (j                ) * (*map)->p + alfa[d * 2    ]             ] += 
							r[d] * w[ii * 2    ] * w[jj * 2    ];
						(*map)->Map[(i                ) * strsz + (j                ) * (*map)->p + alfa[d * 2 + 1] + CNTPARTION] += 
							r[d] * w[ii * 2    ] * w[jj * 2    ];
						if ((i + nearest_y[ii] >= 0) && (i + nearest_y[ii] <= sizeY - 1))
						{
							(*map)->Map[(i + nearest_y[ii]) * strsz + (j                ) * (*map)->p + alfa[d * 2    ]             ] += 
								r[d] * w[ii * 2 + 1] * w[jj * 2    ];
							(*map)->Map[(i + nearest_y[ii]) * strsz + (j                ) * (*map)->p + alfa[d * 2 + 1] + CNTPARTION] += 
								r[d] * w[ii * 2 + 1] * w[jj * 2    ];
						}
						if ((j + nearest_x[jj] >= 0) && (j + nearest_x[jj] <= sizeX - 1))
						{
							(*map)->Map[(i                ) * strsz + (j + nearest_x[jj]) * (*map)->p + alfa[d * 2    ]             ] += 
								r[d] * w[ii * 2    ] * w[jj * 2 + 1];
							(*map)->Map[(i                ) * strsz + (j + nearest_x[jj]) * (*map)->p + alfa[d * 2 + 1] + CNTPARTION] += 
								r[d] * w[ii * 2    ] * w[jj * 2 + 1];
						}
						if ((i + nearest_y[ii] >= 0) && (i + nearest_y[ii] <= sizeY - 1) && (j + nearest_x[jj] >= 0) && (j + nearest_x[jj] <= sizeX - 1))
						{
							(*map)->Map[(i + nearest_y[ii]) * strsz + (j + nearest_x[jj]) * (*map)->p + alfa[d * 2    ]             ] += 
								r[d] * w[ii * 2 + 1] * w[jj * 2 + 1];
							(*map)->Map[(i + nearest_y[ii]) * strsz + (j + nearest_x[jj]) * (*map)->p + alfa[d * 2 + 1] + CNTPARTION] += 
								r[d] * w[ii * 2 + 1] * w[jj * 2 + 1];
						}
					}
                }/*for(jj = 0; jj < k; jj++)*/
            }/*for(ii = 0; ii < k; ii++)*/
        }/*for(j = 1; j < sizeX - 1; j++)*/
    }/*for(i = 1; i < sizeY - 1; i++)*/
    
    cvReleaseImage(&dx);
    cvReleaseImage(&dy);


    free(w);
    free(nearest_x);
    free(nearest_y);

    free(r);
    free(alfa);

    return LATENT_SVM_OK;
}
Exemplo n.º 12
0
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
// avoid unnecessary operations
void NavEKF2_core::setWindMagStateLearningMode()
{
    // If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
    bool setWindInhibit = (!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE);
    if (!inhibitWindStates && setWindInhibit) {
        inhibitWindStates = true;
    } else if (inhibitWindStates && !setWindInhibit) {
        inhibitWindStates = false;
        // set states and variances
        if (yawAlignComplete && useAirspeed()) {
            // if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading
            // which assumes the vehicle has launched into the wind
             Vector3f tempEuler;
            stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
            float windSpeed =  sqrtf(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas;
            stateStruct.wind_vel.x = windSpeed * cosf(tempEuler.z);
            stateStruct.wind_vel.y = windSpeed * sinf(tempEuler.z);

            // set the wind sate variances to the measurement uncertainty
            for (uint8_t index=22; index<=23; index++) {
                P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(_ahrs->get_EAS2TAS(), 0.9f, 10.0f));
            }
        } else {
            // set the variances using a typical wind speed
            for (uint8_t index=22; index<=23; index++) {
                P[index][index] = sq(5.0f);
            }
        }
    }

    // determine if the vehicle is manoevring
    if (accNavMagHoriz > 0.5f) {
        manoeuvring = true;
    } else {
        manoeuvring = false;
    }

    // Determine if learning of magnetic field states has been requested by the user
    uint8_t magCal = effective_magCal();
    bool magCalRequested =
            ((magCal == 0) && inFlight) || // when flying
            ((magCal == 1) && manoeuvring)  || // when manoeuvring
            ((magCal == 3) && firstInflightYawInit && firstInflightMagInit) || // when initial in-air yaw and mag field reset is complete
            (magCal == 4); // all the time

    // Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
    // we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
    bool magCalDenied = !use_compass() || (magCal == 2) || (onGround && magCal != 4);

    // Inhibit the magnetic field calibration if not requested or denied
    bool setMagInhibit = !magCalRequested || magCalDenied;
    if (!inhibitMagStates && setMagInhibit) {
        inhibitMagStates = true;
    } else if (inhibitMagStates && !setMagInhibit) {
        inhibitMagStates = false;
        // when commencing use of magnetic field states, set the variances equal to the observation uncertainty
        for (uint8_t index=16; index<=21; index++) {
            P[index][index] = sq(frontend->_magNoise);
        }
        // request a reset of the yaw and magnetic field states if not done before
        if (!magStateInitComplete || (!firstInflightMagInit && inFlight)) {
            magYawResetRequest = true;
        }
    }

    // If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed
    // because we want it re-done for each takeoff
    if (onGround) {
        firstInflightYawInit = false;
        firstInflightMagInit = false;
    }

    // Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
    // if we are not using those states
    if (inhibitMagStates && inhibitWindStates) {
        stateIndexLim = 15;
    } else if (inhibitWindStates) {
        stateIndexLim = 21;
    } else {
        stateIndexLim = 23;
    }
}
Exemplo n.º 13
0
/**
 * body()
 *
 * Creates a display list for the body of a person.
 */
void body()
{
   GLfloat fan_angle;   //angle for a triangle fan
   int sections = 40;   //number of triangles in a triangle fan

   //generate a display list for a body
   body_list_id = glGenLists(1);
   //compile the new display list
   glNewList(body_list_id, GL_COMPILE);

   glPushMatrix();
      glTranslatef(0.0f, 5.05f, 0.0f);
      glRotatef(90, 1.0, 0.0, 0.0);

      //create a torso with a cylinder
      glPushMatrix();
         GLUquadricObj * torso;
         torso = gluNewQuadric();
         gluQuadricDrawStyle(torso, GLU_FILL);
         gluCylinder(torso, 1.0f, 1.0f, 2.25f, 25, 25);
      glPopMatrix();

      //triangle fan for top of body
      glPushMatrix();
         glRotatef(180.0, 0.0, 1.0, 0.0);
         glBegin(GL_TRIANGLE_FAN);
         glNormal3f(0.0f, 0.0f, 1.0f);
         glVertex3f(0.0f, 0.0f, 0.0f);
         for(int i = 0; i <= sections; ++i) {
            fan_angle = (GLfloat) i * 2 * PI / sections;
            glVertex3f(cosf(fan_angle), sinf(fan_angle), 0.0f);
         }
         glEnd();
      glPopMatrix();

      //triangle fan for bottom of torso
      glPushMatrix();
         glTranslatef(0.0, 0.0, 2.25);
         glBegin(GL_TRIANGLE_FAN);
         glNormal3f(0.0f, 0.0f, 1.0f);
         glVertex3f(0.0f, 0.0f, 0.0f);
         for(int i = 0; i <= sections; ++i) {
            fan_angle = (GLfloat) i * 2 * PI / sections;
            glVertex3f(cosf(fan_angle), sinf(fan_angle), 0.0f);
         }
         glEnd();
      glPopMatrix();

      //arm 1
      glPushMatrix();
         glTranslatef(1.25f, 0.0f, 0.0f);
         GLUquadricObj * arm1;
         arm1 = gluNewQuadric();
         gluQuadricDrawStyle(arm1, GLU_FILL);
         gluCylinder(arm1, 0.25f, 0.25f, 2.5f, 12, 12);
      glPopMatrix();

      //triangle fan for top of arm 1
      glPushMatrix();
         glTranslatef(1.25, 0.0, 0.0);
         glRotatef(180.0, 0.0, 1.0, 0.0);
         glBegin(GL_TRIANGLE_FAN);
         glNormal3f(0.0f, 0.0f, 1.0f);
         glVertex3f(0.0f, 0.0f, 0.0f);
         sections = 12;
         for(int i = 0; i <= sections; ++i) {
            fan_angle = (GLfloat) i * 2 * PI / sections;
            glVertex3f(0.25*cosf(fan_angle), 0.25*sinf(fan_angle), 0.0f);
         }
         glEnd();
      glPopMatrix();

      //triangle fan for bottom of arm 1
      glPushMatrix();
         glTranslatef(1.25, 0.0, 2.5);
         glBegin(GL_TRIANGLE_FAN);
         glNormal3f(0.0f, 0.0f, 1.0f);
         glVertex3f(0.0f, 0.0f, 0.0f);
         sections = 12;
         for(int i = 0; i <= sections; ++i) {
            fan_angle = (GLfloat) i * 2 * PI / sections;
            glVertex3f(0.25*cosf(fan_angle), 0.25*sinf(fan_angle), 0.0f);
         }
         glEnd();
      glPopMatrix();

      //arm 2
      glPushMatrix();
         glTranslatef(-1.25f, 0.0f, 0.0f);
         GLUquadricObj * arm2;
         arm2 = gluNewQuadric();
         gluQuadricDrawStyle(arm2, GLU_FILL);
         gluCylinder(arm2, 0.25f, 0.25f, 2.5f, 12, 12);
      glPopMatrix();

      //triangle fan for top of arm 2
      glPushMatrix();
         glTranslatef(-1.25, 0.0, 0.0);
         glRotatef(180.0, 0.0, 1.0, 0.0);
         glBegin(GL_TRIANGLE_FAN);
         glNormal3f(0.0f, 0.0f, 1.0f);
         glVertex3f(0.0f, 0.0f, 0.0f);
         sections = 12;
         for(int i = 0; i <= sections; ++i) {
            fan_angle = (GLfloat) i * 2 * PI / sections;
            glVertex3f(0.25*cosf(fan_angle), 0.25*sinf(fan_angle), 0.0f);
         }
         glEnd();
      glPopMatrix();

      //triangle fan for bottom of arm 2
      glPushMatrix();
         glTranslatef(-1.25, 0.0, 2.5);
         glBegin(GL_TRIANGLE_FAN);
         glNormal3f(0.0f, 0.0f, 1.0f);
         glVertex3f(0.0f, 0.0f, 0.0f);
         sections = 12;
         for(int i = 0; i <= sections; ++i) {
            fan_angle = (GLfloat) i * 2 * PI / sections;
            glVertex3f(0.25*cosf(fan_angle), 0.25*sinf(fan_angle), 0.0f);
         }
         glEnd();
      glPopMatrix();

      //leg 1
      glPushMatrix();
         glTranslatef(0.5f, 0.0f, 2.25f);
         GLUquadricObj * leg1;
         leg1 = gluNewQuadric();
         gluQuadricDrawStyle(leg1, GLU_FILL);
         gluCylinder(leg1, 0.5f, 0.2f, 2.8f, 12, 12);
      glPopMatrix();

      //triangle fan for bottom of leg 1
      glPushMatrix();
         glTranslatef(0.5, 0.0, 5.05);
         glBegin(GL_TRIANGLE_FAN);
         glNormal3f(0.0f, 0.0f, 1.0f);
         glVertex3f(0.0f, 0.0f, 0.0f);
         sections = 12;
         for(int i = 0; i <= sections; ++i) {
            fan_angle = (GLfloat) i * 2 * PI / sections;
            glVertex3f(0.2*cosf(fan_angle), 0.2*sinf(fan_angle), 0.0f);
         }
         glEnd();
      glPopMatrix();

      //leg 2
      glPushMatrix();
         glTranslatef(-0.5f, 0.0f, 2.25f);
         GLUquadricObj * leg2;
         leg2 = gluNewQuadric();
         gluQuadricDrawStyle(leg2, GLU_FILL);
         gluCylinder(leg2, 0.5f, 0.2f, 2.8f, 12, 12);
      glPopMatrix();

      //triangle fan for bottom of leg 2
      glPushMatrix();
         glTranslatef(-0.5, 0.0, 5.05);
         glBegin(GL_TRIANGLE_FAN);
         glNormal3f(0.0f, 0.0f, 1.0f);
         glVertex3f(0.0f, 0.0f, 0.0f);
         sections = 12;
         for(int i = 0; i <= sections; ++i) {
            fan_angle = (GLfloat) i * 2 * PI / sections;
            glVertex3f(0.2*cosf(fan_angle), 0.2*sinf(fan_angle), 0.0f);
         }
         glEnd();
      glPopMatrix();

   glPopMatrix();

   glEndList();
}
Exemplo n.º 14
0
void draw(HyperspaceSaverSettings *inSettings){
	if(inSettings->first){
		if(inSettings->dUseTunnels){  // only tunnels use caustic textures
			glDisable(GL_FOG);
			// Caustic textures can only be created after rendering context has been created
			// because they have to be drawn and then read back from the framebuffer.
#ifdef WIN32
			if(doingPreview) // super fast for Windows previewer
				inSettings->theCausticTextures = new causticTextures(8, inSettings->numAnimTexFrames, 32, 32, 1.0f, 0.01f, 10.0f);
			else  // normal
#endif
				inSettings->theCausticTextures = new causticTextures(8, inSettings->numAnimTexFrames, 100, 256, 1.0f, 0.01f, 20.0f);
			glEnable(GL_FOG);
		}
		if(inSettings->dShaders){
#ifdef WIN32
			if(doingPreview) // super fast for Windows previewer
				inSettings->theWNCM = new wavyNormalCubeMaps(inSettings->numAnimTexFrames, 32);
			else  // normal
#endif
				inSettings->theWNCM = new wavyNormalCubeMaps(inSettings->numAnimTexFrames, 128);
		}
		glViewport(inSettings->viewport[0], inSettings->viewport[1], inSettings->viewport[2], inSettings->viewport[3]);
		inSettings->first = 0;
	}

	// Variables for printing text
	static float computeTime = 0.0f;
	static float drawTime = 0.0f;
	//static rsTimer computeTimer, drawTimer;
	// start compute time timer
	//computeTimer.tick();

	glMatrixMode(GL_MODELVIEW);

	// Camera movements
	static float camHeading[3] = {0.0f, 0.0f, 0.0f};  // current, target, and last
	static int changeCamHeading = 1;
	static float camHeadingChangeTime[2] = {20.0f, 0.0f};  // total, elapsed
	static float camRoll[3] = {0.0f, 0.0f, 0.0f};  // current, target, and last
	static int changeCamRoll = 1;
	static float camRollChangeTime[2] = {1.0f, 0.0f};  // total, elapsed
	camHeadingChangeTime[1] += inSettings->frameTime;
	if(camHeadingChangeTime[1] >= camHeadingChangeTime[0]){  // Choose new direction
		camHeadingChangeTime[0] = rsRandf(15.0f) + 5.0f;
		camHeadingChangeTime[1] = 0.0f;
		camHeading[2] = camHeading[1];  // last = target
		if(changeCamHeading){
			// face forward most of the time
			if(rsRandi(6))
				camHeading[1] = 0.0f;
			// face backward the rest of the time
			else if(rsRandi(2))
				camHeading[1] = RS_PI;
			else
				camHeading[1] = -RS_PI;
			changeCamHeading = 0;
		}
		else
			changeCamHeading = 1;
	}
	float t = camHeadingChangeTime[1] / camHeadingChangeTime[0];
	t = 0.5f * (1.0f - cosf(RS_PI * t));
	camHeading[0] = camHeading[1] * t + camHeading[2] * (1.0f - t);
	camRollChangeTime[1] += inSettings->frameTime;
	if(camRollChangeTime[1] >= camRollChangeTime[0]){  // Choose new roll angle
		camRollChangeTime[0] = rsRandf(5.0f) + 10.0f;
		camRollChangeTime[1] = 0.0f;
		camRoll[2] = camRoll[1];  // last = target
		if(changeCamRoll){
			camRoll[1] = rsRandf(RS_PIx2*2) - RS_PIx2;
			changeCamRoll = 0;
		}
		else
			changeCamRoll = 1;
	}
	t = camRollChangeTime[1] / camRollChangeTime[0];
	t = 0.5f * (1.0f - cosf(RS_PI * t));
	camRoll[0] = camRoll[1] * t + camRoll[2] * (1.0f - t);

	static float pathDir[3] = {0.0f, 0.0f, -1.0f};
	inSettings->thePath->moveAlongPath(float(inSettings->dSpeed) * inSettings->frameTime * 0.04f);
	inSettings->thePath->update(inSettings->frameTime);
	inSettings->thePath->getPoint(inSettings->dDepth + 2, inSettings->thePath->step, inSettings->camPos);
	inSettings->thePath->getBaseDirection(inSettings->dDepth + 2, inSettings->thePath->step, pathDir);
	float pathAngle = atan2f(-pathDir[0], -pathDir[2]);

	glLoadIdentity();
	glRotatef((pathAngle + camHeading[0]) * RS_RAD2DEG, 0, 1, 0);
	glRotatef(camRoll[0] * RS_RAD2DEG, 0, 0, 1);
	glGetFloatv(GL_MODELVIEW_MATRIX, inSettings->billboardMat);
	glLoadIdentity();
	glRotatef(-camRoll[0] * RS_RAD2DEG, 0, 0, 1);
	glRotatef((-pathAngle - camHeading[0]) * RS_RAD2DEG, 0, 1, 0);
	glTranslatef(inSettings->camPos[0]*-1, inSettings->camPos[1]*-1, inSettings->camPos[2]*-1);
	glGetDoublev(GL_MODELVIEW_MATRIX, inSettings->modelMat);
	inSettings->unroll = camRoll[0] * RS_RAD2DEG;

	if(inSettings->dUseGoo){
		// calculate diagonal fov
		float diagFov = 0.5f * float(inSettings->dFov) / RS_RAD2DEG;
		diagFov = tanf(diagFov);
		diagFov = sqrtf(diagFov * diagFov + (diagFov * inSettings->aspectRatio * diagFov * inSettings->aspectRatio));
		diagFov = 2.0f * atanf(diagFov);
		inSettings->theGoo->update(inSettings->camPos[0], inSettings->camPos[2], pathAngle + camHeading[0], diagFov, inSettings);
	}

	// measure compute time
	//computeTime += computeTimer.tick();
	// start draw time timer
	//drawTimer.tick();

	// clear
	glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
	glClear(GL_COLOR_BUFFER_BIT);

	// draw stars
	glCullFace(GL_BACK);
	glEnable(GL_CULL_FACE);
	glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE);
	glEnable(GL_BLEND);
	glEnable(GL_TEXTURE_2D);
	glActiveTextureARB(GL_TEXTURE2_ARB);
	glBindTexture(GL_TEXTURE_2D, NULL);
	glActiveTextureARB(GL_TEXTURE1_ARB);
	glBindTexture(GL_TEXTURE_2D, NULL);
	glActiveTextureARB(GL_TEXTURE0_ARB);
	glBindTexture(GL_TEXTURE_2D, inSettings->flaretex[0]);
	static float temppos[2];
	for(int i=0; i<inSettings->dStars; i++){
		temppos[0] = inSettings->stars[i]->pos[0] - inSettings->camPos[0];
		temppos[1] = inSettings->stars[i]->pos[2] - inSettings->camPos[2];
		if(temppos[0] > inSettings->depth){
			inSettings->stars[i]->pos[0] -= inSettings->depth * 2.0f;
			inSettings->stars[i]->lastPos[0] -= inSettings->depth * 2.0f;
		}
		if(temppos[0] < inSettings->depth*-1){
			inSettings->stars[i]->pos[0] += inSettings->depth * 2.0f;
			inSettings->stars[i]->lastPos[0] += inSettings->depth * 2.0f;
		}
		if(temppos[1] > inSettings->depth){
			inSettings->stars[i]->pos[2] -= inSettings->depth * 2.0f;
			inSettings->stars[i]->lastPos[2] -= inSettings->depth * 2.0f;
		}
		if(temppos[1] < inSettings->depth*-1){
			inSettings->stars[i]->pos[2] += inSettings->depth * 2.0f;
			inSettings->stars[i]->lastPos[2] += inSettings->depth * 2.0f;
		}
		inSettings->stars[i]->draw(inSettings->camPos, inSettings->unroll, inSettings->modelMat, inSettings->projMat, inSettings->viewport);
	}
	glDisable(GL_CULL_FACE);

	// pick animated texture frame
	static float textureTime = 0.0f;
	textureTime += inSettings->frameTime;
	// loop frames every 2 seconds
	const float texFrameTime(2.0f / float(inSettings->numAnimTexFrames));
	while(textureTime > texFrameTime){
		textureTime -= texFrameTime;
		inSettings->whichTexture ++;
	}
	while(inSettings->whichTexture >= inSettings->numAnimTexFrames)
		inSettings->whichTexture -= inSettings->numAnimTexFrames;

	// alpha component gets normalmap lerp value
	const float lerp = textureTime / texFrameTime;

	// draw goo
	if(inSettings->dUseGoo){
		// calculate color
		static float goo_rgb_phase[3] = {-0.1f, -0.1f, -0.1f};
		static float goo_rgb_speed[3] = {rsRandf(0.02f) + 0.02f, rsRandf(0.02f) + 0.02f, rsRandf(0.02f) + 0.02f};
		float goo_rgb[4];
		for(int i=0; i<3; i++){
			goo_rgb_phase[i] += goo_rgb_speed[i] * inSettings->frameTime;
			if(goo_rgb_phase[i] >= RS_PIx2)
				goo_rgb_phase[i] -= RS_PIx2;
			goo_rgb[i] = sinf(goo_rgb_phase[i]);
			if(goo_rgb[i] < 0.0f)
				goo_rgb[i] = 0.0f;
		}
		// setup textures
		if(inSettings->dShaders){
			goo_rgb[3] = lerp;
			glDisable(GL_TEXTURE_2D);
			glEnable(GL_TEXTURE_CUBE_MAP_ARB);
			glActiveTextureARB(GL_TEXTURE2_ARB);
			glBindTexture(GL_TEXTURE_CUBE_MAP_ARB, inSettings->nebulatex);
			glActiveTextureARB(GL_TEXTURE1_ARB);
			glBindTexture(GL_TEXTURE_CUBE_MAP_ARB, inSettings->theWNCM->texture[(inSettings->whichTexture + 1) % inSettings->numAnimTexFrames]);
			glActiveTextureARB(GL_TEXTURE0_ARB);
			glBindTexture(GL_TEXTURE_CUBE_MAP_ARB, inSettings->theWNCM->texture[inSettings->whichTexture]);
			glUseProgramObjectARB(gooProgram);
		}
		else{
			goo_rgb[3] = 1.0f;
			glBindTexture(GL_TEXTURE_2D, inSettings->nebulatex);
			glEnable(GL_TEXTURE_2D);
			glTexGeni(GL_S, GL_TEXTURE_GEN_MODE, GL_SPHERE_MAP);
			glTexGeni(GL_T, GL_TEXTURE_GEN_MODE, GL_SPHERE_MAP);
			glEnable(GL_TEXTURE_GEN_S);
			glEnable(GL_TEXTURE_GEN_T);
		}
		// draw it
		glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
		glBlendFunc(GL_SRC_ALPHA, GL_ONE);
		glEnable(GL_BLEND);
		glColor4fv(goo_rgb);
		inSettings->theGoo->draw();
		if(inSettings->dShaders){
			glDisable(GL_TEXTURE_CUBE_MAP_ARB);
			glUseProgramObjectARB(0);
		}
		else{
			glDisable(GL_TEXTURE_GEN_S);
			glDisable(GL_TEXTURE_GEN_T);
		}
	}

	// update starburst
	static float starBurstTime = 300.0f;  // burst after 5 minutes
	starBurstTime -= inSettings->frameTime;
	if(starBurstTime <= 0.0f){
		float pos[] = {inSettings->camPos[0] + (pathDir[0] * inSettings->depth * (0.5f + rsRandf(0.5f))),
			rsRandf(2.0f) - 1.0f,
			inSettings->camPos[2] + (pathDir[2] * inSettings->depth * (0.5f + rsRandf(0.5f)))};
		inSettings->theStarBurst->restart(pos);  // it won't actually restart unless it's ready to
		starBurstTime = rsRandf(240.0f) + 60.0f;  // burst again within 1-5 minutes
	}
	if(inSettings->dShaders)
		inSettings->theStarBurst->draw(lerp, inSettings);
	else
		inSettings->theStarBurst->draw(inSettings);

	// draw tunnel
	if(inSettings->dUseTunnels){
		inSettings->theTunnel->make(inSettings->frameTime, inSettings->dShaders);
		glCullFace(GL_BACK);
		glEnable(GL_CULL_FACE);
		glEnable(GL_TEXTURE_2D);
		if(inSettings->dShaders){
			glActiveTextureARB(GL_TEXTURE1_ARB);
			glBindTexture(GL_TEXTURE_2D, inSettings->theCausticTextures->caustictex[(inSettings->whichTexture + 1) % inSettings->numAnimTexFrames]);
			glActiveTextureARB(GL_TEXTURE0_ARB);
			glBindTexture(GL_TEXTURE_2D, inSettings->theCausticTextures->caustictex[inSettings->whichTexture]);
			glUseProgramObjectARB(tunnelProgram);
			inSettings->theTunnel->draw(lerp);
			glUseProgramObjectARB(0);
		}
		else{
			glBindTexture(GL_TEXTURE_2D, inSettings->theCausticTextures->caustictex[inSettings->whichTexture]);
			inSettings->theTunnel->draw();
		}
		glDisable(GL_CULL_FACE);
	}

	// draw sun with lens flare
	glDisable(GL_FOG);
	float flarepos[3] = {0.0f, 2.0f, 0.0f};
	glBindTexture(GL_TEXTURE_2D, inSettings->flaretex[0]);
	inSettings->sunStar->draw(inSettings->camPos, inSettings->unroll, inSettings->modelMat, inSettings->projMat, inSettings->viewport);
	float diff[3] = {flarepos[0] - inSettings->camPos[0], flarepos[1] - inSettings->camPos[1], flarepos[2] - inSettings->camPos[2]};
	float alpha = 0.5f - 0.005f * sqrtf(diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2]);
	if(alpha > 0.0f)
		flare(flarepos, 1.0f, 1.0f, 1.0f, alpha, inSettings);
	glEnable(GL_FOG);

	// measure draw time
	//drawTime += drawTimer.tick();

	// write text
	static float totalTime = 0.0f;
	totalTime += inSettings->frameTime;
	static std::vector<std::string> strvec;
	static int frames = 0;
	++frames;
	if(frames == 60){
		strvec.clear();
		std::string str1 = "         FPS = " + to_string(60.0f / totalTime);
		strvec.push_back(str1);
		std::string str2 = "compute time = " + to_string(computeTime / 60.0f);
		strvec.push_back(str2);
		std::string str3 = "   draw time = " + to_string(drawTime / 60.0f);
		strvec.push_back(str3);
		totalTime = 0.0f;
		computeTime = 0.0f;
		drawTime = 0.0f;
		frames = 0;
	}
	if(inSettings->kStatistics){
		glMatrixMode(GL_PROJECTION);
		glPushMatrix();
		glLoadIdentity();
		glOrtho(0.0f, 50.0f * inSettings->aspectRatio, 0.0f, 50.0f, -1.0f, 1.0f);

		glMatrixMode(GL_MODELVIEW);
		glPushMatrix();
		glLoadIdentity();
		glTranslatef(1.0f, 48.0f, 0.0f);

		glColor3f(1.0f, 0.6f, 0.0f);
		inSettings->textwriter->draw(strvec);

		glPopMatrix();
		glMatrixMode(GL_PROJECTION);
		glPopMatrix();
	}
	
#ifdef WIN32
	wglSwapLayerBuffers(hdc, WGL_SWAP_MAIN_PLANE);
#endif
#ifdef RS_XSCREENSAVER
	glXSwapBuffers(xdisplay, xwindow);
#endif
}
Exemplo n.º 15
0
int do_assemble2(program *program, program2 * program2)
{
  int pc = 0;
  int nextpc = 0;
  long long counter = 0;
  instruction2 ist;
  int iname, arg1, arg2, arg3;
    
  while (1) {
    pc = nextpc;

    
    ist = program2->insts[pc];
    iname = ist.name[0];
    arg1 = ist.name[1];
    arg2 = ist.name[2];
    arg3 = ist.name[3];

    /*
      ++counter;

      if(counter > 575600 && counter < 575719)
      {
      print_label_from_index(program, pc);
      print_instruction(program->insts[pc]);
      print_register();
      }
    */
	
    if (iname == 0) {
      // printf("this is nop\n");
    }
    DO_INST_1(1, +)
      DO_INST_1(2, -)
      DO_INST_1(3, *)
      DO_INST_1(4, &)
      DO_INST_1(5, |)
      /*ALU命令 */
    else if (iname == 6) {
      regist[ist.name[1]] = ~(regist[ist.name[2]] | regist[ist.name[3]]);
    }
    else if (iname == 7) {
      regist[ist.name[1]] = regist[ist.name[2]] ^ regist[ist.name[3]];
    }
    else if (iname == 8) {
      regist[ist.name[1]] = regist[ist.name[2]] + ist.name[3];
    }
    else if (iname == 9) {
      regist[ist.name[1]] = regist[ist.name[2]] - ist.name[3];
    }
    else if (iname == 10) {
      regist[ist.name[1]] = regist[ist.name[2]] * ist.name[3];
    }
    else if (iname == 11) {
      regist[ist.name[1]] = regist[ist.name[2]] & ist.name[3];
    }
    else if (iname == 12) {
      regist[ist.name[1]] = regist[ist.name[2]] | ist.name[3];
    }
    else if (iname == 13) {
      regist[ist.name[1]] = ~(regist[ist.name[2]] | ist.name[3]);
    }
    else if (iname == 14) {
      regist[ist.name[1]] = regist[ist.name[2]] ^ ist.name[3];
    }
    /*FPU命令 */
    else if (iname == 15) {
      freg[ist.name[1]] = freg[ist.name[2]] + freg[ist.name[3]];
    }
    else if (iname == 16) {
      freg[ist.name[1]] = freg[ist.name[2]] - freg[ist.name[3]];
    }
    else if (iname == 17) {
      freg[ist.name[1]] = freg[ist.name[2]] * freg[ist.name[3]];
    }
    else if (iname == 18) {
      freg[ist.name[1]] = freg[ist.name[2]] / freg[ist.name[3]];
    }
    else if (iname == 19) {
      freg[ist.name[1]] = 1 / freg[ist.name[2]];
    }
    else if (iname == 20) {
      freg[ist.name[1]] = sqrt(freg[ist.name[2]]);
    }
    else if (iname == 21) {
      freg[ist.name[1]] = floor(freg[ist.name[2]]);
    }
    else if (iname == 22) {
      freg[ist.name[1]] = (float) (regist[ist.name[2]]);
    }


    /*MEM ACSESS命令 */
    else if (iname == 23) {
      //memory_check
      if (check_memory(regist[ist.name[2]] + ist.name[3]) == ACSESS_BAD) {
	printf("Error:ACSESS_BAD :\n");
	//	 printf("%d\n", iname);
	//printf("%lld\n", counter);
	exit(1);
      }
      regist[ist.name[1]] = memory[regist[ist.name[2]] + ist.name[3]].i;
    }

    else if (iname == 24) {
      //memory_check
      if (check_memory(regist[ist.name[2]] + ist.name[3]) == ACSESS_BAD) {
	printf("Error:ACSESS_BAD :\n");
	//printf("%d\n", iname);
	exit(1);
      }
      memory[regist[ist.name[2]] + ist.name[3]].i =  regist[ist.name[1]];
    }

    else if (iname == 25) {
      //memory_check
      if (check_memory(regist[ist.name[2]] + ist.name[3])
	  == ACSESS_BAD) {
	printf("Error:ACSESS_BAD :\n");
	//printf("%d\n", iname);
	exit(1);
      }
      freg[ist.name[1]] =
	memory[regist[ist.name[2]] + ist.name[3]].d;
    }

    else if (iname == 26) {
      //memory_check
      if (check_memory(regist[ist.name[2]] + ist.name[3])
	  == ACSESS_BAD) {
	printf("Error:ACSESS_BAD :\n");
	// printf("%d\n", iname);
	exit(1);
      }
      memory[regist[ist.name[2]] + ist.name[3]].d =
	freg[ist.name[1]];
    }


    /*BRANCH命令 */
    else if (iname == 27) {
      if (regist[ist.name[1]] == regist[ist.name[2]]) {
	nextpc = pc + ist.name[3] - 1;
      }
    }

    else if (iname == 28) {
      if (regist[ist.name[1]] != regist[ist.name[2]]) {
	nextpc = pc + ist.name[3] - 1;
      }
    }
    else if (iname == 29) {
      if (regist[ist.name[1]] > regist[ist.name[2]]) {
	nextpc = pc + ist.name[3] - 1;
      }
    }

    else if (iname == 30) {
      if (regist[ist.name[1]] < regist[ist.name[2]]) {
	nextpc = pc + ist.name[3] - 1;
      }
    }

    else if (iname == 31) {
      if (regist[ist.name[1]] >= regist[ist.name[2]]) {
	nextpc = pc + ist.name[3] - 1;
      }
    }

    else if (iname == 32) {
      if (regist[ist.name[1]] <= regist[ist.name[2]]) {
	nextpc = pc + ist.name[3] - 1;
      }

    } else if (iname == 33) {
      if (freg[ist.name[1]] == freg[ist.name[2]]) {
	nextpc = pc + ist.name[3] - 1;
      }
    }

    else if (iname == 34) {
      if (freg[ist.name[1]] != freg[ist.name[2]]) {
	nextpc = pc + ist.name[3] - 1;
      }
    }

    else if (iname == 35) {
      if (freg[ist.name[1]] > freg[ist.name[2]]) {
	nextpc = pc + ist.name[3] - 1;
      }
    }

    else if (iname == 36) {
      if (freg[ist.name[1]] < freg[ist.name[2]]) {
	nextpc = pc + ist.name[3] - 1;
      }
    }

    else if (iname == 37) {
      if (freg[ist.name[1]] >= freg[ist.name[2]]) {
	nextpc = pc + ist.name[3] - 1;
      }
    }

    else if (iname == 38) {
      if (freg[ist.name[1]] <= freg[ist.name[2]]) {
	nextpc = pc + ist.name[3] - 1;
      }
    }


    /*JUMP命令 */
    else if (iname == 39) {
      if (ist.name[1] == -100) {
	freg[2] = atanf(freg[2]);
	nextpc = pop(&call_stack) - 1;
      } else if (ist.name[1] == -200) {
	freg[2] = sqrtf(freg[2]);
	nextpc = pop(&call_stack) - 1;
      } else if (ist.name[1] == -300) {
	regist[4] = read_int();
	printf("read_int: %d\n", regist[4]);
	nextpc = pop(&call_stack) - 1;
      } else if (ist.name[1] == -400) {
	freg[2] = read_float();
	printf("read_float: %f\n", freg[2]);
	nextpc = pop(&call_stack) - 1;
      } else if (ist.name[1] == -500) {
	printf("aaa\n");
	if(ppp < freg[2])
	  {
	    ppp = freg[2];
	  }
	if(nnn > freg[2])
	  {
	    nnn = freg[2];
	  }
	//print_register();
	nextpc = label_trans(program->insts[pc].name[1], program) - 1;
	//freg[2] = sinf(freg[2]);
	//printf("read_float: %f\n", freg[2]);
	//nextpc = pop(&call_stack) - 1;
      } else if (ist.name[1] == -600) {
	freg[2] = cosf(freg[2]);
	//printf("read_float: %f\n", freg[2]);
	nextpc = pop(&call_stack) - 1;
      } else if (ist.name[1] == -700) {
	//print_int
	print_int();
	nextpc = pop(&call_stack) - 1;
      } else if (ist.name[1] == -800) {
	print_float();
	nextpc = pop(&call_stack) - 1;
      }
      else
	{
	  nextpc = ist.name[1] - 1;
	}
      /*
	if(strncmp(program->insts[pc].name[1] , "L_main", 6) == 0)
	{
	print_memory();
	}
      */
    }


    else if (iname == 40) {
          
      nextpc = ist.name[1] - 1;
      /*特殊ラベル*/
      if (ist.name[1] == -100) {
	freg[2] = atanf(freg[2]);
	nextpc = pc;
      } else if (ist.name[1] == -200) {
	freg[2] = sqrtf(freg[2]);
	nextpc = pc;
      } else if (ist.name[1] == -300) {
	regist[4] = read_int();
	printf("read_int: %d\n", regist[4]);
	nextpc = pc;
      } else if (ist.name[1] == -400) {
	freg[2] = read_float();
	printf("read_float: %f\n", freg[2]);
	nextpc = pc;
      } else if (ist.name[1] == -500) {
	printf("aab\n");
	kkk = 1;
	if(ppp < freg[2])
	  {
	    ppp = freg[2];
	  }
	if(nnn > freg[2])
	  {
	    nnn = freg[2];
	  }
	push(&call_stack, (pc + 1));
	nextpc = label_trans(program->insts[pc].name[1], program) - 1;
	
	//freg[2] = sinf(freg[2]);
	//printf("read_float: %f\n", freg[2]);
	//nextpc = pc;
      } else if (ist.name[1] == -600) {
	freg[2] = cosf(freg[2]);
	//printf("read_float: %f\n", freg[2]);
	nextpc = pc;
      } else if (ist.name[1] == -700) {
	//print_int
	print_int();
	nextpc = pc;
      } else if (ist.name[1] == -800) {
	print_float();
	nextpc = pc;
      } else {
	push(&call_stack, (pc + 1));
      }


    }

    else if (iname == 41) {
      nextpc = pop(&call_stack) - 1;
    }

    else if (iname = 42) {
      print_register();
      print_memory();
    }
    //命令が存在しなかった場合error parseでやっているのでいらない。
    //printf("ist = %d\n",iname);

    nextpc++;


    /*命令がラストの行まで行けば処理を終了する */
    if (nextpc >= program2->inst_count) {
      break;
    }
  }

  return 0;
}
Exemplo n.º 16
0
int main( int argc, char* argv[] )
{
    // Length of vectors
    unsigned int n = 100000;
 
    // Host input vectors
    double *h_a;
    double *h_b;
    // Host output vector
    double *h_c;
 
    // Device input buffers
    cl_mem d_a;
    cl_mem d_b;
    // Device output buffer
    cl_mem d_c;
 
    cl_platform_id cpPlatform;        // OpenCL platform
    cl_device_id device_id;           // device ID
    cl_context context;               // context
    cl_command_queue queue;           // command queue
    cl_program program;               // program
    cl_kernel kernel;                 // kernel
 
    // Size, in bytes, of each vector
    size_t bytes = n*sizeof(double);
 
    // Allocate memory for each vector on host
    h_a = (double*)malloc(bytes);
    h_b = (double*)malloc(bytes);
    h_c = (double*)malloc(bytes);
 
    // Initialize vectors on host
    int i;
    for( i = 0; i < n; i++ )
    {
        h_a[i] = sinf(i)*sinf(i);
        h_b[i] = cosf(i)*cosf(i);
    }
 
    size_t globalSize, localSize;
    cl_int err;
 
    // Number of work items in each local work group
    localSize = 64;
 
    // Number of total work items - localSize must be devisor
    globalSize = ceil(n/(float)localSize)*localSize;
 
    // Bind to platform
    err = clGetPlatformIDs(1, &cpPlatform, NULL);
 
    // Get ID for the device
    err = clGetDeviceIDs(cpPlatform, CL_DEVICE_TYPE_GPU, 1, &device_id, NULL);
 
    // Create a context  
    context = clCreateContext(0, 1, &device_id, NULL, NULL, &err);
 
    // Create a command queue 
    queue = clCreateCommandQueue(context, device_id, 0, &err);
 
    // Create the compute program from the source buffer
    program = clCreateProgramWithSource(context, 1,
                            (const char **) & kernelSource, NULL, &err);
 
    // Build the program executable 
    clBuildProgram(program, 0, NULL, NULL, NULL, NULL);
 
    // Create the compute kernel in the program we wish to run
    kernel = clCreateKernel(program, "vecAdd", &err);
 
    // Create the input and output arrays in device memory for our calculation
    d_a = clCreateBuffer(context, CL_MEM_READ_ONLY, bytes, NULL, NULL);
    d_b = clCreateBuffer(context, CL_MEM_READ_ONLY, bytes, NULL, NULL);
    d_c = clCreateBuffer(context, CL_MEM_WRITE_ONLY, bytes, NULL, NULL);
 
    // Write our data set into the input array in device memory
    err = clEnqueueWriteBuffer(queue, d_a, CL_TRUE, 0,
                                   bytes, h_a, 0, NULL, NULL);
    err |= clEnqueueWriteBuffer(queue, d_b, CL_TRUE, 0,
                                   bytes, h_b, 0, NULL, NULL);
 
    // Set the arguments to our compute kernel
    err  = clSetKernelArg(kernel, 0, sizeof(cl_mem), &d_a);
    err |= clSetKernelArg(kernel, 1, sizeof(cl_mem), &d_b);
    err |= clSetKernelArg(kernel, 2, sizeof(cl_mem), &d_c);
    err |= clSetKernelArg(kernel, 3, sizeof(unsigned int), &n);
 
    // Execute the kernel over the entire range of the data set  
    err = clEnqueueNDRangeKernel(queue, kernel, 1, NULL, &globalSize, &localSize,
                                                              0, NULL, NULL);
 
    // Wait for the command queue to get serviced before reading back results
    clFinish(queue);
 
    // Read the results from the device
    clEnqueueReadBuffer(queue, d_c, CL_TRUE, 0,
                                bytes, h_c, 0, NULL, NULL );
 
    //Sum up vector c and print result divided by n, this should equal 1 within error
    double sum = 0;
    for(i=0; i<n; i++)
        sum += h_c[i];
    printf("final result: %f\n", sum/(double)n);
 
    // release OpenCL resources
    clReleaseMemObject(d_a);
    clReleaseMemObject(d_b);
    clReleaseMemObject(d_c);
    clReleaseProgram(program);
    clReleaseKernel(kernel);
    clReleaseCommandQueue(queue);
    clReleaseContext(context);
 
    //release host memory
    free(h_a);
    free(h_b);
    free(h_c);
 
    return 0;
}
Exemplo n.º 17
0
	virtual void compute (int count, FAUSTFLOAT** input, FAUSTFLOAT** output) {
		float 	fSlow0 = (0.5f * ((int(float(fcheckbox0)))?2:float(fslider0)));
		float 	fSlow1 = ((int(float(fcheckbox1)))?(0 - fSlow0):fSlow0);
		float 	fSlow2 = float(fslider1);
		float 	fSlow3 = powf(10,(0.05f * float(fslider2)));
		float 	fSlow4 = expf((fConst1 * (0 - (3.141592653589793f * float(fslider3)))));
		float 	fSlow5 = faustpower<2>(fSlow4);
		float 	fSlow6 = (fConst2 * float(fslider4));
		float 	fSlow7 = cosf(fSlow6);
		float 	fSlow8 = sinf(fSlow6);
		float 	fSlow9 = (0 - fSlow8);
		float 	fSlow10 = float(fslider5);
		float 	fSlow11 = (6.283185307179586f * fSlow10);
		float 	fSlow12 = (0.5f * ((6.283185307179586f * max(fSlow10, float(fslider6))) - fSlow11));
		float 	fSlow13 = float(fslider7);
		float 	fSlow14 = (fConst1 * fSlow13);
		float 	fSlow15 = (0 - (2 * fSlow4));
		float 	fSlow16 = (fConst1 * faustpower<2>(fSlow13));
		float 	fSlow17 = (fConst1 * faustpower<3>(fSlow13));
		float 	fSlow18 = (fConst1 * faustpower<4>(fSlow13));
		float 	fSlow19 = (1 - fSlow0);
		FAUSTFLOAT* input0 = input[0];
		FAUSTFLOAT* input1 = input[1];
		FAUSTFLOAT* output0 = output[0];
		FAUSTFLOAT* output1 = output[1];
		for (int i=0; i<count; i++) {
			float fTemp0 = (float)input0[i];
			float fTemp1 = (float)input1[i];
			iVec0[0] = 1;
			fRec5[0] = ((fSlow8 * fRec6[1]) + (fSlow7 * fRec5[1]));
			fRec6[0] = ((1 + ((fSlow7 * fRec6[1]) + (fSlow9 * fRec5[1]))) - iVec0[1]);
			float fTemp2 = (fSlow11 + (fSlow12 * (1 - fRec5[0])));
			float fTemp3 = (fRec4[1] * cosf((fSlow14 * fTemp2)));
			fRec4[0] = (0 - (((fSlow15 * fTemp3) + (fSlow5 * fRec4[2])) - ((fSlow3 * fTemp0) + (fSlow2 * fRec0[1]))));
			float fTemp4 = (fRec3[1] * cosf((fSlow16 * fTemp2)));
			fRec3[0] = ((fSlow15 * (fTemp3 - fTemp4)) + (fRec4[2] + (fSlow5 * (fRec4[0] - fRec3[2]))));
			float fTemp5 = (fRec2[1] * cosf((fSlow17 * fTemp2)));
			fRec2[0] = ((fSlow15 * (fTemp4 - fTemp5)) + (fRec3[2] + (fSlow5 * (fRec3[0] - fRec2[2]))));
			float fTemp6 = (fRec1[1] * cosf((fSlow18 * fTemp2)));
			fRec1[0] = ((fSlow15 * (fTemp5 - fTemp6)) + (fRec2[2] + (fSlow5 * (fRec2[0] - fRec1[2]))));
			fRec0[0] = ((fSlow5 * fRec1[0]) + ((fSlow15 * fTemp6) + fRec1[2]));
			output0[i] = (FAUSTFLOAT)((fSlow3 * (fTemp0 * fSlow19)) + (fRec0[0] * fSlow1));
			float fTemp7 = (fSlow11 + (fSlow12 * (1 - fRec6[0])));
			float fTemp8 = (fRec11[1] * cosf((fSlow14 * fTemp7)));
			fRec11[0] = (0 - (((fSlow15 * fTemp8) + (fSlow5 * fRec11[2])) - ((fSlow3 * fTemp1) + (fSlow2 * fRec7[1]))));
			float fTemp9 = (fRec10[1] * cosf((fSlow16 * fTemp7)));
			fRec10[0] = ((fSlow15 * (fTemp8 - fTemp9)) + (fRec11[2] + (fSlow5 * (fRec11[0] - fRec10[2]))));
			float fTemp10 = (fRec9[1] * cosf((fSlow17 * fTemp7)));
			fRec9[0] = ((fSlow15 * (fTemp9 - fTemp10)) + (fRec10[2] + (fSlow5 * (fRec10[0] - fRec9[2]))));
			float fTemp11 = (fRec8[1] * cosf((fSlow18 * fTemp7)));
			fRec8[0] = ((fSlow15 * (fTemp10 - fTemp11)) + (fRec9[2] + (fSlow5 * (fRec9[0] - fRec8[2]))));
			fRec7[0] = ((fSlow5 * fRec8[0]) + ((fSlow15 * fTemp11) + fRec8[2]));
			output1[i] = (FAUSTFLOAT)((fSlow3 * (fTemp1 * fSlow19)) + (fRec7[0] * fSlow1));
			// post processing
			fRec7[1] = fRec7[0];
			fRec8[2] = fRec8[1]; fRec8[1] = fRec8[0];
			fRec9[2] = fRec9[1]; fRec9[1] = fRec9[0];
			fRec10[2] = fRec10[1]; fRec10[1] = fRec10[0];
			fRec11[2] = fRec11[1]; fRec11[1] = fRec11[0];
			fRec0[1] = fRec0[0];
			fRec1[2] = fRec1[1]; fRec1[1] = fRec1[0];
			fRec2[2] = fRec2[1]; fRec2[1] = fRec2[0];
			fRec3[2] = fRec3[1]; fRec3[1] = fRec3[0];
			fRec4[2] = fRec4[1]; fRec4[1] = fRec4[0];
			fRec6[1] = fRec6[0];
			fRec5[1] = fRec5[0];
			iVec0[1] = iVec0[0];
		}
	}
Exemplo n.º 18
0
void LightingApp::UpdateScene(float dt)
{
	// Convert Spherical to Cartesian coordinates.
	float x = mRadius*sinf(mPhi)*cosf(mTheta);
	float z = mRadius*sinf(mPhi)*sinf(mTheta);
	float y = mRadius*cosf(mPhi);

	mEyePosW = XMFLOAT3(x, y, z);

	// Build the view matrix.
	XMVECTOR pos    = XMVectorSet(x, y, z, 1.0f);
	XMVECTOR target = XMVectorZero();
	XMVECTOR up     = XMVectorSet(0.0f, 1.0f, 0.0f, 0.0f);

	XMMATRIX V = XMMatrixLookAtLH(pos, target, up);
	XMStoreFloat4x4(&mView, V);

	//
	// Every quarter second, generate a random wave.
	//
	static float t_base = 0.0f;
	if( (mTimer.TotalTime() - t_base) >= 0.25f )
	{
		t_base += 0.25f;
 
		DWORD i = 5 + rand() % (mWaves.RowCount()-10);
		DWORD j = 5 + rand() % (mWaves.ColumnCount()-10);

		float r = MathHelper::RandF(1.0f, 2.0f);

		mWaves.Disturb(i, j, r);
	}

	mWaves.Update(dt);

	//
	// Update the wave vertex buffer with the new solution.
	//
	
	D3D11_MAPPED_SUBRESOURCE mappedData;
	HR(md3dImmediateContext->Map(mWavesVB, 0, D3D11_MAP_WRITE_DISCARD, 0, &mappedData));

	Vertex* v = reinterpret_cast<Vertex*>(mappedData.pData);
	for(UINT i = 0; i < mWaves.VertexCount(); ++i)
	{
		v[i].Pos    = mWaves[i];
		v[i].Normal = mWaves.Normal(i);
	}

	md3dImmediateContext->Unmap(mWavesVB, 0);

	//
	// Animate the lights.
	//

	// Circle light over the land surface.
	mPointLight.Position.x = 70.0f*cosf( 0.2f*mTimer.TotalTime() );
	mPointLight.Position.z = 70.0f*sinf( 0.2f*mTimer.TotalTime() );
	mPointLight.Position.y = MathHelper::Max(GetHillHeight(mPointLight.Position.x, 
		mPointLight.Position.z), -3.0f) + 10.0f;


	// The spotlight takes on the camera position and is aimed in the
	// same direction the camera is looking.  In this way, it looks
	// like we are holding a flashlight.
	mSpotLight.Position = mEyePosW;
	XMStoreFloat3(&mSpotLight.Direction, XMVector3Normalize(target - pos));
}
Exemplo n.º 19
0
void FlipY3D::update(float time)
{
    float angle = (float)M_PI * time; // 180 degrees
    float mz = sinf( angle );
    angle = angle / 2.0f;     // x calculates degrees from 0 to 90
    float my = cosf(angle);
    
    Vec3    v0, v1, v, diff;
    
    v0 = getOriginalVertex(Vec2(1.0f, 1.0f));
    v1 = getOriginalVertex(Vec2());
    
    float    y0 = v0.y;
    float    y1 = v1.y;
    float y;
    Vec2    a, b, c, d;
    
    if (y0 > y1)
    {
        // Normal Grid
        a.setZero();
        b.set(0.0f, 1.0f);
        c.set(1.0f, 0.0f);
        d.set(1.0f, 1.0f);
        y = y0;
    }
    else
    {
        // Reversed Grid
        b.setZero();
        a.set(0.0f, 1.0f);
        d.set(1.0f, 0.0f);
        c.set(1.0f, 1.0f);
        y = y1;
    }
    
    diff.y = y - y * my;
    diff.z = fabsf(floorf((y * mz) / 4.0f));
    
    // bottom-left
    v = getOriginalVertex(a);
    v.y = diff.y;
    v.z += diff.z;
    setVertex(a, v);
    
    // upper-left
    v = getOriginalVertex(b);
    v.y -= diff.y;
    v.z -= diff.z;
    setVertex(b, v);
    
    // bottom-right
    v = getOriginalVertex(c);
    v.y = diff.y;
    v.z += diff.z;
    setVertex(c, v);
    
    // upper-right
    v = getOriginalVertex(d);
    v.y -= diff.y;
    v.z -= diff.z;
    setVertex(d, v);
}
Exemplo n.º 20
0
	float LightingApp::GetHillHeight(float x, float z)const
	{
		return 0.3f*(z*sinf(0.1f*x) + x*cosf(0.1f*z));
	}
Exemplo n.º 21
0
// return a ground speed estimate in m/s
Vector2f AP_AHRS::groundspeed_vector(void)
{
    // Generate estimate of ground speed vector using air data system
    Vector2f gndVelADS;
    Vector2f gndVelGPS;
    float airspeed = 0;
    const bool gotAirspeed = airspeed_estimate_true(&airspeed);
    const bool gotGPS = (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D);
    if (gotAirspeed) {
        const Vector3f wind = wind_estimate();
        const Vector2f wind2d(wind.x, wind.y);
        const Vector2f airspeed_vector(_cos_yaw * airspeed, _sin_yaw * airspeed);
        gndVelADS = airspeed_vector - wind2d;
    }

    // Generate estimate of ground speed vector using GPS
    if (gotGPS) {
        const float cog = radians(AP::gps().ground_course_cd()*0.01f);
        gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * AP::gps().ground_speed();
    }
    // If both ADS and GPS data is available, apply a complementary filter
    if (gotAirspeed && gotGPS) {
        // The LPF is applied to the GPS and the HPF is applied to the air data estimate
        // before the two are summed
        //Define filter coefficients
        // alpha and beta must sum to one
        // beta = dt/Tau, where
        // dt = filter time step (0.1 sec if called by nav loop)
        // Tau = cross-over time constant (nominal 2 seconds)
        // More lag on GPS requires Tau to be bigger, less lag allows it to be smaller
        // To-Do - set Tau as a function of GPS lag.
        const float alpha = 1.0f - beta;
        // Run LP filters
        _lp = gndVelGPS * beta  + _lp * alpha;
        // Run HP filters
        _hp = (gndVelADS - _lastGndVelADS) + _hp * alpha;
        // Save the current ADS ground vector for the next time step
        _lastGndVelADS = gndVelADS;
        // Sum the HP and LP filter outputs
        return _hp + _lp;
    }
    // Only ADS data is available return ADS estimate
    if (gotAirspeed && !gotGPS) {
        return gndVelADS;
    }
    // Only GPS data is available so return GPS estimate
    if (!gotAirspeed && gotGPS) {
        return gndVelGPS;
    }

    if (airspeed > 0) {
        // we have a rough airspeed, and we have a yaw. For
        // dead-reckoning purposes we can create a estimated
        // groundspeed vector
        Vector2f ret(cosf(yaw), sinf(yaw));
        ret *= airspeed;
        // adjust for estimated wind
        const Vector3f wind = wind_estimate();
        ret.x += wind.x;
        ret.y += wind.y;
        return ret;
    }

    return Vector2f(0.0f, 0.0f);
}
Exemplo n.º 22
0
void do_kink(ParticleKey *state, const float par_co[3], const float par_vel[3], const float par_rot[4], float time, float freq, float shape,
             float amplitude, float flat, short type, short axis, float obmat[4][4], int smooth_start)
{
	float kink[3] = {1.f, 0.f, 0.f}, par_vec[3], q1[4] = {1.f, 0.f, 0.f, 0.f};
	float t, dt = 1.f, result[3];

	if (ELEM(type, PART_KINK_NO, PART_KINK_SPIRAL))
		return;

	CLAMP(time, 0.f, 1.f);

	if (shape != 0.0f && !ELEM(type, PART_KINK_BRAID)) {
		if (shape < 0.0f)
			time = (float)pow(time, 1.f + shape);
		else
			time = (float)pow(time, 1.f / (1.f - shape));
	}

	t = time * freq * (float)M_PI;
	
	if (smooth_start) {
		dt = fabsf(t);
		/* smooth the beginning of kink */
		CLAMP(dt, 0.f, (float)M_PI);
		dt = sinf(dt / 2.f);
	}

	if (!ELEM(type, PART_KINK_RADIAL)) {
		float temp[3];

		kink[axis] = 1.f;

		if (obmat)
			mul_mat3_m4_v3(obmat, kink);
		
		mul_qt_v3(par_rot, kink);

		/* make sure kink is normal to strand */
		project_v3_v3v3(temp, kink, par_vel);
		sub_v3_v3(kink, temp);
		normalize_v3(kink);
	}

	copy_v3_v3(result, state->co);
	sub_v3_v3v3(par_vec, par_co, state->co);

	switch (type) {
		case PART_KINK_CURL:
		{
			float curl_offset[3];
			
			/* rotate kink vector around strand tangent */
			mul_v3_v3fl(curl_offset, kink, amplitude);
			axis_angle_to_quat(q1, par_vel, t);
			mul_qt_v3(q1, curl_offset);
			
			interp_v3_v3v3(par_vec, state->co, par_co, flat);
			add_v3_v3v3(result, par_vec, curl_offset);
			break;
		}
		case PART_KINK_RADIAL:
		{
			if (flat > 0.f) {
				float proj[3];
				/* flatten along strand */
				project_v3_v3v3(proj, par_vec, par_vel);
				madd_v3_v3fl(result, proj, flat);
			}

			madd_v3_v3fl(result, par_vec, -amplitude * sinf(t));
			break;
		}
		case PART_KINK_WAVE:
		{
			madd_v3_v3fl(result, kink, amplitude * sinf(t));

			if (flat > 0.f) {
				float proj[3];
				/* flatten along wave */
				project_v3_v3v3(proj, par_vec, kink);
				madd_v3_v3fl(result, proj, flat);

				/* flatten along strand */
				project_v3_v3v3(proj, par_vec, par_vel);
				madd_v3_v3fl(result, proj, flat);
			}
			break;
		}
		case PART_KINK_BRAID:
		{
			float y_vec[3] = {0.f, 1.f, 0.f};
			float z_vec[3] = {0.f, 0.f, 1.f};
			float vec_one[3], state_co[3];
			float inp_y, inp_z, length;
		
			if (par_rot) {
				mul_qt_v3(par_rot, y_vec);
				mul_qt_v3(par_rot, z_vec);
			}

			negate_v3(par_vec);
			normalize_v3_v3(vec_one, par_vec);

			inp_y = dot_v3v3(y_vec, vec_one);
			inp_z = dot_v3v3(z_vec, vec_one);

			if (inp_y > 0.5f) {
				copy_v3_v3(state_co, y_vec);

				mul_v3_fl(y_vec, amplitude * cosf(t));
				mul_v3_fl(z_vec, amplitude / 2.f * sinf(2.f * t));
			}
			else if (inp_z > 0.0f) {
				mul_v3_v3fl(state_co, z_vec, sinf((float)M_PI / 3.f));
				madd_v3_v3fl(state_co, y_vec, -0.5f);

				mul_v3_fl(y_vec, -amplitude * cosf(t + (float)M_PI / 3.f));
				mul_v3_fl(z_vec, amplitude / 2.f * cosf(2.f * t + (float)M_PI / 6.f));
			}
			else {
				mul_v3_v3fl(state_co, z_vec, -sinf((float)M_PI / 3.f));
				madd_v3_v3fl(state_co, y_vec, -0.5f);

				mul_v3_fl(y_vec, amplitude * -sinf(t + (float)M_PI / 6.f));
				mul_v3_fl(z_vec, amplitude / 2.f * -sinf(2.f * t + (float)M_PI / 3.f));
			}

			mul_v3_fl(state_co, amplitude);
			add_v3_v3(state_co, par_co);
			sub_v3_v3v3(par_vec, state->co, state_co);

			length = normalize_v3(par_vec);
			mul_v3_fl(par_vec, MIN2(length, amplitude / 2.f));

			add_v3_v3v3(state_co, par_co, y_vec);
			add_v3_v3(state_co, z_vec);
			add_v3_v3(state_co, par_vec);

			shape = 2.f * (float)M_PI * (1.f + shape);

			if (t < shape) {
				shape = t / shape;
				shape = (float)sqrt((double)shape);
				interp_v3_v3v3(result, result, state_co, shape);
			}
			else {
				copy_v3_v3(result, state_co);
			}
			break;
		}
	}

	/* blend the start of the kink */
	if (dt < 1.f)
		interp_v3_v3v3(state->co, state->co, result, dt);
	else
		copy_v3_v3(state->co, result);
}
Exemplo n.º 23
0
/*
================
R_RotateBmodel
================
*/
void R_RotateBmodel (void)
{
	float	angle, s, c, temp1[3][3], temp2[3][3], temp3[3][3];

// TODO: should use a look-up table
// TODO: should really be stored with the entity instead of being reconstructed
// TODO: could cache lazily, stored in the entity
// TODO: share work with R_SetUpAliasTransform

// yaw
	angle = currententity->angles[YAW];		
	angle = angle * M_PI*2 / 360;
	s = sinf(angle);
	c = cosf(angle);

	temp1[0][0] = c;
	temp1[0][1] = s;
	temp1[0][2] = 0;
	temp1[1][0] = -s;
	temp1[1][1] = c;
	temp1[1][2] = 0;
	temp1[2][0] = 0;
	temp1[2][1] = 0;
	temp1[2][2] = 1;


// pitch
	angle = currententity->angles[PITCH];		
	angle = angle * M_PI*2 / 360;
	s = sinf(angle);
	c = cosf(angle);

	temp2[0][0] = c;
	temp2[0][1] = 0;
	temp2[0][2] = -s;
	temp2[1][0] = 0;
	temp2[1][1] = 1;
	temp2[1][2] = 0;
	temp2[2][0] = s;
	temp2[2][1] = 0;
	temp2[2][2] = c;

	R_ConcatRotations (temp2, temp1, temp3);

// roll
	angle = currententity->angles[ROLL];		
	angle = angle * M_PI*2 / 360;
	s = sinf(angle);
	c = cosf(angle);

	temp1[0][0] = 1;
	temp1[0][1] = 0;
	temp1[0][2] = 0;
	temp1[1][0] = 0;
	temp1[1][1] = c;
	temp1[1][2] = s;
	temp1[2][0] = 0;
	temp1[2][1] = -s;
	temp1[2][2] = c;

	R_ConcatRotations (temp1, temp3, entity_rotation);

//
// rotate modelorg and the transformation matrix
//
	R_EntityRotate (modelorg);
	R_EntityRotate (vpn);
	R_EntityRotate (vright);
	R_EntityRotate (vup);

	R_TransformFrustum ();
}
Exemplo n.º 24
0
Arquivo: usc.hpp Projeto: Bastl34/PCL
template <typename PointInT, typename PointOutT, typename PointRFT> bool
pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::initCompute ()
{
  if (!Feature<PointInT, PointOutT>::initCompute ())
  {
    PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
    return (false);
  }

  // Default LRF estimation alg: SHOTLocalReferenceFrameEstimation
  typename SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>::Ptr lrf_estimator(new SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>());
  lrf_estimator->setRadiusSearch (local_radius_);
  lrf_estimator->setInputCloud (input_);
  lrf_estimator->setIndices (indices_);
  if (!fake_surface_)
    lrf_estimator->setSearchSurface(surface_);

  if (!FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames (indices_->size (), lrf_estimator))
  {
    PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
    return (false);
  }

  if (search_radius_< min_radius_)
  {
    PCL_ERROR ("[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().c_str ());
    return (false);
  }

  // Update descriptor length
  descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_;

  // Compute radial, elevation and azimuth divisions
  float azimuth_interval = 360.0f / static_cast<float> (azimuth_bins_);
  float elevation_interval = 180.0f / static_cast<float> (elevation_bins_);

  // Reallocate divisions and volume lut
  radii_interval_.clear ();
  phi_divisions_.clear ();
  theta_divisions_.clear ();
  volume_lut_.clear ();

  // Fills radii interval based on formula (1) in section 2.1 of Frome's paper
  radii_interval_.resize (radius_bins_ + 1);
  for (size_t j = 0; j < radius_bins_ + 1; j++)
    radii_interval_[j] = static_cast<float> (exp (log (min_radius_) + ((static_cast<float> (j) / static_cast<float> (radius_bins_)) * log (search_radius_/min_radius_))));

  // Fill theta didvisions of elevation
  theta_divisions_.resize (elevation_bins_+1);
  for (size_t k = 0; k < elevation_bins_+1; k++)
    theta_divisions_[k] = static_cast<float> (k) * elevation_interval;

  // Fill phi didvisions of elevation
  phi_divisions_.resize (azimuth_bins_+1);
  for (size_t l = 0; l < azimuth_bins_+1; l++)
    phi_divisions_[l] = static_cast<float> (l) * azimuth_interval;

  // LookUp Table that contains the volume of all the bins
  // "phi" term of the volume integral
  // "integr_phi" has always the same value so we compute it only one time
  float integr_phi  = pcl::deg2rad (phi_divisions_[1]) - pcl::deg2rad (phi_divisions_[0]);
  // exponential to compute the cube root using pow
  float e = 1.0f / 3.0f;
  // Resize volume look up table
  volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_);
  // Fill volumes look up table
  for (size_t j = 0; j < radius_bins_; j++)
  {
    // "r" term of the volume integral
    float integr_r = (radii_interval_[j+1]*radii_interval_[j+1]*radii_interval_[j+1] / 3) - (radii_interval_[j]*radii_interval_[j]*radii_interval_[j]/ 3);

    for (size_t k = 0; k < elevation_bins_; k++)
    {
      // "theta" term of the volume integral
      float integr_theta = cosf (deg2rad (theta_divisions_[k])) - cosf (deg2rad (theta_divisions_[k+1]));
      // Volume
      float V = integr_phi * integr_theta * integr_r;
      // Compute cube root of the computed volume commented for performance but left
      // here for clarity
      // float cbrt = pow(V, e);
      // cbrt = 1 / cbrt;

      for (size_t l = 0; l < azimuth_bins_; l++)
        // Store in lut 1/cbrt
        //volume_lut_[ (l*elevation_bins_*radius_bins_) + k*radius_bins_ + j ] = cbrt;
        volume_lut_[(l*elevation_bins_*radius_bins_) + k*radius_bins_ + j] = 1.0f / powf (V, e);
    }
  }
  return (true);
}
Exemplo n.º 25
0
/* Compute scattering angle, scattering angle azimuth, dip angle, dip angle azimuth
   from source and receiver elevation/azimuth angles */
static void sf_cram_point3_angles (float sb, float sa, float hb, float ha,
                                   float *oa, float *oz, float *da, float *dz) {
    float sv[3], hv[3], ov[3], dv[3], pv[3], nv[3], mv[3];
    float a, l, dl, dll;

    /* Create shot and receiver vectors */
    sv[0] = cosf (sb);          /* z */
    sv[1] = sinf (sb)*cosf (sa); /* x */
    sv[2] = sinf (sb)*sinf (sa); /* y */
    hv[0] = cosf (hb);          /* z */
    hv[1] = sinf (hb)*cosf (ha); /* x */
    hv[2] = sinf (hb)*sinf (ha); /* y */

    /* Scattering angle vector */
    ov[0] = -sv[0] + hv[0];
    ov[1] = -sv[1] + hv[1];
    ov[2] = -sv[2] + hv[2];
    /* Dip vector */
    dv[0] = sv[0] + hv[0];
    dv[1] = sv[1] + hv[1];
    dv[2] = sv[2] + hv[2];
    dl = sqrtf (dv[0]*dv[0] + dv[1]*dv[1] + dv[2]*dv[2]);

    if (fabsf (sb - hb) > 1e-4 ||
        fabsf (sa - ha) > 1e-4) {
        /* Scattering angle - dot product between vectors */
        a = sv[0]*hv[0] + sv[1]*hv[1] + sv[2]*hv[2];
        if (a > 1.0)
            a = 1.0;
        if (a < -1.0)
            a = -1.0;
        *oa = acosf (a);
        /* Cross-product between the two vectors - contained in the reflection plane */
        pv[0] = ov[1]*dv[2] - ov[2]*dv[1];
        pv[1] = ov[2]*dv[0] - ov[0]*dv[2];
        pv[2] = ov[0]*dv[1] - ov[1]*dv[0];
        /* Cross-product between the x direction (reference vector) and the dip -
           contained in the reflection plane */
        nv[0] = dv[2];  /* 1.0*dv[2] - 0.0*dv[1] */
        nv[1] = 0.0;    /* 0.0*dv[0] - 0.0*dv[2] */
        nv[2] = -dv[0]; /* 0.0*dv[1] - 1.0*dv[0] */
        /* Scattering angle azimuth - angle between the above two vectors in the
           reflection plane - see Fomel and Sava (2005) */
        dll = sqrtf (pv[0]*pv[0] + pv[1]*pv[1] + pv[2]*pv[2])*
              sqrtf (nv[0]*nv[0] + nv[1]*nv[1] + nv[2]*nv[2]);
        a = (pv[0]*nv[0] + pv[1]*nv[1] + pv[2]*nv[2])/dll;
        if (a > 1.0)
            a = 1.0;
        if (a < -1.0)
            a = -1.0;
        *oz = acosf (a);
        /* Cross product between the above two vectors - should be parallel
           to the dip */
        mv[0] = nv[1]*pv[2] - nv[2]*pv[1];
        mv[1] = nv[2]*pv[0] - nv[0]*pv[2];
        mv[2] = nv[0]*pv[1] - nv[1]*pv[0];
        dll = dl*sqrtf (mv[0]*mv[0] + mv[1]*mv[1] + mv[2]*mv[2]);
        /* Check if it has the same direction with the dip */
        a = (dv[0]*mv[0] + dv[1]*mv[1] + dv[2]*mv[2])/dll;
        if (a < 0.0) /* Otherwise, flip the scattering azimuth */
            *oz = SF_PI - *oz;
        else
            *oa *= -1.0;
    } else {
        /* Scattering angle */
        *oa = 0.0;
        /* Scattering angle azimuth */
        *oz = 0.0;
    }

    /* Dip angle - angle between z axis and dv */
    a = dv[0]/dl;
    *da = acosf (a);
    /* Dip angle azimuth - angle between projection
       of dv onto x-y plane and x component of dv */
    l = hypotf (dv[1], dv[2]);
    if (l != 0.0) {
        a = dv[1]/l;
        if (a > 1.0)
            a = 1.0;
        if (a < -1.0)
            a = -1.0;
        *dz = acosf (a);
        if (dv[2] < 0.0)
            *dz = SF_PI - *dz;
        else
            *da *= -1.0;
    } else {
        *dz = 0.0;
    }
}
Exemplo n.º 26
0
void world::set_location(const char *name)
{
    m_heights.clear();
    m_meshes.clear();
    m_qtree = nya_math::quadtree();

    fhm_file fhm;
    if (!fhm.open((std::string("Map/") + name + ".fhm").c_str()))
        return;

    assert(fhm.get_chunk_size(4) == location_size*location_size);
    fhm.read_chunk_data(4, m_height_patches);

    m_heights.resize(fhm.get_chunk_size(5)/4);
    assert(!m_heights.empty());
    fhm.read_chunk_data(5, &m_heights[0]);

    for (int i = 0; i < fhm.get_chunks_count(); ++i)
    {
        if (fhm.get_chunk_type(i) == 'HLOC')
        {
            nya_memory::tmp_buffer_scoped buf(fhm.get_chunk_size(i));
            fhm.read_chunk_data(i, buf.get_data());
            m_meshes.resize(m_meshes.size() + 1);
            m_meshes.back().load(buf.get_data(), buf.get_size());
        }
    }

    fhm.close();

    if (!fhm.open((std::string("Map/") + name + "_mpt.fhm").c_str()))
        return;

    assert(fhm.get_chunks_count() == m_meshes.size());

    for (int i = 0; i < fhm.get_chunks_count(); ++i)
    {
        assert(fhm.get_chunk_type(i) == 'xtpm');

        nya_memory::tmp_buffer_scoped buf(fhm.get_chunk_size(i));
        fhm.read_chunk_data(i, buf.get_data());
        nya_memory::memory_reader reader(buf.get_data(), buf.get_size());
        reader.seek(128);
        const auto off = m_instances.size();
        const auto count = reader.read<uint32_t>();
        reader.skip(16);
        m_instances.resize(off + count);
        for (uint32_t j = 0; j < count; ++j)
        {
            auto &inst = m_instances[off + j];
            inst.mesh_idx = i;
            inst.pos = reader.read<nya_math::vec3>();
            const float yaw = reader.read<float>();
            inst.yaw_s = sinf(yaw);
            inst.yaw_c = cosf(yaw);
            inst.bbox = nya_math::aabb(m_meshes[i].bbox, inst.pos,
                                       nya_math::quat(0.0f, yaw, 0.0f), nya_math::vec3(1.0f, 1.0f, 1.0f));
        }
    }

    fhm.close();

    const int size = 64000;
    m_qtree = nya_math::quadtree(-size, -size, size * 2, size * 2, 10);

    for(int i=0;i<(int)m_instances.size();++i)
        m_qtree.add_object(m_instances[i].bbox, i);
}
Exemplo n.º 27
0
void get_sight_vector(float rx, float ry, float *vx, float *vy, float *vz) {
    float m = cosf(ry);
    *vx = cosf(rx - RADIANS(90)) * m;
    *vy = sinf(ry);
    *vz = sinf(rx - RADIANS(90)) * m;
}
Exemplo n.º 28
0
void sphere::SetupShape(void)
{
	Erase((2 * resolution + 1) * (resolution + 1), 4 * (resolution + 1) * resolution); 
	// To Do
	// 
	// Setup the data for a sphere

	float pi = 3.14159265358979323846f;

	/*
	Strips refer to the "columns" of the sphere,
	where as, rings, refer to the "rows" of the sphere.
	*/



	int longitudeLines = resolution + 1;
	int latitudeLines = resolution + 1;
	nVerts = longitudeLines*latitudeLines;
	int numTriangleVerts = resolution*resolution * 2 * 3;
	int currentVertexIdx = 0;
	float currentPhi = 0.0f;
	float phiIncrement = (pi / (float)(resolution));
	float currentTheta = 0.0f;
	float thetaIncrement = 2.0f * (pi / (float)(resolution));

	for (int i = 0; i < longitudeLines; i++) //for each strip
	{
		currentPhi = 0.0f; //reset Phi since its a new strip
		for (int j = 0; j < latitudeLines; j++) //for each ring
		{
			//Note: Each face has four vertices...which will be used to make 

			vertices[currentVertexIdx].position = point(cosf(currentTheta)*sinf(currentPhi), sinf(currentTheta)*sinf(currentPhi), cosf(currentPhi)) * radius;
			vertices[currentVertexIdx].normal = vector(cosf(currentTheta)*sinf(currentPhi), sinf(currentTheta)*sinf(currentPhi), cosf(currentPhi));
			if (IsZero(vertices[currentVertexIdx].normal)){
				vertices[currentVertexIdx].normal.Normalize();
			}

			currentVertexIdx++; //skip forward to next idx for new face
			currentPhi += phiIncrement;
		}

		currentTheta += thetaIncrement; //go to next strip
	}

	for (int i = 0; i < resolution; i++) //for each strip
	{

		for (int j = 0; j < resolution; j++) //for each ring
		{
			//Note: Each face has four vertices...which will be used to make 

			faces[nFaces].vertices[0] = j + (i*latitudeLines);
			faces[nFaces].vertices[1] = j + (i*latitudeLines) + 1;
			faces[nFaces++].vertices[2] = j + ((i + 1)*latitudeLines);

			faces[nFaces].vertices[0] = j + ((i + 1)*latitudeLines);
			faces[nFaces].vertices[1] = j + (i*latitudeLines) + 1;
			faces[nFaces++].vertices[2] = j + ((i + 1)*latitudeLines) + 1;

		}
	}
}
Exemplo n.º 29
0
void Copter::update_ground_effect_detector(void)
{
    if(!motors.armed()) {
        // disarmed - disable ground effect and return
        gndeffect_state.takeoff_expected = false;
        gndeffect_state.touchdown_expected = false;
        ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected);
        ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected);
        return;
    }

    // variable initialization
    uint32_t tnow_ms = millis();
    float xy_des_speed_cms = 0.0f;
    float xy_speed_cms = 0.0f;
    float des_climb_rate_cms = pos_control.get_desired_velocity().z;

    if (pos_control.is_active_xy()) {
        Vector3f vel_target = pos_control.get_vel_target();
        vel_target.z = 0.0f;
        xy_des_speed_cms = vel_target.length();
    }

    if (position_ok() || optflow_position_ok()) {
        Vector3f vel = inertial_nav.get_velocity();
        vel.z = 0.0f;
        xy_speed_cms = vel.length();
    }

    // takeoff logic

    // if we are armed and haven't yet taken off
    if (motors.armed() && ap.land_complete && !gndeffect_state.takeoff_expected) {
        gndeffect_state.takeoff_expected = true;
    }

    // if we aren't taking off yet, reset the takeoff timer, altitude and complete flag
    bool throttle_up = mode_has_manual_throttle(control_mode) && g.rc_3.control_in > 0;
    if (!throttle_up && ap.land_complete) {
        gndeffect_state.takeoff_time_ms = tnow_ms;
        gndeffect_state.takeoff_alt_cm = inertial_nav.get_altitude();
    }

    // if we are in takeoff_expected and we meet the conditions for having taken off
    // end the takeoff_expected state
    if (gndeffect_state.takeoff_expected && (tnow_ms-gndeffect_state.takeoff_time_ms > 5000 || inertial_nav.get_altitude()-gndeffect_state.takeoff_alt_cm > 50.0f)) {
        gndeffect_state.takeoff_expected = false;
    }

    // landing logic
    Vector3f angle_target_rad = attitude_control.get_att_target_euler_cd() * radians(0.01f);
    bool small_angle_request = cosf(angle_target_rad.x)*cosf(angle_target_rad.y) > cosf(radians(7.5f));
    bool xy_speed_low = (position_ok() || optflow_position_ok()) && xy_speed_cms <= 125.0f;
    bool xy_speed_demand_low = pos_control.is_active_xy() && xy_des_speed_cms <= 125.0f;
    bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control.is_active_xy()) || (control_mode == ALT_HOLD && small_angle_request);

    bool descent_demanded = pos_control.is_active_z() && des_climb_rate_cms < 0.0f;
    bool slow_descent_demanded = descent_demanded && des_climb_rate_cms >= -100.0f;
    bool z_speed_low = abs(inertial_nav.get_velocity_z()) <= 60.0f;
    bool slow_descent = (slow_descent_demanded || (z_speed_low && descent_demanded));

    gndeffect_state.touchdown_expected = slow_horizontal && slow_descent;

    // Prepare the EKF for ground effect if either takeoff or touchdown is expected.
    ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected);
    ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected);
}
Exemplo n.º 30
0
LedgeId CLedgeManager::FindNearestLedge( const Vec3 &referencePosition, const Vec3 &testDirection, float maxDistance /*= 2.0f*/, float angleRange /*= DEG2RAD(35.0f)*/, float extendedAngleRange /*= DEG2RAD(50.0f)*/ ) const
{
	if (m_editorManager.IsInEditorMode())
	{
		return m_editorManager.FindNearestLedge( referencePosition, testDirection, maxDistance, angleRange, extendedAngleRange );
	}
	else
	{
		LedgeId bestLedgeId;

		float closestDistanceSq = maxDistance* maxDistance;
		const float fCosMaxAngleTable[2] = { cosf(angleRange), cosf(extendedAngleRange) };
		const float side[2] = { 1.0f, -1.0f };

		SLedgeInfo ledgeInfo;

		const uint32 ledgeObjectCount = m_levelLedges.m_ledgeCount;
		for(uint32 objectIdx = 0; objectIdx < ledgeObjectCount; ++objectIdx)
		{
			const SLedgeObject& ledgeObject = m_levelLedges.m_pLedgeObjects[objectIdx];
			const uint32 startMarkerIdx = ledgeObject.m_markersStartIdx;
			const uint32 endMarkerIdx = ledgeObject.m_markersStartIdx + ledgeObject.m_markersCount;
			CRY_ASSERT ( endMarkerIdx <= m_levelLedges.m_markerCount );
			
			const uint32 sideCount = 1 + ((ledgeObject.m_ledgeFlags[LedgeSide_In] & kLedgeFlag_isDoubleSided) != 0);
			const bool enabled = (ledgeObject.m_ledgeFlags[LedgeSide_In] & kLedgeFlag_enabled) != 0;

			CRY_ASSERT(sideCount <= 2);

			uint32 currentSide = 0;
			do 
			{
				for (uint32 markerIdx = startMarkerIdx; markerIdx < (endMarkerIdx - 1); ++markerIdx)
				{
					ELedgeFlagBitfield flags = kLedgeFlag_none;

					if (m_levelLedges.m_pMarkers[markerIdx].m_endOrCorner)
					{
						flags |= kledgeRunTimeOnlyFlag_p0IsEndOrCorner;
					}
					if (m_levelLedges.m_pMarkers[markerIdx+1].m_endOrCorner)
					{
						flags |= kledgeRunTimeOnlyFlag_p1IsEndOrCorner;
					}

					ledgeInfo = SLedgeInfo( ledgeObject.m_entityId, m_levelLedges.m_pMarkers[markerIdx].m_worldPosition, m_levelLedges.m_pMarkers[markerIdx+1].m_worldPosition,
						m_levelLedges.m_pMarkers[markerIdx].m_facingDirection * side[currentSide], flags, ledgeObject.m_ledgeCornerEndAdjustAmount );

					// Explanation: (Please do not delete this comment)
					//	The item can be skipped if the angle is too big.
					//	Since only the cosine of angles are compared,
					//	bigger angles result in smaller values (hence the less_than comparison)
					const uint32 thresholdIdx = ((ledgeObject.m_ledgeFlags[currentSide] & (kLedgeFlag_useVault|kLedgeFlag_useHighVault)) != 0);
					CRY_ASSERT( thresholdIdx < 2 );

					const float fCosMaxAngle = fCosMaxAngleTable[thresholdIdx];

					const Vec3 vPosToLedge = _FindVectorToClosestPointOnLedge( referencePosition, ledgeInfo );

					float distanceSq;
					if( IsBestLedge( vPosToLedge, testDirection, ledgeInfo, closestDistanceSq, fCosMaxAngle, enabled, distanceSq ) == false )
						continue;
					
					bestLedgeId = LedgeId( objectIdx, (markerIdx - startMarkerIdx), currentSide );
					closestDistanceSq = distanceSq;
				}

				currentSide++;

			} while ( currentSide < sideCount );
		}

		return bestLedgeId;
	}
}