void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
{
m_velocitySampleCount = 0;
const int debugIdx = debug ? debug->idx : -1;
dtCrowdAgent** agents = m_activeAgents;
int nagents = getActiveAgents(agents, m_maxAgents);
// Check that all agents still have valid paths.
checkPathValidity(agents, nagents, dt);
// Update async move request and path finder.
updateMoveRequest(dt);
// Optimize path topology.
updateTopologyOptimization(agents, nagents, dt);
// Register agents to proximity grid.
m_grid->clear();
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
const float* p = ag->npos;
const float r = ag->params.radius;
m_grid->addItem((unsigned short)i, p[0]-r, p[2]-r, p[0]+r, p[2]+r);
}
// Get nearby navmesh segments and agents to collide with.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
// Update the collision boundary after certain distance has been passed or
// if it has become invalid.
const float updateThr = ag->params.collisionQueryRange*0.25f;
if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(updateThr) ||
!ag->boundary.isValid(m_navquery, &m_filter))
{
ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange,
m_navquery, &m_filter);
}
// Query neighbour agents
ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange,
ag, ag->neis, DT_CROWDAGENT_MAX_NEIGHBOURS,
agents, nagents, m_grid);
for (int j = 0; j < ag->nneis; j++)
ag->neis[j].idx = getAgentIndex(agents[ag->neis[j].idx]);
}
// Find next corner to steer to.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
// Find corners for steering
ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys,
DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filter);
// Check to see if the corner after the next corner is directly visible,
// and short cut to there.
if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_VIS) && ag->ncorners > 0)
{
const float* target = &ag->cornerVerts[dtMin(1,ag->ncorners-1)*3];
ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery, &m_filter);
// Copy data for debug purposes.
if (debugIdx == i)
{
dtVcopy(debug->optStart, ag->corridor.getPos());
dtVcopy(debug->optEnd, target);
}
}
else
{
// Copy data for debug purposes.
if (debugIdx == i)
{
dtVset(debug->optStart, 0,0,0);
dtVset(debug->optEnd, 0,0,0);
}
}
}
// Trigger off-mesh connections (depends on corners).
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
// Check
const float triggerRadius = ag->params.radius*2.25f;
if (overOffmeshConnection(ag, triggerRadius))
{
// Prepare to off-mesh connection.
const int idx = ag - m_agents;
dtCrowdAgentAnimation* anim = &m_agentAnims[idx];
// Adjust the path over the off-mesh connection.
dtPolyRef refs[2];
if (ag->corridor.moveOverOffmeshConnection(ag->cornerPolys[ag->ncorners-1], refs,
anim->startPos, anim->endPos, m_navquery))
{
dtVcopy(anim->initPos, ag->npos);
anim->polyRef = refs[1];
anim->active = 1;
anim->t = 0.0f;
anim->tmax = (dtVdist2D(anim->startPos, anim->endPos) / ag->params.maxSpeed) * 0.5f;
ag->state = DT_CROWDAGENT_STATE_OFFMESH;
ag->ncorners = 0;
ag->nneis = 0;
continue;
}
else
{
// Path validity check will ensure that bad/blocked connections will be replanned.
}
}
}
// Calculate steering.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE)
continue;
float dvel[3] = {0,0,0};
if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
{
dtVcopy(dvel, ag->targetPos);
ag->desiredSpeed = dtVlen(ag->targetPos);
}
else
{
// Calculate steering direction.
if (ag->params.updateFlags & DT_CROWD_ANTICIPATE_TURNS)
calcSmoothSteerDirection(ag, dvel);
else
calcStraightSteerDirection(ag, dvel);
// Calculate speed scale, which tells the agent to slowdown at the end of the path.
const float slowDownRadius = ag->params.radius*2; // TODO: make less hacky.
const float speedScale = getDistanceToGoal(ag, slowDownRadius) / slowDownRadius;
ag->desiredSpeed = ag->params.maxSpeed;
dtVscale(dvel, dvel, ag->desiredSpeed * speedScale);
}
// Separation
if (ag->params.updateFlags & DT_CROWD_SEPARATION)
{
const float separationDist = ag->params.collisionQueryRange;
const float invSeparationDist = 1.0f / separationDist;
const float separationWeight = ag->params.separationWeight;
float w = 0;
float disp[3] = {0,0,0};
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
float diff[3];
dtVsub(diff, ag->npos, nei->npos);
diff[1] = 0;
const float distSqr = dtVlenSqr(diff);
if (distSqr < 0.00001f)
continue;
if (distSqr > dtSqr(separationDist))
continue;
const float dist = sqrtf(distSqr);
const float weight = separationWeight * (1.0f - dtSqr(dist*invSeparationDist));
dtVmad(disp, disp, diff, weight/dist);
w += 1.0f;
}
if (w > 0.0001f)
{
// Adjust desired velocity.
dtVmad(dvel, dvel, disp, 1.0f/w);
// Clamp desired velocity to desired speed.
const float speedSqr = dtVlenSqr(dvel);
const float desiredSqr = dtSqr(ag->desiredSpeed);
if (speedSqr > desiredSqr)
dtVscale(dvel, dvel, desiredSqr/speedSqr);
}
}
// Set the desired velocity.
dtVcopy(ag->dvel, dvel);
}
// Velocity planning.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->params.updateFlags & DT_CROWD_OBSTACLE_AVOIDANCE)
{
m_obstacleQuery->reset();
// Add neighbours as obstacles.
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
m_obstacleQuery->addCircle(nei->npos, nei->params.radius, nei->vel, nei->dvel);
}
// Append neighbour segments as obstacles.
for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
{
const float* s = ag->boundary.getSegment(j);
if (dtTriArea2D(ag->npos, s, s+3) < 0.0f)
continue;
m_obstacleQuery->addSegment(s, s+3);
}
dtObstacleAvoidanceDebugData* vod = 0;
if (debugIdx == i)
vod = debug->vod;
// Sample new safe velocity.
bool adaptive = true;
int ns = 0;
const dtObstacleAvoidanceParams* params = &m_obstacleQueryParams[ag->params.obstacleAvoidanceType];
if (adaptive)
{
ns = m_obstacleQuery->sampleVelocityAdaptive(ag->npos, ag->params.radius, ag->desiredSpeed,
ag->vel, ag->dvel, ag->nvel, params, vod);
}
else
{
ns = m_obstacleQuery->sampleVelocityGrid(ag->npos, ag->params.radius, ag->desiredSpeed,
ag->vel, ag->dvel, ag->nvel, params, vod);
}
m_velocitySampleCount += ns;
}
else
{
// If not using velocity planning, new velocity is directly the desired velocity.
dtVcopy(ag->nvel, ag->dvel);
}
}
// Integrate.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
integrate(ag, dt);
}
// Handle collisions.
static const float COLLISION_RESOLVE_FACTOR = 0.7f;
for (int iter = 0; iter < 4; ++iter)
{
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
const int idx0 = getAgentIndex(ag);
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
dtVset(ag->disp, 0,0,0);
float w = 0;
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
const int idx1 = getAgentIndex(nei);
float diff[3];
dtVsub(diff, ag->npos, nei->npos);
diff[1] = 0;
float dist = dtVlenSqr(diff);
if (dist > dtSqr(ag->params.radius + nei->params.radius))
continue;
dist = sqrtf(dist);
float pen = (ag->params.radius + nei->params.radius) - dist;
if (dist < 0.0001f)
{
// Agents on top of each other, try to choose diverging separation directions.
if (idx0 > idx1)
dtVset(diff, -ag->dvel[2],0,ag->dvel[0]);
else
dtVset(diff, ag->dvel[2],0,-ag->dvel[0]);
pen = 0.01f;
}
else
{
pen = (1.0f/dist) * (pen*0.5f) * COLLISION_RESOLVE_FACTOR;
}
dtVmad(ag->disp, ag->disp, diff, pen);
w += 1.0f;
}
if (w > 0.0001f)
{
const float iw = 1.0f / w;
dtVscale(ag->disp, ag->disp, iw);
}
}
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
dtVadd(ag->npos, ag->npos, ag->disp);
}
}
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
// Move along navmesh.
ag->corridor.movePosition(ag->npos, m_navquery, &m_filter);
// Get valid constrained position back.
dtVcopy(ag->npos, ag->corridor.getPos());
// If not using path, truncate the corridor to just one poly.
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
{
ag->corridor.reset(ag->corridor.getFirstPoly(), ag->npos);
}
}
// Update agents using off-mesh connection.
for (int i = 0; i < m_maxAgents; ++i)
{
dtCrowdAgentAnimation* anim = &m_agentAnims[i];
if (!anim->active)
continue;
dtCrowdAgent* ag = agents[i];
anim->t += dt;
if (anim->t > anim->tmax)
{
// Reset animation
anim->active = 0;
// Prepare agent for walking.
ag->state = DT_CROWDAGENT_STATE_WALKING;
continue;
}
// Update position
const float ta = anim->tmax*0.15f;
const float tb = anim->tmax;
if (anim->t < ta)
{
const float u = tween(anim->t, 0.0, ta);
dtVlerp(ag->npos, anim->initPos, anim->startPos, u);
}
else
{
const float u = tween(anim->t, ta, tb);
dtVlerp(ag->npos, anim->startPos, anim->endPos, u);
}
// Update velocity.
dtVset(ag->vel, 0,0,0);
dtVset(ag->dvel, 0,0,0);
}
}
float App::compute_gaussian(float n, float theta) // theta = Blur Amount
{
return (float)((1.0f / sqrtf(2 * (float)clan::PI * theta)) * expf(-(n * n) / (2.0f * theta * theta)));
}
void AudioNoiseWidget::paintEvent(QPaintEvent *) {
QPainter paint(this);
QPalette pal;
paint.fillRect(rect(), pal.color(QPalette::Background));
AudioInputPtr ai = g.ai;
if (ai.get() == NULL || ! ai->sppPreprocess)
return;
QPolygonF poly;
ai->qmSpeex.lock();
spx_int32_t ps_size = 0;
speex_preprocess_ctl(ai->sppPreprocess, SPEEX_PREPROCESS_GET_PSD_SIZE, &ps_size);
STACKVAR(spx_int32_t, noise, ps_size);
STACKVAR(spx_int32_t, ps, ps_size);
speex_preprocess_ctl(ai->sppPreprocess, SPEEX_PREPROCESS_GET_PSD, ps);
speex_preprocess_ctl(ai->sppPreprocess, SPEEX_PREPROCESS_GET_NOISE_PSD, noise);
ai->qmSpeex.unlock();
qreal sx, sy;
sx = (static_cast<float>(width()) - 1.0f) / static_cast<float>(ps_size);
sy = static_cast<float>(height()) - 1.0f;
poly << QPointF(0.0f, height() - 1);
float fftmul = 1.0 / (32768.0);
for (int i=0; i < ps_size; i++) {
qreal xp, yp;
xp = i * sx;
yp = sqrtf(sqrtf(static_cast<float>(noise[i]))) - 1.0f;
yp = yp * fftmul;
yp = qMin<qreal>(yp * 3000.0f, 1.0f);
yp = (1 - yp) * sy;
poly << QPointF(xp, yp);
}
poly << QPointF(width() - 1, height() - 1);
poly << QPointF(0.0f, height() - 1);
paint.setPen(Qt::blue);
paint.setBrush(Qt::blue);
paint.drawPolygon(poly);
poly.clear();
for (int i=0;i < ps_size; i++) {
qreal xp, yp;
xp = i * sx;
yp = sqrtf(sqrtf(static_cast<float>(ps[i]))) - 1.0f;
yp = yp * fftmul;
yp = qMin(yp * 3000.0, 1.0);
yp = (1 - yp) * sy;
poly << QPointF(xp, yp);
}
paint.setPen(Qt::red);
paint.drawPolyline(poly);
}
float rcSqrt(float x)
{
return sqrtf(x);
}
float mc_next_scatter(float g, float3 *dir,RandType *ran, RandType *ran0, mcconfig *cfg, float *pmom){
float nextslen;
float sphi,cphi,tmp0,theta,stheta,ctheta,tmp1;
float3 p;
rand_need_more(ran,ran0);
//random scattering length (normalized)
#ifdef MMC_USE_SSE_MATH
nextslen=rand_next_scatlen_ps(ran);
#else
nextslen=rand_next_scatlen(ran);
#endif
//random arimuthal angle
#ifdef MMC_USE_SSE_MATH
rand_next_aangle_sincos(ran,&sphi,&cphi);
#else
tmp0=TWO_PI*rand_next_aangle(ran); //next arimuth angle
mmc_sincosf(tmp0,&sphi,&cphi);
#endif
//Henyey-Greenstein Phase Function, "Handbook of Optical Biomedical Diagnostics",2002,Chap3,p234
//see Boas2002
if(g>EPS){ //if g is too small, the distribution of theta is bad
tmp0=(1.f-g*g)/(1.f-g+2.f*g*rand_next_zangle(ran));
tmp0*=tmp0;
tmp0=(1.f+g*g-tmp0)/(2.f*g);
// when ran=1, CUDA will give me 1.000002 for tmp0 which produces nan later
if(tmp0> 1.f) tmp0=1.f;
if(tmp0<-1.f) tmp0=-1.f;
theta=acosf(tmp0);
stheta=sqrt(1.f-tmp0*tmp0);
//stheta=sinf(theta);
ctheta=tmp0;
}else{
theta=acosf(2.f*rand_next_zangle(ran)-1.f);
mmc_sincosf(theta,&stheta,&ctheta);
}
if( dir->z>-1.f+EPS && dir->z<1.f-EPS ) {
tmp0=1.f-dir->z*dir->z; //reuse tmp to minimize registers
tmp1=1.f/sqrtf(tmp0);
tmp1=stheta*tmp1;
p.x=tmp1*(dir->x*dir->z*cphi - dir->y*sphi) + dir->x*ctheta;
p.y=tmp1*(dir->y*dir->z*cphi + dir->x*sphi) + dir->y*ctheta;
p.z=-tmp1*tmp0*cphi + dir->z*ctheta;
}else{
p.x=stheta*cphi;
p.y=stheta*sphi;
p.z=(dir->z>0.f)?ctheta:-ctheta;
}
if (cfg->ismomentum)
pmom[0]+=(1.f-ctheta);
dir->x=p.x;
dir->y=p.y;
dir->z=p.z;
return nextslen;
}
/*
** Function called to update rendering
*/
void DisplayFunc (void)
{
const float t = glutGet (GLUT_ELAPSED_TIME) / 1000.;
const float delta = 2. / RESOLUTION;
const unsigned int length = 2 * (RESOLUTION + 1);
const float xn = (RESOLUTION + 1) * delta + 1;
unsigned int i;
unsigned int j;
float x;
float y;
unsigned int indice;
unsigned int preindice;
/* Yes, I know, this is quite ugly... */
float v1x;
float v1y;
float v1z;
float v2x;
float v2y;
float v2z;
float v3x;
float v3y;
float v3z;
float vax;
float vay;
float vaz;
float vbx;
float vby;
float vbz;
float nx;
float ny;
float nz;
float l;
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glLoadIdentity ();
glTranslatef (0, 0, -translate_z);
glRotatef (rotate_y, 1, 0, 0);
glRotatef (rotate_x, 0, 1, 0);
/* Vertices */
for (j = 0; j < RESOLUTION; j++)
{
y = (j + 1) * delta - 1;
for (i = 0; i <= RESOLUTION; i++)
{
indice = 6 * (i + j * (RESOLUTION + 1));
x = i * delta - 1;
surface[indice + 3] = x;
surface[indice + 4] = z (x, y, t);
surface[indice + 5] = y;
if (j != 0)
{
/* Values were computed during the previous loop */
preindice = 6 * (i + (j - 1) * (RESOLUTION + 1));
surface[indice] = surface[preindice + 3];
surface[indice + 1] = surface[preindice + 4];
surface[indice + 2] = surface[preindice + 5];
}
else
{
surface[indice] = x;
surface[indice + 1] = z (x, -1, t);
surface[indice + 2] = -1;
}
}
}
/* Normals */
for (j = 0; j < RESOLUTION; j++)
for (i = 0; i <= RESOLUTION; i++)
{
indice = 6 * (i + j * (RESOLUTION + 1));
v1x = surface[indice + 3];
v1y = surface[indice + 4];
v1z = surface[indice + 5];
v2x = v1x;
v2y = surface[indice + 1];
v2z = surface[indice + 2];
if (i < RESOLUTION)
{
v3x = surface[indice + 9];
v3y = surface[indice + 10];
v3z = v1z;
}
else
{
v3x = xn;
v3y = z (xn, v1z, t);
v3z = v1z;
}
vax = v2x - v1x;
vay = v2y - v1y;
vaz = v2z - v1z;
vbx = v3x - v1x;
vby = v3y - v1y;
vbz = v3z - v1z;
nx = (vby * vaz) - (vbz * vay);
ny = (vbz * vax) - (vbx * vaz);
nz = (vbx * vay) - (vby * vax);
l = sqrtf (nx * nx + ny * ny + nz * nz);
if (l != 0)
{
l = 1 / l;
normal[indice + 3] = nx * l;
normal[indice + 4] = ny * l;
normal[indice + 5] = nz * l;
}
else
{
normal[indice + 3] = 0;
normal[indice + 4] = 1;
normal[indice + 5] = 0;
}
if (j != 0)
{
/* Values were computed during the previous loop */
preindice = 6 * (i + (j - 1) * (RESOLUTION + 1));
normal[indice] = normal[preindice + 3];
normal[indice + 1] = normal[preindice + 4];
normal[indice + 2] = normal[preindice + 5];
}
else
{
/* v1x = v1x; */
v1y = z (v1x, (j - 1) * delta - 1, t);
v1z = (j - 1) * delta - 1;
/* v3x = v3x; */
v3y = z (v3x, v2z, t);
v3z = v2z;
vax = v1x - v2x;
vay = v1y - v2y;
vaz = v1z - v2z;
vbx = v3x - v2x;
vby = v3y - v2y;
vbz = v3z - v2z;
nx = (vby * vaz) - (vbz * vay);
ny = (vbz * vax) - (vbx * vaz);
nz = (vbx * vay) - (vby * vax);
l = sqrtf (nx * nx + ny * ny + nz * nz);
if (l != 0)
{
l = 1 / l;
normal[indice] = nx * l;
normal[indice + 1] = ny * l;
normal[indice + 2] = nz * l;
}
else
{
normal[indice] = 0;
normal[indice + 1] = 1;
normal[indice + 2] = 0;
}
}
}
/* The ground */
glPolygonMode (GL_FRONT_AND_BACK, GL_FILL);
glDisable (GL_TEXTURE_2D);
glColor3f (1, 0.9, 0.7);
glBegin (GL_TRIANGLE_FAN);
glVertex3f (-1, 0, -1);
glVertex3f (-1, 0, 1);
glVertex3f ( 1, 0, 1);
glVertex3f ( 1, 0, -1);
glEnd ();
glTranslatef (0, 0.2, 0);
/* Render wireframe? */
if (wire_frame != 0)
glPolygonMode (GL_FRONT_AND_BACK, GL_LINE);
/* The water */
glEnable (GL_TEXTURE_2D);
glColor3f (1, 1, 1);
glEnableClientState (GL_NORMAL_ARRAY);
glEnableClientState (GL_VERTEX_ARRAY);
glNormalPointer (GL_FLOAT, 0, normal);
glVertexPointer (3, GL_FLOAT, 0, surface);
for (i = 0; i < RESOLUTION; i++)
glDrawArrays (GL_TRIANGLE_STRIP, i * length, length);
/* Draw normals? */
if (normals != 0)
{
glDisable (GL_TEXTURE_2D);
glColor3f (1, 0, 0);
glBegin (GL_LINES);
for (j = 0; j < RESOLUTION; j++)
for (i = 0; i <= RESOLUTION; i++)
{
indice = 6 * (i + j * (RESOLUTION + 1));
glVertex3fv (&(surface[indice]));
glVertex3f (surface[indice] + normal[indice] / 50,
surface[indice + 1] + normal[indice + 1] / 50,
surface[indice + 2] + normal[indice + 2] / 50);
}
glEnd ();
}
/* End */
glFlush ();
glutSwapBuffers ();
glutPostRedisplay ();
}
static float RdIntegral(float alphap, float A) {
float sqrtTerm = sqrtf(3.f * (1.f - alphap));
return alphap / 2.f * (1.f + expf(-4.f/3.f * A * sqrtTerm)) *
expf(-sqrtTerm);
}
void Functions::vectorMagnitude(Aurora::NWScript::FunctionContext &ctx) {
float x, y, z;
ctx.getParams()[0].getVector(x, y, z);
ctx.getReturn() = sqrtf(x*x + y*y + z*z);
}
void Functions::sqrt(Aurora::NWScript::FunctionContext &ctx) {
ctx.getReturn() = sqrtf(ctx.getParams()[0].getFloat());
}
static void compute_step_tv2_inner_simd(unsigned w, unsigned h, unsigned nchannel, struct aux auxs[nchannel], float alpha, unsigned x, unsigned y, double *tv2) {
__m128 g_xxs[3] = {0};
__m128 g_xy_syms[3] = {0};
__m128 g_yys[3] = {0};
const __m128 mtwo = _mm_set_ps1(2.);
const __m128 minf = _mm_set_ps1(INFINITY);
const __m128 mzero = _mm_set_ps1(0.);
__m128 malpha = _mm_set_ps1(alpha * 1./sqrtf(nchannel));
for(unsigned c = 0; c < nchannel; c++) {
struct aux *aux = &auxs[c];
__m128 g_x = _mm_load_ps(p(aux->temp[0], x, y, w, h));
__m128 g_y = _mm_load_ps(p(aux->temp[1], x, y, w, h));
// backward x
g_xxs[c] = g_x - _mm_loadu_ps(p(aux->temp[0], x-1, y, w, h));
// backward x
__m128 g_yx = g_y - _mm_loadu_ps(p(aux->temp[1], x-1, y, w, h));
// backward y
__m128 g_xy = g_x - _mm_load_ps(p(aux->temp[0], x, y-1, w, h));
// backward y
g_yys[c] = g_y - _mm_load_ps(p(aux->temp[1], x, y-1, w, h));
// symmetrize
g_xy_syms[c] = (g_xy + g_yx) / mtwo;
}
// norm
__m128 g2_norm = mzero;
for(unsigned c = 0; c < nchannel; c++) {
g2_norm += SQR(g_xxs[c]) + mtwo * SQR(g_xy_syms[c]) + SQR(g_yys[c]);
}
g2_norm = _mm_sqrt_ps(g2_norm);
__m128 alpha_norm = malpha * g2_norm;
*tv2 += alpha_norm[0];
*tv2 += alpha_norm[1];
*tv2 += alpha_norm[2];
*tv2 += alpha_norm[3];
// set zeroes to infinity
g2_norm = _mm_or_ps(g2_norm, _mm_and_ps(minf, _mm_cmpeq_ps(g2_norm, mzero)));
for(unsigned c = 0; c < nchannel; c++) {
__m128 g_xx = g_xxs[c];
__m128 g_yy = g_yys[c];
__m128 g_xy_sym = g_xy_syms[c];
struct aux *aux = &auxs[c];
// N.B. for same exact result as the c version,
// we must calculate the objective gradient from right to left
{
float *pobj_ur = p(aux->obj_gradient, x+1, y-1, w, h);
__m128 obj_ur = _mm_loadu_ps(pobj_ur);
obj_ur += malpha * ((-g_xy_sym) / g2_norm);
_mm_storeu_ps(pobj_ur, obj_ur);
}
{
float *pobj_r = p(aux->obj_gradient, x+1, y, w, h);
__m128 obj_r = _mm_loadu_ps(pobj_r);
obj_r += malpha * ((g_xy_sym + g_xx) / g2_norm);
_mm_storeu_ps(pobj_r, obj_r);
}
{
float *pobj_u = p(aux->obj_gradient, x, y-1, w, h);
__m128 obj_u = _mm_load_ps(pobj_u);
obj_u += malpha * ((g_yy + g_xy_sym) / g2_norm);
_mm_store_ps(pobj_u, obj_u);
}
{
float *pobj = p(aux->obj_gradient, x, y, w, h);
__m128 obj = _mm_load_ps(pobj);
obj += malpha * (-(mtwo * g_xx + mtwo * g_xy_sym + mtwo * g_yy) / g2_norm);
_mm_store_ps(pobj, obj);
}
{
float *pobj_b = p(aux->obj_gradient, x, y+1, w, h);
__m128 obj_b = _mm_load_ps(pobj_b);
obj_b += malpha * ((g_yy + g_xy_sym) / g2_norm);
_mm_store_ps(pobj_b, obj_b);
}
{
float *pobj_l = p(aux->obj_gradient, x-1, y, w, h);
__m128 obj_l = _mm_loadu_ps(pobj_l);
obj_l += malpha * ((g_xy_sym + g_xx) / g2_norm);
_mm_storeu_ps(pobj_l, obj_l);
}
{
float *pobj_lb = p(aux->obj_gradient, x-1, y+1, w, h);
__m128 obj_lb = _mm_loadu_ps(pobj_lb);
obj_lb += malpha * ((-g_xy_sym) / g2_norm);
_mm_storeu_ps(pobj_lb, obj_lb);
}
}
}
static void compute_step_tv_inner_simd(unsigned w, unsigned h, unsigned nchannel, struct aux auxs[nchannel], unsigned x, unsigned y, double *tv) {
const __m128 minf = _mm_set_ps1(INFINITY);
const __m128 mzero = _mm_set_ps1(0.);
__m128 g_xs[3] = {0};
__m128 g_ys[3] = {0};
for(unsigned c = 0; c < nchannel; c++) {
struct aux *aux = &auxs[c];
__m128 here = _mm_load_ps(p(aux->fdata, x, y, w, h));
// forward gradient x
g_xs[c] = _mm_loadu_ps(p(aux->fdata, x+1, y, w, h)) - here;
// forward gradient y
g_ys[c] = _mm_loadu_ps(p(aux->fdata, x, y+1, w, h)) - here;
}
// norm
__m128 g_norm = mzero;
for(unsigned c = 0; c < nchannel; c++) {
g_norm += SQR(g_xs[c]);
g_norm += SQR(g_ys[c]);
}
g_norm = _mm_sqrt_ps(g_norm);
float alpha = 1./sqrtf(nchannel);
*tv += alpha * g_norm[0];
*tv += alpha * g_norm[1];
*tv += alpha * g_norm[2];
*tv += alpha * g_norm[3];
__m128 malpha = _mm_set_ps1(alpha);
// set zeroes to infinity
g_norm = _mm_or_ps(g_norm, _mm_and_ps(minf, _mm_cmpeq_ps(g_norm, mzero)));
// compute derivatives
for(unsigned c = 0; c < nchannel; c++) {
__m128 g_x = g_xs[c];
__m128 g_y = g_ys[c];
struct aux *aux = &auxs[c];
// N.B. for numerical stability and same exact result as the c version,
// we must calculate the objective gradient at x+1 before x
{
float *pobj_r = p(aux->obj_gradient, x+1, y, w, h);
__m128 obj_r = _mm_loadu_ps(pobj_r);
obj_r += malpha * g_x / g_norm;
_mm_storeu_ps(pobj_r, obj_r);
}
{
float *pobj = p(aux->obj_gradient, x, y, w, h);
__m128 obj = _mm_load_ps(pobj);
obj += malpha * -(g_x + g_y) / g_norm;
_mm_store_ps(pobj, obj);
}
{
float *pobj_b = p(aux->obj_gradient, x, y+1, w, h);
__m128 obj_b = _mm_load_ps(pobj_b);
obj_b += malpha * g_y / g_norm;
_mm_store_ps(pobj_b, obj_b);
}
}
// store
for(unsigned c = 0; c < nchannel; c++) {
struct aux *aux = &auxs[c];
_mm_store_ps(p(aux->temp[0], x, y, w, h), g_xs[c]);
_mm_store_ps(p(aux->temp[1], x, y, w, h), g_ys[c]);
}
}
int main(int argc, char *argv[])
{
int i, k, n1, n2, n12, n[2], niter, iter, liter, rect[2];
float mean, a0, ai, norm, norm2, lam;
float **dat, **ang, **p1, **p2, ***den, *dena, *rat, **out;
sf_file inp, dip;
sf_init(argc,argv);
inp = sf_input("in");
dip = sf_output("out");
if (SF_FLOAT != sf_gettype(inp)) sf_error("Need float input");
if (!sf_histint(inp,"n1",&n1)) sf_error("No n1= in input");
if (!sf_histint(inp,"n2",&n2)) sf_error("No n2= in input");
n12 = n1*n2;
n[0] = n1;
n[1] = n2;
if (!sf_getint("liter",&liter)) liter=100;
/* number of linear iterations */
if (!sf_getint("niter",&niter)) niter=10;
/* number of iterations */
if (!sf_getint("rect1",&rect[0])) rect[0]=1;
/* vertical smoothing */
if (!sf_getint("rect2",&rect[1])) rect[1]=1;
/* horizontal smoothing */
if (!sf_getfloat("a0",&a0)) a0=0;
/* initial angle */
dat = sf_floatalloc2(n1,n2);
ang = sf_floatalloc2(n1,n2);
out = sf_floatalloc2(n1,n2);
p1 = sf_floatalloc2(n1,n2);
p2 = sf_floatalloc2(n1,n2);
den = sf_floatalloc3(n1,n2,2);
dena = sf_floatalloc(n12);
rat = sf_floatalloc(n12);
sf_floatread(dat[0],n12,inp);
for (i=0; i < n12; i++) {
ang[0][i] = a0;
p1[0][i] = sinf(a0);
p2[0][i] = cosf(a0);
}
opwd_init(n1,n2);
sf_divn_init(2, n12, n, rect, liter, true);
opwd_filter(lagrange,lagrange,NULL,NULL,p1,p2,dat,out);
norm = 0.;
for (i=0; i < n12; i++) {
out[0][i] = dat[0][i] - out[0][i];
norm += out[0][i]*out[0][i];
}
for (iter=0; iter < niter; iter++) {
sf_warning("iter=%d of %d",iter+1,niter);
opwd_filter(lagrange_der,lagrange,NULL,NULL,p1,p2,dat,den[0]);
opwd_filter(lagrange,lagrange_der,NULL,NULL,p1,p2,dat,den[1]);
for(i=0; i < n12; i++) {
dena[i] = den[0][0][i]*p2[0][i]-den[1][0][i]*p1[0][i];
}
mean = 0.;
for(i=0; i < n12; i++) {
mean += dena[i]*dena[i];
}
mean = sqrtf (n12/mean);
for(i=0; i < n12; i++) {
out[0][i] *= mean;
dena[i] *= mean;
}
sf_divn (out[0],dena,rat);
/* Choose step size */
lam = 1.;
for (k=0; k < 8; k++) {
for(i=0; i < n12; i++) {
ai = ang[0][i] + lam*rat[i];
if (ai < -0.5*SF_PI) ai=-0.5*SF_PI;
else if (ai > 0.5*SF_PI) ai= 0.5*SF_PI;
p1[0][i] = sinf(ai);
p2[0][i] = cosf(ai);
}
opwd_filter(lagrange,lagrange,NULL,NULL,p1,p2,dat,out);
norm2 = 0.;
for (i=0; i < n12; i++) {
out[0][i] = dat[0][i] - out[0][i];
norm2 += out[0][i]*out[0][i];
}
if (norm2 < norm) break;
lam *= 0.5;
}
for(i=0; i < n12; i++) {
ang[0][i] += lam*rat[i];
norm = norm2;
}
}
sf_floatwrite(ang[0],n12,dip);
exit(0);
}
/*
* Reduces the input array (in place)
* length specifies the length of the array
*/
int
BinomialOption::binomialOptionCPUReference()
{
refOutput = (float*)malloc(numSamples * sizeof(cl_float4));
CHECK_ALLOCATION(refOutput, "Failed to allocate host memory. (refOutput)");
float* stepsArray = (float*)malloc((numSteps + 1) * sizeof(cl_float4));
CHECK_ALLOCATION(stepsArray, "Failed to allocate host memory. (stepsArray)");
// Iterate for all samples
for(int bid = 0; bid < numSamples; ++bid)
{
float s[4];
float x[4];
float vsdt[4];
float puByr[4];
float pdByr[4];
float optionYears[4];
float inRand[4];
for(int i = 0; i < 4; ++i)
{
inRand[i] = randArray[bid + i];
s[i] = (1.0f - inRand[i]) * 5.0f + inRand[i] * 30.f;
x[i] = (1.0f - inRand[i]) * 1.0f + inRand[i] * 100.f;
optionYears[i] = (1.0f - inRand[i]) * 0.25f + inRand[i] * 10.f;
float dt = optionYears[i] * (1.0f / (float)numSteps);
vsdt[i] = VOLATILITY * sqrtf(dt);
float rdt = RISKFREE * dt;
float r = expf(rdt);
float rInv = 1.0f / r;
float u = expf(vsdt[i]);
float d = 1.0f / u;
float pu = (r - d)/(u - d);
float pd = 1.0f - pu;
puByr[i] = pu * rInv;
pdByr[i] = pd * rInv;
}
/**
* Compute values at expiration date:
* Call option value at period end is v(t) = s(t) - x
* If s(t) is greater than x, or zero otherwise...
* The computation is similar for put options...
*/
for(int j = 0; j <= numSteps; j++)
{
for(int i = 0; i < 4; ++i)
{
float profit = s[i] * expf(vsdt[i] * (2.0f * j - numSteps)) - x[i];
stepsArray[j * 4 + i] = profit > 0.0f ? profit : 0.0f;
}
}
/**
* walk backwards up on the binomial tree of depth numSteps
* Reduce the price step by step
*/
for(int j = numSteps; j > 0; --j)
{
for(int k = 0; k <= j - 1; ++k)
{
for(int i = 0; i < 4; ++i)
{
int index_k = k * 4 + i;
int index_k_1 = (k + 1) * 4 + i;
stepsArray[index_k] = pdByr[i] * stepsArray[index_k_1] + puByr[i] * stepsArray[index_k];
}
}
}
//Copy the root to result
refOutput[bid] = stepsArray[0];
}
free(stepsArray);
return SDK_SUCCESS;
}
PIPE_ALIGN_STACK
static boolean
test_one(unsigned verbose,
FILE *fp,
struct lp_type src_type,
struct lp_type dst_type)
{
LLVMModuleRef module = NULL;
LLVMValueRef func = NULL;
LLVMExecutionEngineRef engine = NULL;
LLVMModuleProviderRef provider = NULL;
LLVMPassManagerRef pass = NULL;
char *error = NULL;
conv_test_ptr_t conv_test_ptr;
boolean success;
const unsigned n = LP_TEST_NUM_SAMPLES;
int64_t cycles[LP_TEST_NUM_SAMPLES];
double cycles_avg = 0.0;
unsigned num_srcs;
unsigned num_dsts;
double eps;
unsigned i, j;
if(verbose >= 1)
dump_conv_types(stdout, src_type, dst_type);
if(src_type.length > dst_type.length) {
num_srcs = 1;
num_dsts = src_type.length/dst_type.length;
}
else {
num_dsts = 1;
num_srcs = dst_type.length/src_type.length;
}
assert(src_type.width * src_type.length == dst_type.width * dst_type.length);
/* We must not loose or gain channels. Only precision */
assert(src_type.length * num_srcs == dst_type.length * num_dsts);
eps = MAX2(lp_const_eps(src_type), lp_const_eps(dst_type));
module = LLVMModuleCreateWithName("test");
func = add_conv_test(module, src_type, num_srcs, dst_type, num_dsts);
if(LLVMVerifyModule(module, LLVMPrintMessageAction, &error)) {
LLVMDumpModule(module);
abort();
}
LLVMDisposeMessage(error);
provider = LLVMCreateModuleProviderForExistingModule(module);
if (LLVMCreateJITCompiler(&engine, provider, 1, &error)) {
if(verbose < 1)
dump_conv_types(stderr, src_type, dst_type);
fprintf(stderr, "%s\n", error);
LLVMDisposeMessage(error);
abort();
}
#if 0
pass = LLVMCreatePassManager();
LLVMAddTargetData(LLVMGetExecutionEngineTargetData(engine), pass);
/* These are the passes currently listed in llvm-c/Transforms/Scalar.h,
* but there are more on SVN. */
LLVMAddConstantPropagationPass(pass);
LLVMAddInstructionCombiningPass(pass);
LLVMAddPromoteMemoryToRegisterPass(pass);
LLVMAddGVNPass(pass);
LLVMAddCFGSimplificationPass(pass);
LLVMRunPassManager(pass, module);
#else
(void)pass;
#endif
if(verbose >= 2)
LLVMDumpModule(module);
conv_test_ptr = (conv_test_ptr_t)LLVMGetPointerToGlobal(engine, func);
if(verbose >= 2)
lp_disassemble(conv_test_ptr);
success = TRUE;
for(i = 0; i < n && success; ++i) {
unsigned src_stride = src_type.length*src_type.width/8;
unsigned dst_stride = dst_type.length*dst_type.width/8;
PIPE_ALIGN_VAR(16) uint8_t src[LP_MAX_VECTOR_LENGTH*LP_MAX_VECTOR_LENGTH];
PIPE_ALIGN_VAR(16) uint8_t dst[LP_MAX_VECTOR_LENGTH*LP_MAX_VECTOR_LENGTH];
double fref[LP_MAX_VECTOR_LENGTH*LP_MAX_VECTOR_LENGTH];
uint8_t ref[LP_MAX_VECTOR_LENGTH*LP_MAX_VECTOR_LENGTH];
int64_t start_counter = 0;
int64_t end_counter = 0;
for(j = 0; j < num_srcs; ++j) {
random_vec(src_type, src + j*src_stride);
read_vec(src_type, src + j*src_stride, fref + j*src_type.length);
}
for(j = 0; j < num_dsts; ++j) {
write_vec(dst_type, ref + j*dst_stride, fref + j*dst_type.length);
}
start_counter = rdtsc();
conv_test_ptr(src, dst);
end_counter = rdtsc();
cycles[i] = end_counter - start_counter;
for(j = 0; j < num_dsts; ++j) {
if(!compare_vec_with_eps(dst_type, dst + j*dst_stride, ref + j*dst_stride, eps))
success = FALSE;
}
if (!success) {
if(verbose < 1)
dump_conv_types(stderr, src_type, dst_type);
fprintf(stderr, "MISMATCH\n");
for(j = 0; j < num_srcs; ++j) {
fprintf(stderr, " Src%u: ", j);
dump_vec(stderr, src_type, src + j*src_stride);
fprintf(stderr, "\n");
}
#if 1
fprintf(stderr, " Ref: ");
for(j = 0; j < src_type.length*num_srcs; ++j)
fprintf(stderr, " %f", fref[j]);
fprintf(stderr, "\n");
#endif
for(j = 0; j < num_dsts; ++j) {
fprintf(stderr, " Dst%u: ", j);
dump_vec(stderr, dst_type, dst + j*dst_stride);
fprintf(stderr, "\n");
fprintf(stderr, " Ref%u: ", j);
dump_vec(stderr, dst_type, ref + j*dst_stride);
fprintf(stderr, "\n");
}
}
}
/*
* Unfortunately the output of cycle counter is not very reliable as it comes
* -- sometimes we get outliers (due IRQs perhaps?) which are
* better removed to avoid random or biased data.
*/
{
double sum = 0.0, sum2 = 0.0;
double avg, std;
unsigned m;
for(i = 0; i < n; ++i) {
sum += cycles[i];
sum2 += cycles[i]*cycles[i];
}
avg = sum/n;
std = sqrtf((sum2 - n*avg*avg)/n);
m = 0;
sum = 0.0;
for(i = 0; i < n; ++i) {
if(fabs(cycles[i] - avg) <= 4.0*std) {
sum += cycles[i];
++m;
}
}
cycles_avg = sum/m;
}
if(fp)
write_tsv_row(fp, src_type, dst_type, cycles_avg, success);
if (!success) {
static boolean firsttime = TRUE;
if(firsttime) {
if(verbose < 2)
LLVMDumpModule(module);
LLVMWriteBitcodeToFile(module, "conv.bc");
fprintf(stderr, "conv.bc written\n");
fprintf(stderr, "Invoke as \"llc -o - conv.bc\"\n");
firsttime = FALSE;
/* abort(); */
}
}
LLVMFreeMachineCodeForFunction(engine, func);
LLVMDisposeExecutionEngine(engine);
if(pass)
LLVMDisposePassManager(pass);
return success;
}
void BGGraphics::pushSingleConnectedNode(ofMesh& mesh, ofVec2f position, float nodeRadius, ofVec2f edgePoint, bool isRoot) {
float innerRadius = nodeRadius - NETWORK_OFFSET;
ofVec2f to = edgePoint - position;
float toDist = to.length();
to /= toDist;
ofVec2f perp = ofVec2f(-to.y, to.x);
float depthFactor = isRoot ? .5 : -.5;
float proj = sqrtf(.5 * innerRadius * innerRadius);
if(toDist > innerRadius + NETWORK_OFFSET) {
float offset = min(innerRadius + .5f * (toDist - innerRadius), 2 * proj);
//perform spline traversal...
ofVec2f controlPoint = position + offset * to;// .5 * (position + innerRadius * to) + .5 * edgePoint;
float angle = acosf(innerRadius / offset);
pushCirclePart(mesh, position, nodeRadius, atan2f(to.y, to.x) + angle, 2 * (M_PI - angle));
int splineOffset = mesh.getVertices().size();
/*
ofVec2f anchorLeft = position + proj * to + proj * perp;
ofVec2f anchorRight = position + proj * to - proj * perp;
ofVec2f toA1 = (anchorLeft - position).normalize();
ofVec2f toA2 = (anchorRight - position).normalize();
*/
float toAng = atan2f(to.y, to.x);
ofVec2f toA1(cosf(toAng + angle), sinf(toAng + angle));
ofVec2f toA2(cosf(toAng - angle), sinf(toAng - angle));
float facU = innerRadius * cosf(angle);
float facV = innerRadius * sinf(angle);
float anchorProj = sqrtf(.5 * innerRadius * innerRadius);
ofVec2f anchorLeft = position + innerRadius * toA1;
ofVec2f anchorRight = position + innerRadius * toA2;
float innerRadiusFactor = .5;// innerRadius / nodeRadius;
//add first point:
pushVertex(mesh, anchorLeft.x + NETWORK_OFFSET * toA1.x, anchorLeft.y + NETWORK_OFFSET * toA1.y, 0, toA1.x, toA1.y, 0, 0, 1);
pushVertex(mesh, anchorLeft.x, anchorLeft.y, 1, 0, 0, 1, 0, innerRadiusFactor);
pushVertex(mesh, position.x, position.y, 1, 0, 0, 1, 0, 0);
pushVertex(mesh, anchorRight.x, anchorRight.y, 1, 0, 0, 1, 0, innerRadiusFactor);
pushVertex(mesh, anchorRight.x + NETWORK_OFFSET * toA2.x, anchorRight.y + NETWORK_OFFSET * toA2.y, 0, toA2.x, toA2.y, 0, 0, 1);
int samples = 8;
for(int i=1; i<samples; ++i) {
float t = i / (float)(samples - 1);
float localDepth = depthFactor * t;
ofVec2f pt, normal;
sampleSpline(anchorLeft, controlPoint, edgePoint, t, pt, normal);
normal = -normal;
/*
//project point on skeleton:
//float projDistance = dot(pt - position, to);
float innerSampleOffset = dot(pt - position, to);//(pt - centerSample).length();
ofVec2f centerSample = position + innerSampleOffset * to;
float centerSampleDepth = depth + depthFactor * (innerSampleOffset / toDist);
centerSampleDepth = (centerSampleDepth + localDepth) * .5;
*/
ofVec2f centerSample;
getIntersection(position, to, pt, normal, centerSample);
//float innerSampleOffset = (pt - centerSample).length();
float innerSampleOffsetFactor = (1 - t) * .5;// innerSampleOffset / (innerSampleOffset + NETWORK_OFFSET);
ofVec2f otherPt = position + reflect(pt - position, to);
ofVec2f otherNormal = reflect(normal, to);
pushVertex(mesh, pt.x + NETWORK_OFFSET * normal.x, pt.y + NETWORK_OFFSET * normal.y, 0, normal.x, normal.y, 0, localDepth, 1);
pushVertex(mesh, pt.x, pt.y, 1, 0, 0, 1, localDepth, innerSampleOffsetFactor);
pushVertex(mesh, centerSample.x, centerSample.y, 1, 0, 0, 1, localDepth, 0);
pushVertex(mesh, otherPt.x, otherPt.y, 1, 0, 0, 1, localDepth, innerSampleOffsetFactor);
pushVertex(mesh, otherPt.x + NETWORK_OFFSET * otherNormal.x, otherPt.y + NETWORK_OFFSET * otherNormal.y, 0, otherNormal.x, otherNormal.y, 0, localDepth, 1);
if(i > 0) {
int offset = splineOffset + 5 * i;
for(int j=0; j<4; ++j) {
mesh.addTriangle(offset + j, offset - 5 + j, offset - 4 + j);
mesh.addTriangle(offset + j, offset + 1 + j, offset - 4 + j);
}
}
}
}
else
pushSeparateNode(mesh, position, nodeRadius);
}
/*
* Effect output
*/
void
Valve::out (float * smpsl, float * smpsr)
{
int i;
float l, r, lout, rout, fx;
if (Pstereo != 0) {
//Stereo
for (i = 0; i < param->PERIOD; i++) {
efxoutl[i] = smpsl[i] * inputvol;
efxoutr[i] = smpsr[i] * inputvol;
};
} else {
for (i = 0; i < param->PERIOD; i++) {
efxoutl[i] =
(smpsl[i] + smpsr[i] ) * inputvol;
};
};
harm->harm_out(efxoutl,efxoutr);
if (Pprefiltering != 0)
applyfilters (efxoutl, efxoutr);
if(Ped) {
for (i =0; i<param->PERIOD; i++) {
efxoutl[i]=Wshape(efxoutl[i]);
if (Pstereo != 0) efxoutr[i]=Wshape(efxoutr[i]);
}
}
for (i =0; i<param->PERIOD; i++) { //soft limiting to 3.0 (max)
fx = efxoutl[i];
if (fx>1.0f) fx = 3.0f - 2.0f/sqrtf(fx);
efxoutl[i] = fx;
fx = efxoutr[i];
if (fx>1.0f) fx = 3.0f - 2.0f/sqrtf(fx);
efxoutr[i] = fx;
}
if (q == 0.0f) {
for (i =0; i<param->PERIOD; i++) {
if (efxoutl[i] == q) fx = fdist;
else fx =efxoutl[i] / (1.0f - powf(2.0f,-dist * efxoutl[i] ));
otml = atk * otml + fx - itml;
itml = fx;
efxoutl[i]= otml;
}
} else {
for (i = 0; i < param->PERIOD; i++) {
if (efxoutl[i] == q) fx = fdist + qcoef;
else fx =(efxoutl[i] - q) / (1.0f - powf(2.0f,-dist * (efxoutl[i] - q))) + qcoef;
otml = atk * otml + fx - itml;
itml = fx;
efxoutl[i]= otml;
}
}
if (Pstereo != 0) {
if (q == 0.0f) {
for (i =0; i<param->PERIOD; i++) {
if (efxoutr[i] == q) fx = fdist;
else fx = efxoutr[i] / (1.0f - powf(2.0f,-dist * efxoutr[i] ));
otmr = atk * otmr + fx - itmr;
itmr = fx;
efxoutr[i]= otmr;
}
} else {
for (i = 0; i < param->PERIOD; i++) {
if (efxoutr[i] == q) fx = fdist + qcoef;
else fx = (efxoutr[i] - q) / (1.0f - powf(2.0f,-dist * (efxoutr[i] - q))) + qcoef;
otmr = atk * otmr + fx - itmr;
itmr = fx;
efxoutr[i]= otmr;
}
}
}
if (Pprefiltering == 0)
applyfilters (efxoutl, efxoutr);
if (Pstereo == 0) memcpy (efxoutr , efxoutl, param->PERIOD * sizeof(float));
float level = dB2rap (60.0f * (float)Plevel / 127.0f - 40.0f);
for (i = 0; i < param->PERIOD; i++) {
lout = efxoutl[i];
rout = efxoutr[i];
l = lout * (1.0f - lrcross) + rout * lrcross;
r = rout * (1.0f - lrcross) + lout * lrcross;
lout = l;
rout = r;
efxoutl[i] = lout * 2.0f * level * panning;
efxoutr[i] = rout * 2.0f * level * (1.0f -panning);
};
};
void
GPS::task_main()
{
/* open the serial port */
_serial_fd = ::open(_port, O_RDWR);
if (_serial_fd < 0) {
DEVICE_LOG("failed to open serial port: %s err: %d", _port, errno);
/* tell the dtor that we are exiting, set error code */
_task = -1;
_exit(1);
}
uint64_t last_rate_measurement = hrt_absolute_time();
unsigned last_rate_count = 0;
/* loop handling received serial bytes and also configuring in between */
while (!_task_should_exit) {
if (_fake_gps) {
_report_gps_pos.timestamp_position = hrt_absolute_time();
_report_gps_pos.lat = (int32_t)47.378301e7f;
_report_gps_pos.lon = (int32_t)8.538777e7f;
_report_gps_pos.alt = (int32_t)1200e3f;
_report_gps_pos.timestamp_variance = hrt_absolute_time();
_report_gps_pos.s_variance_m_s = 10.0f;
_report_gps_pos.c_variance_rad = 0.1f;
_report_gps_pos.fix_type = 3;
_report_gps_pos.eph = 0.9f;
_report_gps_pos.epv = 1.8f;
_report_gps_pos.timestamp_velocity = hrt_absolute_time();
_report_gps_pos.vel_n_m_s = 0.0f;
_report_gps_pos.vel_e_m_s = 0.0f;
_report_gps_pos.vel_d_m_s = 0.0f;
_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s *
_report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
_report_gps_pos.cog_rad = 0.0f;
_report_gps_pos.vel_ned_valid = true;
/* no time and satellite information simulated */
if (!(_pub_blocked)) {
if (_report_gps_pos_pub != nullptr) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
}
usleep(2e5);
} else {
if (_Helper != nullptr) {
delete(_Helper);
/* set to zero to ensure parser is not used while not instantiated */
_Helper = nullptr;
}
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
_Helper = new UBX(_serial_fd, &_report_gps_pos, _p_report_sat_info);
break;
case GPS_DRIVER_MODE_MTK:
_Helper = new MTK(_serial_fd, &_report_gps_pos);
break;
case GPS_DRIVER_MODE_ASHTECH:
_Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info);
break;
default:
break;
}
unlock();
/* the Ashtech driver lies about successful configuration and the
* MTK driver is not well tested, so we really only trust the UBX
* driver for an advance publication
*/
if (_Helper->configure(_baudrate) == 0) {
unlock();
/* reset report */
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
if (_mode == GPS_DRIVER_MODE_UBX) {
/* Publish initial report that we have access to a GPS,
* but set all critical state fields to indicate we have
* no valid position lock
*/
_report_gps_pos.timestamp_time = hrt_absolute_time();
/* reset the timestamps for data, because we have no data yet */
_report_gps_pos.timestamp_position = 0;
_report_gps_pos.timestamp_variance = 0;
_report_gps_pos.timestamp_velocity = 0;
/* set a massive variance */
_report_gps_pos.eph = 10000.0f;
_report_gps_pos.epv = 10000.0f;
_report_gps_pos.fix_type = 0;
if (!(_pub_blocked)) {
if (_report_gps_pos_pub != nullptr) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
}
/* GPS is obviously detected successfully, reset statistics */
_Helper->reset_update_rates();
}
int helper_ret;
while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
// lock();
/* opportunistic publishing - else invalid data would end up on the bus */
if (!(_pub_blocked)) {
if (helper_ret & 1) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
}
if (_p_report_sat_info && (helper_ret & 2)) {
if (_report_sat_info_pub != nullptr) {
orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
} else {
_report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info);
}
}
}
if (helper_ret & 1) { // consider only pos info updates for rate calculation */
last_rate_count++;
}
/* measure update rate every 5 seconds */
if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
last_rate_measurement = hrt_absolute_time();
last_rate_count = 0;
_Helper->store_update_rates();
_Helper->reset_update_rates();
}
if (!_healthy) {
const char *mode_str = "unknown";
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
mode_str = "UBX";
break;
case GPS_DRIVER_MODE_MTK:
mode_str = "MTK";
break;
case GPS_DRIVER_MODE_ASHTECH:
mode_str = "ASHTECH";
break;
default:
break;
}
warnx("module found: %s", mode_str);
_healthy = true;
}
}
if (_healthy) {
warnx("module lost");
_healthy = false;
_rate = 0.0f;
}
lock();
}
lock();
/* select next mode */
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
_mode = GPS_DRIVER_MODE_MTK;
break;
case GPS_DRIVER_MODE_MTK:
_mode = GPS_DRIVER_MODE_ASHTECH;
break;
case GPS_DRIVER_MODE_ASHTECH:
_mode = GPS_DRIVER_MODE_UBX;
break;
default:
break;
}
}
}
warnx("exiting");
::close(_serial_fd);
/* tell the dtor that we are exiting */
_task = -1;
_exit(0);
}
void
Valve::processReplacing (float **inputs,
float **outputs,
int sampleFrames)
{
int i;
float l, r, lout, rout, fx;
param->PERIOD = sampleFrames;
param->fPERIOD = param->PERIOD;
if (Pstereo != 0) {
//Stereo
for (i = 0; i < param->PERIOD; i++) {
outputs[0][i] = inputs[0][i] * inputvol;
outputs[1][i] = inputs[1][i] * inputvol;
};
} else {
for (i = 0; i < param->PERIOD; i++) {
outputs[0][i] =
(inputs[0][i] + inputs[1][i] ) * inputvol;
};
};
harm->harm_out(outputs[0],outputs[1]);
if (Pprefiltering != 0)
applyfilters (outputs[0], outputs[1]);
if(Ped) {
for (i =0; i<param->PERIOD; i++) {
outputs[0][i]=Wshape(outputs[0][i]);
if (Pstereo != 0) outputs[1][i]=Wshape(outputs[1][i]);
}
}
for (i =0; i<param->PERIOD; i++) { //soft limiting to 3.0 (max)
fx = outputs[0][i];
if (fx>1.0f) fx = 3.0f - 2.0f/sqrtf(fx);
outputs[0][i] = fx;
fx = outputs[1][i];
if (fx>1.0f) fx = 3.0f - 2.0f/sqrtf(fx);
outputs[1][i] = fx;
}
if (q == 0.0f) {
for (i =0; i<param->PERIOD; i++) {
if (outputs[0][i] == q) fx = fdist;
else fx =outputs[0][i] / (1.0f - powf(2.0f,-dist * outputs[0][i] ));
otml = atk * otml + fx - itml;
itml = fx;
outputs[0][i]= otml;
}
} else {
for (i = 0; i < param->PERIOD; i++) {
if (outputs[0][i] == q) fx = fdist + qcoef;
else fx =(outputs[0][i] - q) / (1.0f - powf(2.0f,-dist * (outputs[0][i] - q))) + qcoef;
otml = atk * otml + fx - itml;
itml = fx;
outputs[0][i]= otml;
}
}
if (Pstereo != 0) {
if (q == 0.0f) {
for (i =0; i<param->PERIOD; i++) {
if (outputs[1][i] == q) fx = fdist;
else fx = outputs[1][i] / (1.0f - powf(2.0f,-dist * outputs[1][i] ));
otmr = atk * otmr + fx - itmr;
itmr = fx;
outputs[1][i]= otmr;
}
} else {
for (i = 0; i < param->PERIOD; i++) {
if (outputs[1][i] == q) fx = fdist + qcoef;
else fx = (outputs[1][i] - q) / (1.0f - powf(2.0f,-dist * (outputs[1][i] - q))) + qcoef;
otmr = atk * otmr + fx - itmr;
itmr = fx;
outputs[1][i]= otmr;
}
}
}
if (Pprefiltering == 0)
applyfilters (outputs[0], outputs[1]);
if (Pstereo == 0) memcpy (outputs[1] , outputs[0], param->PERIOD * sizeof(float));
float level = dB2rap (60.0f * (float)Plevel / 127.0f - 40.0f);
for (i = 0; i < param->PERIOD; i++) {
lout = outputs[0][i];
rout = outputs[1][i];
l = lout * (1.0f - lrcross) + rout * lrcross;
r = rout * (1.0f - lrcross) + lout * lrcross;
lout = l;
rout = r;
outputs[0][i] = lout * 2.0f * level * panning;
outputs[1][i] = rout * 2.0f * level * (1.0f -panning);
};
};
float Vector4::magnitude (void) const {
return sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]) / abs(v[3]);
}
inline float length(const Vec3& vec)
{
return sqrtf(dot(vec, vec));
}
void
RandomMovementGenerator<Creature>::_setRandomLocation(Creature &creature)
{
float X,Y,Z,z,nx,ny,nz,ori,dist;
creature.GetHomePosition(X, Y, Z, ori);
z = creature.GetPositionZ();
Map const* map = creature.GetBaseMap();
// For 2D/3D system selection
//bool is_land_ok = creature.canWalk();
//bool is_water_ok = creature.canSwim();
bool is_air_ok = creature.canFly();
for (uint32 i = 0; ; ++i)
{
const float angle = (float)rand_norm()*static_cast<float>(M_PI*2);
const float range = (float)rand_norm()*wander_distance;
const float distanceX = range * cos(angle);
const float distanceY = range * sin(angle);
nx = X + distanceX;
ny = Y + distanceY;
// prevent invalid coordinates generation
Trinity::NormalizeMapCoord(nx);
Trinity::NormalizeMapCoord(ny);
dist = (nx - X)*(nx - X) + (ny - Y)*(ny - Y);
if (i == 5)
{
nz = Z;
break;
}
if (is_air_ok) // 3D system above ground and above water (flying mode)
{
const float distanceZ = (float)(rand_norm()) * sqrtf(dist)/2; // Limit height change
nz = Z + distanceZ;
float tz = map->GetHeight(nx, ny, nz-2.0f, false); // Map check only, vmap needed here but need to alter vmaps checks for height.
float wz = map->GetWaterLevel(nx, ny);
if (tz >= nz || wz >= nz)
continue; // Problem here, we must fly above the ground and water, not under. Let's try on next tick
}
//else if (is_water_ok) // 3D system under water and above ground (swimming mode)
else // 2D only
{
dist = dist >= 100.0f ? 10.0f : sqrtf(dist); // 10.0 is the max that vmap high can check (MAX_CAN_FALL_DISTANCE)
// The fastest way to get an accurate result 90% of the time.
// Better result can be obtained like 99% accuracy with a ray light, but the cost is too high and the code is too long.
nz = map->GetHeight(nx,ny,Z+dist-2.0f,false); // Map check
if (fabs(nz-Z)>dist)
{
nz = map->GetHeight(nx,ny,Z-2.0f,true); // Vmap Horizontal or above
if (fabs(nz-Z)>dist)
{
nz = map->GetHeight(nx,ny,Z+dist-2.0f,true); // Vmap Higher
if (fabs(nz-Z)>dist)
continue; // let's forget this bad coords where a z cannot be find and retry at next tick
}
}
}
break;
}
Traveller<Creature> traveller(creature);
creature.SetOrientation(creature.GetAngle(nx,ny));
i_destinationHolder.SetDestination(traveller, nx, ny, nz);
creature.addUnitState(UNIT_STAT_ROAMING);
if (is_air_ok)
{
i_nextMoveTime.Reset(i_destinationHolder.GetTotalTravelTime());
}
//else if (is_water_ok) // Swimming mode to be done with more than this check
else
{
i_nextMoveTime.Reset(urand(500+i_destinationHolder.GetTotalTravelTime(),5000+i_destinationHolder.GetTotalTravelTime()));
creature.AddUnitMovementFlag(MOVEMENTFLAG_WALKING);
}
//Call for creature group update
if (creature.GetFormation() && creature.GetFormation()->getLeader() == &creature)
{
creature.GetFormation()->LeaderMoveTo(nx, ny, nz);
}
}
float BKE_brush_sample_masktex(
const Scene *scene, Brush *br, const float point[2], const int thread, struct ImagePool *pool)
{
UnifiedPaintSettings *ups = &scene->toolsettings->unified_paint_settings;
MTex *mtex = &br->mask_mtex;
float rgba[4], intensity;
if (!mtex->tex) {
return 1.0f;
}
if (mtex->brush_map_mode == MTEX_MAP_MODE_STENCIL) {
float rotation = -mtex->rot;
float point_2d[2] = {point[0], point[1]};
float x, y;
float co[3];
x = point_2d[0] - br->mask_stencil_pos[0];
y = point_2d[1] - br->mask_stencil_pos[1];
if (rotation > 0.001f || rotation < -0.001f) {
const float angle = atan2f(y, x) + rotation;
const float flen = sqrtf(x * x + y * y);
x = flen * cosf(angle);
y = flen * sinf(angle);
}
if (fabsf(x) > br->mask_stencil_dimension[0] || fabsf(y) > br->mask_stencil_dimension[1]) {
zero_v4(rgba);
return 0.0f;
}
x /= (br->mask_stencil_dimension[0]);
y /= (br->mask_stencil_dimension[1]);
co[0] = x;
co[1] = y;
co[2] = 0.0f;
externtex(
mtex, co, &intensity, rgba, rgba + 1, rgba + 2, rgba + 3, thread, pool, false, false);
}
else {
float rotation = -mtex->rot;
float point_2d[2] = {point[0], point[1]};
float x = 0.0f, y = 0.0f; /* Quite warnings */
float invradius = 1.0f; /* Quite warnings */
float co[3];
if (mtex->brush_map_mode == MTEX_MAP_MODE_VIEW) {
/* keep coordinates relative to mouse */
rotation += ups->brush_rotation_sec;
x = point_2d[0] - ups->mask_tex_mouse[0];
y = point_2d[1] - ups->mask_tex_mouse[1];
/* use pressure adjusted size for fixed mode */
invradius = 1.0f / ups->pixel_radius;
}
else if (mtex->brush_map_mode == MTEX_MAP_MODE_TILED) {
/* leave the coordinates relative to the screen */
/* use unadjusted size for tiled mode */
invradius = 1.0f / BKE_brush_size_get(scene, br);
x = point_2d[0];
y = point_2d[1];
}
else if (mtex->brush_map_mode == MTEX_MAP_MODE_RANDOM) {
rotation += ups->brush_rotation_sec;
/* these contain a random coordinate */
x = point_2d[0] - ups->mask_tex_mouse[0];
y = point_2d[1] - ups->mask_tex_mouse[1];
invradius = 1.0f / ups->pixel_radius;
}
x *= invradius;
y *= invradius;
/* it is probably worth optimizing for those cases where
* the texture is not rotated by skipping the calls to
* atan2, sqrtf, sin, and cos. */
if (rotation > 0.001f || rotation < -0.001f) {
const float angle = atan2f(y, x) + rotation;
const float flen = sqrtf(x * x + y * y);
x = flen * cosf(angle);
y = flen * sinf(angle);
}
co[0] = x;
co[1] = y;
co[2] = 0.0f;
externtex(
mtex, co, &intensity, rgba, rgba + 1, rgba + 2, rgba + 3, thread, pool, false, false);
}
CLAMP(intensity, 0.0f, 1.0f);
switch (br->mask_pressure) {
case BRUSH_MASK_PRESSURE_CUTOFF:
intensity = ((1.0f - intensity) < ups->size_pressure_value) ? 1.0f : 0.0f;
break;
case BRUSH_MASK_PRESSURE_RAMP:
intensity = ups->size_pressure_value + intensity * (1.0f - ups->size_pressure_value);
break;
default:
break;
}
return intensity;
}
float vec2::mag() const
{
return sqrtf(x*x + y*y);
}
/* Generic texture sampler for 3D painting systems. point has to be either in
* region space mouse coordinates, or 3d world coordinates for 3D mapping.
*
* rgba outputs straight alpha. */
float BKE_brush_sample_tex_3d(const Scene *scene,
const Brush *br,
const float point[3],
float rgba[4],
const int thread,
struct ImagePool *pool)
{
UnifiedPaintSettings *ups = &scene->toolsettings->unified_paint_settings;
const MTex *mtex = &br->mtex;
float intensity = 1.0;
bool hasrgb = false;
if (!mtex->tex) {
intensity = 1;
}
else if (mtex->brush_map_mode == MTEX_MAP_MODE_3D) {
/* Get strength by feeding the vertex
* location directly into a texture */
hasrgb = externtex(
mtex, point, &intensity, rgba, rgba + 1, rgba + 2, rgba + 3, thread, pool, false, false);
}
else if (mtex->brush_map_mode == MTEX_MAP_MODE_STENCIL) {
float rotation = -mtex->rot;
float point_2d[2] = {point[0], point[1]};
float x, y;
float co[3];
x = point_2d[0] - br->stencil_pos[0];
y = point_2d[1] - br->stencil_pos[1];
if (rotation > 0.001f || rotation < -0.001f) {
const float angle = atan2f(y, x) + rotation;
const float flen = sqrtf(x * x + y * y);
x = flen * cosf(angle);
y = flen * sinf(angle);
}
if (fabsf(x) > br->stencil_dimension[0] || fabsf(y) > br->stencil_dimension[1]) {
zero_v4(rgba);
return 0.0f;
}
x /= (br->stencil_dimension[0]);
y /= (br->stencil_dimension[1]);
co[0] = x;
co[1] = y;
co[2] = 0.0f;
hasrgb = externtex(
mtex, co, &intensity, rgba, rgba + 1, rgba + 2, rgba + 3, thread, pool, false, false);
}
else {
float rotation = -mtex->rot;
float point_2d[2] = {point[0], point[1]};
float x = 0.0f, y = 0.0f; /* Quite warnings */
float invradius = 1.0f; /* Quite warnings */
float co[3];
if (mtex->brush_map_mode == MTEX_MAP_MODE_VIEW) {
/* keep coordinates relative to mouse */
rotation += ups->brush_rotation;
x = point_2d[0] - ups->tex_mouse[0];
y = point_2d[1] - ups->tex_mouse[1];
/* use pressure adjusted size for fixed mode */
invradius = 1.0f / ups->pixel_radius;
}
else if (mtex->brush_map_mode == MTEX_MAP_MODE_TILED) {
/* leave the coordinates relative to the screen */
/* use unadjusted size for tiled mode */
invradius = 1.0f / BKE_brush_size_get(scene, br);
x = point_2d[0];
y = point_2d[1];
}
else if (mtex->brush_map_mode == MTEX_MAP_MODE_RANDOM) {
rotation += ups->brush_rotation;
/* these contain a random coordinate */
x = point_2d[0] - ups->tex_mouse[0];
y = point_2d[1] - ups->tex_mouse[1];
invradius = 1.0f / ups->pixel_radius;
}
x *= invradius;
y *= invradius;
/* it is probably worth optimizing for those cases where
* the texture is not rotated by skipping the calls to
* atan2, sqrtf, sin, and cos. */
if (rotation > 0.001f || rotation < -0.001f) {
const float angle = atan2f(y, x) + rotation;
const float flen = sqrtf(x * x + y * y);
x = flen * cosf(angle);
y = flen * sinf(angle);
}
co[0] = x;
co[1] = y;
co[2] = 0.0f;
hasrgb = externtex(
mtex, co, &intensity, rgba, rgba + 1, rgba + 2, rgba + 3, thread, pool, false, false);
}
intensity += br->texture_sample_bias;
if (!hasrgb) {
rgba[0] = intensity;
rgba[1] = intensity;
rgba[2] = intensity;
rgba[3] = 1.0f;
}
/* For consistency, sampling always returns color in linear space */
else if (ups->do_linear_conversion) {
IMB_colormanagement_colorspace_to_scene_linear_v3(rgba, ups->colorspace);
}
return intensity;
}
bool A_Star::FindPath(TreadmillMap* map2d) {
unordered_set<Node*> openList; // note that costList makes this redundant
unordered_set<Node*> closedList;
unordered_map<Node*, Node*> parentList;
unordered_map<Node*, float> costList;
Node* current = map2d->GetStart();
Node* currentBest = current;
float bestHeuristic = FLT_MAX;
openList.insert(current);
parentList[current] = nullptr;
costList[current] = map2d->CalcHeuristic(current); // ->GetHeuristicDist();
int debugCounter = 0;
int limit = 2 * map2d->GetMapWidthNodes() * map2d->GetMapLengthNodes();
while (debugCounter++ < limit) {
// find lowest F cost and assign that node as current
if (map2d->CalcHeuristic(current) < bestHeuristic) { // July 14 update: must calculate heuristic before
bestHeuristic = current->GetHeuristicDist();
currentBest = current;
}
current = NextNode(costList);
openList.erase(current);
closedList.insert(current);
if (current == nullptr || current == map2d->GetGoal()) {
if (current == nullptr)
current = currentBest;
// print the final path and distance
deque<Node*> finalPath = GetPath(parentList, current);
float distance = 0;
finalPath[0]->SetPath(true);
map2d->PathNodeList.push_back(finalPath[0]);
for (int i = 1; i < finalPath.size(); i++) {
finalPath[i]->SetPath(true);
map2d->PathNodeList.push_back(finalPath[i]);
float tempDist = map2d->CalcNodeWidthCm(); // dist between two nodes
if (finalPath[i]->GetDiagonals().count(finalPath[i - 1]) != 0)
tempDist = sqrtf(2 * tempDist*tempDist);
distance += tempDist;
}
if (current == map2d->GetGoal())
map2d->PathNodeList.push_back(current); // pad this list with another Goal so the dynamic mapping will move the S to G
/*for each (Node* node in closedList) {
node->SetVisited(true);
}*/
/*output.algorithmName = "A Star";
output.hasSolution = current == map2d->GetGoal();
output.nodesTotal = map2d->GetMapWidthNodes() * map2d->GetMapLengthNodes();
output.nodesVisited = (int)openList.size() + (int)closedList.size();
output.percentVisited = output.nodesVisited / ((double)output.nodesTotal);
output.solutionDistance = distance;
output.solutionTime = duration;
output.widthResolution = map2d->GetMapWidthNodes();*/
return current == map2d->GetGoal();
//break;
}
// note that paths contain the final node too
deque<Node*> adjacent = current->GetAllAdjacent();
for (int i = 0; i < adjacent.size(); i++) {
Node* node = adjacent[i];
if (node == nullptr || closedList.count(node) != 0) continue;
if (node->IsOccupied() || node->IsOccupationPredicted) {
closedList.insert(node);
continue;
}
// if this is not in OPEN or if its a shorter path than the one in OPEN
// then add/replace in OPEN as needed
float temp = map2d->CalcHeuristic(current); // July 14 change: Heuristics must be calculated the first time they are used
float deltaG = map2d->CalcNodeWidthCm(); // dist between two nodes
if (current->GetDiagonals().count(node) != 0)
deltaG = sqrtf(2 * deltaG*deltaG); // get diagonal distance
float newPath = costList[current] - current->GetHeuristicDist() + deltaG + map2d->CalcHeuristic(node);
if (openList.count(node) == 0) {
openList.insert(node);
costList[node] = newPath;
parentList[node] = current;
}
else {
// check if this path is shorter
float oldPath = costList[node];
if (newPath < oldPath) {
parentList[node] = current;
costList[node] = newPath;
}
}
}
costList.erase(current); // because it is not in the open list anymore
}
return false;
//return output;
}
void BGGraphics::drawMesh(ofMesh & mesh, ofVec2f nodeLocation, float nodeRadius, float nodeDepth, bool isExternal, bool deform, ofVec2f surfaceNormal) {
if(depthTest)
ofEnableDepthTest();
else
ofDisableDepthTest();
float revealParam = 0.0;
float winParam = -1.0;
float flowStartTime = .1;
float assimilateStartTime = .4;
float burstStartTime = .8;
if(mRevealParameter < flowStartTime) {
revealParam = 0.0;
winParam = -1;
}
else if(mRevealParameter < assimilateStartTime) {
float t = (mRevealParameter - flowStartTime) / (assimilateStartTime - flowStartTime);
revealParam = .5 - .5 * cos(t * M_PI);
winParam = - 1;
}
else if(mRevealParameter < burstStartTime) {
float t = (mRevealParameter - assimilateStartTime) / (burstStartTime - assimilateStartTime);
revealParam = 1.0;
winParam = -1 + t;
}
else {
float t = (mRevealParameter - burstStartTime) / (1. - burstStartTime);
revealParam = 1.0;
winParam = t;
}
float minOffset = 10.0;
float maxOffset = 20.0 + 20.0 * revealParam + 40.0 * (1. + winParam);
float recalcOffset = minOffset;
if(drawMode == 0) {
//glow:
recalcOffset = maxOffset;
}
//sheen effect:
float t = winParam;/// pow(winParam, 3.);// MAX(0.0f, MIN(1.0f, 1.0 * winParam + 0));
ofVec2f sheenSource((1-t) * -0.981 + t * 1.182, (1-t) * -0.138 + t * .441);
ofVec2f sheenEnd((1-t) * -0.234 + t * 1.929, (1-t) * 0.37 + t * 0.949);
//transform to fit 'networkBounds'
ofPoint center = networkBounds.getCenter();
sheenSource.x = center.x + (sheenSource.x - .5) * networkBounds.width;
sheenSource.y = center.y + (sheenSource.y - .5) * networkBounds.height;
sheenEnd.x = center.x + (sheenEnd.x - .5) * networkBounds.width;
sheenEnd.y = center.y + (sheenEnd.y - .5) * networkBounds.height;
ofVec2f sheenUnitVector(sheenEnd.x - sheenSource.x, sheenEnd.y - sheenSource.y);
float sheenLength = sqrtf(sheenUnitVector.x * sheenUnitVector.x + sheenUnitVector.y * sheenUnitVector.y);
sheenUnitVector.x /= sheenLength;
sheenUnitVector.y /= sheenLength;
ofColor darkColor = bgResources.getColorSetting(NetworkDarkColorKey)->value;
ofColor lightColor = bgResources.getColorSetting(NetworkLightColorKey)->value;
mNetworkShader.begin();
mNetworkShader.setUniform1f("uTime", mTime);
mNetworkShader.setUniform2f("uResolution", 1024, 768);
mNetworkShader.setUniform1f("uMaxDepth", maxDepth);
mNetworkShader.setUniform1f("uRevealParameter", revealParam);
mNetworkShader.setUniform1f("uBaseHue", 0.3); //RED: 0.1 GREEN: 0.3 //vec3(0,3, 0.7, .7);
mNetworkShader.setUniform3f("uBaseRGBDark", darkColor.r / 255.0, darkColor.g / 255.0, darkColor.b / 255.0);
mNetworkShader.setUniform3f("uBaseRGBLight", lightColor.r / 255.0, lightColor.g / 255.0, lightColor.b / 255.0);
mNetworkShader.setUniform1i("uDrawMode", drawMode);
mNetworkShader.setUniform1f("uBoundOffset", recalcOffset);// boundOffset);
mNetworkShader.setUniform1f("uDepthOffset", nodeDepth);
mNetworkShader.setUniform1i("uDeformNode", deform ? 1 : 0);
mNetworkShader.setUniform2f("uSurfaceNormal", surfaceNormal.x, surfaceNormal.y);
mNetworkShader.setUniform1f("uMinGlowRad", minOffset / maxOffset);
mNetworkShader.setUniform1f("uWinAnimParameter", winParam);
mNetworkShader.setUniform2f("uSheenFrom", sheenSource.x, sheenSource.y);
mNetworkShader.setUniform2f("uSheenUnitVector", sheenUnitVector.x, sheenUnitVector.y);
mNetworkShader.setUniform1f("uSheenLength", sheenLength);
/*
uniform float uWinAnimParameter;
uniform vec2 uSheenFrom;
uniform vec2 uSheenUnitVector;
uniform float uSheenLength;
*/
mNetworkShader.setUniform1f("uNodeRadius", nodeRadius);
mNetworkShader.setUniform1i("uNodeIsExternal", isExternal ? 1 : 0);
mNetworkShader.setUniform2f("uNodeCenter", nodeLocation.x, nodeLocation.y);
mesh.draw();
mNetworkShader.end();
ofDisableDepthTest();
if(renderWireframe)
mesh.drawWireframe();
}
void AudioEchoWidget::paintEvent(QPaintEvent *) {
QPainter paint(this);
paint.scale(width(), height());
paint.fillRect(rect(), Qt::black);
AudioInputPtr ai = g.ai;
if (! ai || ! ai->sesEcho)
return;
ai->qmSpeex.lock();
spx_int32_t sz;
speex_echo_ctl(ai->sesEcho, SPEEX_ECHO_GET_IMPULSE_RESPONSE_SIZE, &sz);
STACKVAR(spx_int32_t, w, sz);
STACKVAR(float, W, sz);
speex_echo_ctl(ai->sesEcho, SPEEX_ECHO_GET_IMPULSE_RESPONSE, w);
ai->qmSpeex.unlock();
int N = 160;
int n = 2 * N;
int M = sz / n;
drft_lookup d;
mumble_drft_init(&d, n);
for (int j=0;j<M;j++) {
for (int i=0;i<n;i++)
W[j*n+i] = static_cast<float>(w[j*n+i]) / static_cast<float>(n);
mumble_drft_forward(&d, & W[j*n]);
}
mumble_drft_clear(&d);
float xscale = 1.0f / static_cast<float>(N);
float yscale = 1.0f / static_cast<float>(M);
for (int j = 0; j < M; j++) {
for (int i=1;i < N; i++) {
float xa = static_cast<float>(i) * xscale;
float ya = static_cast<float>(j) * yscale;
float xb = xa + xscale;
float yb = ya + yscale;
const QColor &c = mapEchoToColor(sqrtf(W[j*n+2*i]*W[j*n+2*i]+W[j*n+2*i-1]*W[j*n+2*i-1]) / 65536.f);
paint.fillRect(QRectF(QPointF(xa, ya), QPointF(xb, yb)), c);
}
}
QPolygonF poly;
xscale = 1.0f / (2.0f * static_cast<float>(n));
yscale = 1.0f / (200.0f * 32767.0f);
for (int i = 0; i < 2 * n; i++) {
poly << QPointF(static_cast<float>(i) * xscale, 0.5f + static_cast<float>(w[i]) * yscale);
}
paint.setPen(QPen(QBrush(QColor::fromRgbF(1.0f, 0.0f, 1.0f)), 0));
paint.drawPolyline(poly);
}
/*
offsetIndex: [0] skeleton
[1] spline
[2] offset spline
*/
ofVec2f BGGraphics::calculateInternalTexOffset(float t, bool isSourceSpline, bool isSourceSegment, int offsetIndex) {
const float triangleHeight = .5 * tanf(M_PI / 3.0);
const float baseSize = sqrtf(3.0);
const float halfBaseSize = .5 * baseSize;
const ofVec2f source(0, 1);
const ofVec2f sink1(-halfBaseSize, -.5);
const ofVec2f sink2(halfBaseSize, -.5);
const ofVec2f center = (source + sink1 + sink2) / 3.0;
const float bezierOffset = 0.5 * baseSize;
const float maxInternalOffset = (.25 * source - .5 * center + .25 * sink1).length();
const float centerStretchFactor = (maxInternalOffset + bezierOffset) / bezierOffset;
ofVec2f focusPt = isSourceSpline ? ofVec2f(baseSize, 1) : ofVec2f(0, -2);
float fromFocusAngle = M_PI * (isSourceSpline ? (1.0 + t / 3.0) : ((1.0 + t) / 3.0));
ofVec2f toPtVector(cosf(fromFocusAngle), sinf(fromFocusAngle));
float offset = (offsetIndex == 2) ? (.5 * baseSize) : baseSize;
ofVec2f xy = focusPt + offset * toPtVector;
if(offsetIndex == 0) {
//project point on base spline
ofVec2f projBase = isSourceSegment ? ofVec2f(0,1) : ofVec2f(halfBaseSize, -.5);
xy = dot(xy, projBase) * projBase;
}
//in case we are dealing with the center point:
if(offsetIndex == -1)
xy = ofVec2f(0,0);
const ofVec2f cornerTL = source + (sink1 - sink2);
const ofVec2f cornerTR = source + (sink2 - sink1);
const ofVec2f cornerB = sink1 + (sink2 - source);
ofVec2f vecSource = (center - source).normalize();
ofVec2f vecSink1 = (sink1 - center).normalize();
ofVec2f vecSink2 = (sink2 - center).normalize();
float traversalDistance = 2. * (center - source).length();
float projSource = dot(xy - source, vecSource);
float projSink1 = dot(xy - sink1, vecSink1);
float projSink2 = dot(xy - sink2, vecSink2);
float orSource = cross(xy - source, vecSource);
float orSink1 = cross(xy - sink1, vecSink1);
float orSink2 = cross(xy - sink2, vecSink2);
float val1 = projSource / traversalDistance;
float val2 = 1.0 + projSink1 / traversalDistance;
float val3 = 1.0 + projSink2 / traversalDistance;
float offsetX = 0;
if(ABS(projSource) < .0001)
offsetX = val1;
else if(ABS(projSink1) < .0001)
offsetX = val2;
else if(ABS(projSink2) < .0001)
offsetX = val3;
else {
float power = 2.0;
float weight1 = powf(1.0 / ABS(projSource), power);
float weight2 = powf(1.0 / ABS(projSink1), power);
float weight3 = powf(1.0 / ABS(projSink2), power);
float sumWeight = weight1 + weight2 + weight3;
offsetX = (weight1 / sumWeight) * val1
+ (weight2 / sumWeight) * val2
+ (weight3 / sumWeight) * val3;
}
ofVec2f to = xy - focusPt;
float toDist = to.length();
to /= toDist;
float dist = MAX(0.0, toDist - bezierOffset);
float maxAng = M_PI / 6.;
float angle = acos(dot(to, (center - focusPt).normalize()));
float maxOffset = baseSize / cos(M_PI / 6.0 - angle) - bezierOffset;
float circDistFrac = dist / (baseSize - bezierOffset);
float projDistFrac = dist / maxOffset;
float angleFrac = 1. - angle / maxAng;
float offFactor = pow(projDistFrac, 2.0 + abs(angleFrac) * projDistFrac);
float offsetY = (1. - offFactor) * circDistFrac + offFactor * projDistFrac;
offsetY = 1. - offsetY;
if(isnan(offsetX) || isnan(offsetY))
cout << "OFFSET VALUE is NaN" << endl;
return ofVec2f(offsetX - .5, offsetY);
}
void AudioStats::on_Tick_timeout() {
AudioInputPtr ai = g.ai;
if (ai.get() == NULL || ! ai->sppPreprocess)
return;
bool nTalking = ai->isTransmitting();
QString txt;
txt.sprintf("%06.2f dB",ai->dPeakMic);
qlMicLevel->setText(txt);
txt.sprintf("%06.2f dB",ai->dPeakSpeaker);
qlSpeakerLevel->setText(txt);
txt.sprintf("%06.2f dB",ai->dPeakSignal);
qlSignalLevel->setText(txt);
spx_int32_t ps_size = 0;
speex_preprocess_ctl(ai->sppPreprocess, SPEEX_PREPROCESS_GET_PSD_SIZE, &ps_size);
STACKVAR(spx_int32_t, noise, ps_size);
STACKVAR(spx_int32_t, ps, ps_size);
speex_preprocess_ctl(ai->sppPreprocess, SPEEX_PREPROCESS_GET_PSD, ps);
speex_preprocess_ctl(ai->sppPreprocess, SPEEX_PREPROCESS_GET_NOISE_PSD, noise);
float s = 0.0f;
float n = 0.0001f;
int start = (ps_size * 300) / SAMPLE_RATE;
int stop = (ps_size * 2000) / SAMPLE_RATE;
for (int i=start;i<stop;i++) {
s += sqrtf(static_cast<float>(ps[i]));
n += sqrtf(static_cast<float>(noise[i]));
}
txt.sprintf("%06.3f",s / n);
qlMicSNR->setText(txt);
spx_int32_t v;
speex_preprocess_ctl(ai->sppPreprocess, SPEEX_PREPROCESS_GET_AGC_GAIN, &v);
float fv = powf(10.0f, (static_cast<float>(v) / 20.0f));
txt.sprintf("%03.0f%%",100.0f / fv);
qlMicVolume->setText(txt);
txt.sprintf("%03.0f%%",ai->fSpeechProb * 100.0f);
qlSpeechProb->setText(txt);
txt.sprintf("%04.1f kbit/s",static_cast<float>(ai->iBitrate) / 1000.0f);
qlBitrate->setText(txt);
if (nTalking != bTalking) {
bTalking = nTalking;
QFont f = qlSpeechProb->font();
f.setBold(bTalking);
qlSpeechProb->setFont(f);
}
if (g.uiDoublePush > 1000000)
txt = tr(">1000 ms");
else
txt.sprintf("%04llu ms",g.uiDoublePush / 1000);
qlDoublePush->setText(txt);
abSpeech->iBelow = iroundf(g.s.fVADmin * 32767.0f + 0.5f);
abSpeech->iAbove = iroundf(g.s.fVADmax * 32767.0f + 0.5f);
if (g.s.vsVAD == Settings::Amplitude) {
abSpeech->iValue = iroundf((32767.f/96.0f) * (96.0f + ai->dPeakCleanMic) + 0.5f);
} else {
abSpeech->iValue = iroundf(ai->fSpeechProb * 32767.0f + 0.5f);
}
abSpeech->update();
anwNoise->update();
if (aewEcho)
aewEcho->update();
}
virtual void onDrawContent(SkCanvas* canvas) {
canvas->translate(SkIntToScalar(10), SkIntToScalar(10));
SkPaint bluePaint;
bluePaint.setARGB(0xff, 0x0, 0x0, 0xff);
SkPaint bmpPaint;
SkShader* bmpShader = SkShader::CreateBitmapShader(fBitmap, SkShader::kRepeat_TileMode, SkShader::kRepeat_TileMode);
bmpPaint.setShader(bmpShader);
bmpShader->unref();
bluePaint.setStrokeWidth(3);
bmpPaint.setStrokeWidth(3);
SkPaint paints[] = { bluePaint, bmpPaint };
SkRect rect;
SkScalar dx = SkIntToScalar(80);
SkScalar dy = SkIntToScalar(100);
SkMatrix matrix;
for (size_t p = 0; p < SK_ARRAY_COUNT(paints); ++p) {
for (int stroke = 0; stroke < 2; ++stroke) {
paints[p].setStyle(stroke ? SkPaint::kStroke_Style : SkPaint::kFill_Style);
for (int a = 0; a < 3; ++ a) {
paints[p].setAntiAlias(a > 0);
paints[p].setAlpha(a > 1 ? 0x80 : 0xff);
canvas->save();
rect = SkRect::MakeLTRB(SkFloatToScalar(0.f),
SkFloatToScalar(0.f),
SkFloatToScalar(40.f),
SkFloatToScalar(40.f));
canvas->drawRect(rect, paints[p]);
canvas->translate(dx, 0);
rect = SkRect::MakeLTRB(SkFloatToScalar(0.5f),
SkFloatToScalar(0.5f),
SkFloatToScalar(40.5f),
SkFloatToScalar(40.5f));
canvas->drawRect(rect, paints[p]);
canvas->translate(dx, 0);
rect = SkRect::MakeLTRB(SkFloatToScalar(0.5f),
SkFloatToScalar(0.5f),
SkFloatToScalar(40.f),
SkFloatToScalar(40.f));
canvas->drawRect(rect, paints[p]);
canvas->translate(dx, 0);
rect = SkRect::MakeLTRB(SkFloatToScalar(0.75f),
SkFloatToScalar(0.75f),
SkFloatToScalar(40.75f),
SkFloatToScalar(40.75f));
canvas->drawRect(rect, paints[p]);
canvas->translate(dx, 0);
canvas->save();
canvas->translate(SkFloatToScalar(.33f), SkFloatToScalar(.67f));
rect = SkRect::MakeLTRB(SkFloatToScalar(0.0f),
SkFloatToScalar(0.0f),
SkFloatToScalar(40.0f),
SkFloatToScalar(40.0f));
canvas->drawRect(rect, paints[p]);
canvas->restore();
canvas->translate(dx, 0);
canvas->save();
matrix.setRotate(SkFloatToScalar(45.f));
canvas->concat(matrix);
canvas->translate(SkFloatToScalar(20.0f / sqrtf(2.f)),
SkFloatToScalar(20.0f / sqrtf(2.f)));
rect = SkRect::MakeLTRB(SkFloatToScalar(-20.0f),
SkFloatToScalar(-20.0f),
SkFloatToScalar(20.0f),
SkFloatToScalar(20.0f));
canvas->drawRect(rect, paints[p]);
canvas->restore();
canvas->translate(dx, 0);
canvas->save();
canvas->rotate(SkFloatToScalar(90.f));
rect = SkRect::MakeLTRB(SkFloatToScalar(0.0f),
SkFloatToScalar(0.0f),
SkFloatToScalar(40.0f),
SkFloatToScalar(-40.0f));
canvas->drawRect(rect, paints[p]);
canvas->restore();
canvas->translate(dx, 0);
canvas->save();
canvas->rotate(SkFloatToScalar(90.f));
rect = SkRect::MakeLTRB(SkFloatToScalar(0.5f),
SkFloatToScalar(0.5f),
SkFloatToScalar(40.5f),
SkFloatToScalar(-40.5f));
canvas->drawRect(rect, paints[p]);
canvas->restore();
canvas->translate(dx, 0);
canvas->save();
matrix.setScale(SkFloatToScalar(-1.f), SkFloatToScalar(-1.f));
canvas->concat(matrix);
rect = SkRect::MakeLTRB(SkFloatToScalar(0.5f),
SkFloatToScalar(0.5f),
SkFloatToScalar(-40.5f),
SkFloatToScalar(-40.5f));
canvas->drawRect(rect, paints[p]);
canvas->restore();
canvas->translate(dx, 0);
canvas->save();
matrix.setScale(SkFloatToScalar(2.1f), SkFloatToScalar(4.1f));
canvas->concat(matrix);
rect = SkRect::MakeLTRB(SkFloatToScalar(0.1f),
SkFloatToScalar(0.1f),
SkFloatToScalar(19.1f),
SkFloatToScalar(9.1f));
canvas->drawRect(rect, paints[p]);
canvas->restore();
canvas->translate(dx, 0);
canvas->restore();
canvas->translate(0, dy);
}
}
}
}
