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);
}
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;
}
}
}
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;
}
// *****************************************************************
//
// *****************************************************************
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;
}
}
}
}
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 = ¤tProfile[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;
}
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);
}
/// 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);
}
//
/// \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;
}
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);
}
}
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;
};
/*
// 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;
}
// 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;
}
}
/**
* 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();
}
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
}
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;
}
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;
}
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];
}
}
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));
}
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);
}
float LightingApp::GetHillHeight(float x, float z)const
{
return 0.3f*(z*sinf(0.1f*x) + x*cosf(0.1f*z));
}
// 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);
}
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);
}
/*
================
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 ();
}
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);
}
/* 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;
}
}
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);
}
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;
}
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;
}
}
}
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);
}
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;
}
}
