static void
make_color_inactive (EelBackground *self,
GdkColor *color)
{
double intensity, saturation;
gushort t;
if (!self->details->is_active) {
saturation = 0.7;
intensity = color->red * 0.30 + color->green * 0.59 + color->blue * 0.11;
color->red = SATURATE (color->red);
color->green = SATURATE (color->green);
color->blue = SATURATE (color->blue);
if (intensity > G_MAXUSHORT / 2)
{
color->red *= 0.9;
color->green *= 0.9;
color->blue *= 0.9;
}
else
{
color->red *= 1.25;
color->green *= 1.25;
color->blue *= 1.25;
}
color->red = CLAMP_COLOR (color->red);
color->green = CLAMP_COLOR (color->green);
color->blue = CLAMP_COLOR (color->blue);
}
}
void
eel_make_color_inactive (GdkRGBA *color)
{
double intensity, saturation;
gdouble t;
saturation = 0.7;
intensity = color->red * 0.30 + color->green * 0.59 + color->blue * 0.11;
color->red = SATURATE (color->red);
color->green = SATURATE (color->green);
color->blue = SATURATE (color->blue);
if (intensity > 0.5) {
color->red *= 0.9;
color->green *= 0.9;
color->blue *= 0.9;
} else {
color->red *= 1.25;
color->green *= 1.25;
color->blue *= 1.25;
}
color->red = CLAMP_COLOR (color->red);
color->green = CLAMP_COLOR (color->green);
color->blue = CLAMP_COLOR (color->blue);
}
void qmf_decomp(const spx_word16_t *xx, const spx_word16_t *aa, spx_sig_t *y1, spx_sig_t *y2, int N, int M, spx_word16_t *mem, char *stack)
{
int i,j,k,M2;
spx_word16_t *a;
spx_word16_t *x;
spx_word16_t *x2;
a = PUSH(stack, M, spx_word16_t);
x = PUSH(stack, N+M-1, spx_word16_t);
x2=x+M-1;
M2=M>>1;
for (i=0;i<M;i++)
a[M-i-1]= aa[i];
for (i=0;i<M-1;i++)
x[i]=mem[M-i-2];
for (i=0;i<N;i++)
x[i+M-1]=SATURATE(PSHR(xx[i],1),16383);
for (i=0,k=0;i<N;i+=2,k++)
{
y1[k]=0;
y2[k]=0;
for (j=0;j<M2;j++)
{
y1[k]+=SHR(MULT16_16(a[j],ADD16(x[i+j],x2[i-j])),1);
y2[k]-=SHR(MULT16_16(a[j],SUB16(x[i+j],x2[i-j])),1);
j++;
y1[k]+=SHR(MULT16_16(a[j],ADD16(x[i+j],x2[i-j])),1);
y2[k]+=SHR(MULT16_16(a[j],SUB16(x[i+j],x2[i-j])),1);
}
}
for (i=0;i<M-1;i++)
mem[i]=SATURATE(PSHR(xx[N-i-1],1),16383);
}
void Drawer::SetCamPosCenter(int pCamPosX, int pCamPosY)
{
pCamPosX -= tilesOnScreenX * TILESIZE / 2;
pCamPosY -= tilesOnScreenY * TILESIZE / 2;
m_camPosX = SATURATE(pCamPosX, m_camPosXMin, m_camPosXMax);
m_camPosY = SATURATE(pCamPosY, m_camPosYMin, m_camPosYMax);
}
/*\ Filter an image with the a, r, g, b filters in fil
|*| NB: This is currently not very optimal, and could probably be improved
\*/
void
__imlib_FilterImage(ImlibImage * im, ImlibFilter * fil)
{
int x, y, a, r, g, b, ad, rd, gd, bd;
DATA32 *data, *p1, *p2;
data = malloc(im->w * im->h * sizeof(DATA32));
if (!data)
return;
ad = __imlib_FilterCalcDiv(&fil->alpha);
rd = __imlib_FilterCalcDiv(&fil->red);
gd = __imlib_FilterCalcDiv(&fil->green);
bd = __imlib_FilterCalcDiv(&fil->blue);
p1 = im->data;
p2 = data;
for (y = 0; y < im->h; y++)
{
for (x = 0; x < im->w; x++)
{
*p2 = *p1;
if (ad)
{
a = __imlib_FilterGet(&fil->alpha, im->data, im->w, im->h, x,
y);
a /= ad;
A_VAL(p2) = SATURATE(a);
}
if (rd)
{
r = __imlib_FilterGet(&fil->red, im->data, im->w, im->h, x,
y);
r /= rd;
R_VAL(p2) = SATURATE(r);
}
if (gd)
{
g = __imlib_FilterGet(&fil->green, im->data, im->w, im->h, x,
y);
g /= gd;
G_VAL(p2) = SATURATE(g);
}
if (bd)
{
b = __imlib_FilterGet(&fil->blue, im->data, im->w, im->h, x,
y);
b /= bd;
B_VAL(p2) = SATURATE(b);
}
p1++;
p2++;
}
}
free(im->data);
im->data = data;
}
double Object::G(const Vector &wo, const Vector &wi, const Vector &wh, const Vector &n)
{
double NdotWH = SATURATE(Vector::dot(n, wh));
double NdotWO = SATURATE(Vector::dot(n, wo));
double NdotWI = SATURATE(Vector::dot(n, wi));
double WOdotWh = SATURATE(Vector::dot(wo, wh));
return std::min(1.0, std::min((2.0 * NdotWH * NdotWO / WOdotWh), (2.0 * NdotWH * NdotWI / WOdotWh)));
}
/**
* @internal
* @param vbi Initialized vbi decoding context.
* @param d Destination palette.
* @param s Source palette.
* @param entries Size of source and destination palette.
*
* Transposes the source palette by @a vbi->brightness and @a vbi->contrast.
*/
void
vbi_transp_colormap(vbi_decoder *vbi, vbi_rgba *d, vbi_rgba *s, int entries)
{
int brig, cont;
brig = SATURATE(vbi->brightness, 0, 255);
cont = SATURATE(vbi->contrast, -128, +127);
while (entries--) {
*d++ = VBI_RGBA(transp(VBI_R(*s), brig, cont),
transp(VBI_G(*s), brig, cont),
transp(VBI_B(*s), brig, cont));
s++;
}
}
static inline int
transp(int val, int brig, int cont)
{
int r = (((val - 128) * cont) / 64) + brig;
return SATURATE(r, 0, 255);
}
void generateAdaptativeCodebookVector(word16_t excitationVector[], int16_t intPitchDelay, int16_t fracPitchDelay)
{
int n,i,j;
word16_t *delayedExcitationVector;
word16_t *b30Increased;
word16_t *b30Decreased;
/* fracPitchDelay is in range [-1, 1], convert it to [0,2] needed by eqA.8 */
fracPitchDelay = -fracPitchDelay;
if (fracPitchDelay <0) { /* if fracPitchDelay is 1 -> pitchDelay of int+(1/3) -> int+1-(2/3)*/
intPitchDelay++;
fracPitchDelay = 2;
}
/**/
delayedExcitationVector = &(excitationVector[-intPitchDelay]); /* delayedExcitationVector is used to address the excitation vector at index -intPitchDelay (-k in eq40) */
b30Increased = &(b30[fracPitchDelay]); /* b30 increased points to b30[fracPitchDelay] : b30[t] in eq40. b30 in Q15 */
b30Decreased = &(b30[3-fracPitchDelay]); /* b30 decreased points to b30[-fracPitchDelay] : b30[3-t] in eq40. b30 in Q15 */
for (n=0; n<L_SUBFRAME; n++) {
word32_t acc = 0; /* acc in Q15 */
for (i=0, j=0; i<10; i++, j+=3) { /* j is used as a 3*i index */
acc = MAC16_16(acc, delayedExcitationVector[n-i], b30Increased[j]); /* WARNING: spec 3.7.1 and A.8 give an equation leading to delayedExcitationVector[n+i] but ITU code uses delayedExcitationVector[n-i], implemented as code */
acc = MAC16_16(acc, delayedExcitationVector[n+1+i], b30Decreased[j]);
}
excitationVector[n] = SATURATE(PSHR(acc, 15), MAXINT16); /* acc in Q15, shift/round to unscaled value and check overflow on 16 bits */
}
}
void preProcessing(bcg729EncoderChannelContextStruct *encoderChannelContext, word16_t signal[], word16_t preProcessedSignal[]) {
int i;
word16_t inputX2;
word32_t acc; /* in Q12 */
for(i=0; i<L_FRAME; i++) {
inputX2 = encoderChannelContext->inputX1;
encoderChannelContext->inputX1 = encoderChannelContext->inputX0;
encoderChannelContext->inputX0 = signal[i];
/* compute with acc and coefficients in Q12 */
acc = MULT16_32_Q12(A1, encoderChannelContext->outputY1); /* Y1 in Q15.12 * A1 in Q1.12 -> acc in Q17.12*/
acc = MAC16_32_Q12(acc, A2, encoderChannelContext->outputY2); /* Y2 in Q15.12 * A2 in Q0.12 -> Q15.12 + acc in Q17.12 -> acc in Q18.12 */
/* 3*(Xi in Q15.0 * Bi in Q0.12)->Q17.12 + acc in Q18.12 -> acc in 19.12 */
acc = MAC16_16(acc, encoderChannelContext->inputX0, B0);
acc = MAC16_16(acc, encoderChannelContext->inputX1, B1);
acc = MAC16_16(acc, inputX2, B2);
/* acc in Q19.12 : We must check it won't overflow
- the Q15.12 of Y
- the Q15.0 extracted from it by shifting 12 right
-> saturate to 28 bits -> acc in Q15.12 */
acc = SATURATE(acc, MAXINT28);
preProcessedSignal[i] = PSHR(acc,12); /* extract integer value of the Q15.12 representation */
encoderChannelContext->outputY2 = encoderChannelContext->outputY1;
encoderChannelContext->outputY1 = acc;
}
return;
}
void CBurnEffectManager::Update(float deltaTime)
{
TBurnPoints::iterator it = m_burnPoints.begin();
for (; it != m_burnPoints.end();)
{
SBurnPoint& burnPoint = *it;
if (burnPoint.m_accumulationDelay > 0.0f)
burnPoint.m_accumulationDelay -= deltaTime;
else
burnPoint.m_accumulation -= deltaTime * burnPoint.m_pBurnParams->m_dissipation;
burnPoint.m_accumulation = SATURATE(burnPoint.m_accumulation);
if (burnPoint.m_accumulation <= 0)
{
DestroyBurnEffect(&burnPoint);
it = m_burnPoints.erase(it);
}
else
{
UpdateBurnEffect(&burnPoint);
++it;
}
}
ApplySurroundingDamage(deltaTime);
if (g_pGameCVars->pl_mike_debug)
DebugDraw();
}
void SaturateColor3f(CVec3 &color)
{
float sat = r_saturation->value;
if (sat != 1.0f)
{
float r = color[0] * 255;
float g = color[1] * 255;
float b = color[2] * 255;
float light = (r + g + b) / 3;
SATURATE(r,light,sat);
SATURATE(g,light,sat);
SATURATE(b,light,sat);
color[0] = r / 255;
color[1] = g / 255;
color[2] = b / 255;
}
}
void SaturateColor4b(color_t *c)
{
float sat = r_saturation->value;
if (sat != 1.0f)
{
float r = c->c[0];
float g = c->c[1];
float b = c->c[2];
float light = (r + g + b) / 3;
SATURATE(r,light,sat);
SATURATE(g,light,sat);
SATURATE(b,light,sat);
c->c[0] = appRound(r);
c->c[1] = appRound(g);
c->c[2] = appRound(b);
}
}
Color Object::sample_torrancesparrow(const Vector &wo, Vector &wi, const Vector &n, const Point &p, unsigned short *Xi, double &pdf)
{
// inverting wo
Vector eye = (wo * (-1)).normalize();
Vector wh;
// if not direct light
if(wi.length() > 1.1)
{
// importance sampling
wh = randomImportanceSamplingHalfway(n, Xi);
// if wh is not in the same hemisphere, turn it;
if(Vector::dot(eye, wh) <= 0.0) wh = wh * (-1);
// reflecting wo based on wh
wi = wo + 2 * Vector::dot(wh, eye) * wh;
}
else
{
// do not calculate torrancesparrow on direct illumination
pdf = 1;
return Color();
}
// cos and plus cos
double cos_o = SATURATE(Vector::dot(n, eye));
double cos_i = SATURATE(Vector::dot(n, wi));
double cos_alpha = SATURATE(Vector::dot(eye, wh));
double cos_h = SATURATE(Vector::dot(n, wh));
if(cos_i == 0.f || cos_o == 0.f)
{
pdf = 1;
return Color();
}
pdf = (((material.alpha+1.0) * pow(cos_h, material.alpha)) / (2.0 * M_PI * 4.f * cos_alpha));
// if pdf 0, resampling
if(pdf <= 0.0) return sample_torrancesparrow(wo, wi, n, p, Xi, pdf);
Color F;
F.r = FCond(cos_alpha, material.absortion.r);
F.g = FCond(cos_alpha, material.absortion.g);
F.b = FCond(cos_alpha, material.absortion.b);
return (get_color(p) * (F * ((D(wh, n) * G(eye, wi, wh, n) / (4.f * cos_i * cos_o))))) * material.kA;
}
void iir_mem2(const spx_sig_t *x, const spx_coef_t *den, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
{
int i,j;
spx_word32_t xi,yi,nyi;
for (i=0;i<N;i++)
{
int yh,yl;
xi=SATURATE(x[i],805306368);
yi = SATURATE(xi + (mem[0]<<2),805306368);
nyi = -yi;
yh = yi>>15; yl=yi&0x00007fff;
for (j=0;j<ord-1;j++)
{
mem[j] = MAC16_32_Q15(mem[j+1],den[j+1],nyi);
}
mem[ord-1] = - MULT16_32_Q15(den[ord],yi);
y[i] = yi;
}
}
void fir_mem2(const spx_sig_t *x, const spx_coef_t *num, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
{
int i,j;
spx_word32_t xi,yi;
for (i=0;i<N;i++)
{
int xh,xl;
xi=SATURATE(x[i],805306368);
yi = xi + (mem[0]<<2);
xh = xi>>15; xl=xi&0x00007fff;
for (j=0;j<ord-1;j++)
{
mem[j] = MAC16_32_Q15(mem[j+1], num[j+1],xi);
}
mem[ord-1] = MULT16_32_Q15(num[ord],xi);
y[i] = SATURATE(yi,805306368);
}
}
void filter_mem2(const spx_sig_t *x, const spx_coef_t *num, const spx_coef_t *den, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
{
int i,j;
spx_sig_t xi,yi,nyi;
for (i=0;i<N;i++)
{
int xh,xl,yh,yl;
xi=SATURATE(x[i],805306368);
yi = SATURATE(ADD32(xi, SHL(mem[0],2)),805306368);
nyi = -yi;
xh = xi>>15; xl=xi&0x00007fff; yh = yi>>15; yl=yi&0x00007fff;
for (j=0;j<ord-1;j++)
{
mem[j] = MAC16_32_Q15(MAC16_32_Q15(mem[j+1], num[j+1],xi), den[j+1],nyi);
}
mem[ord-1] = SUB32(MULT16_32_Q15(num[ord],xi), MULT16_32_Q15(den[ord],yi));
y[i] = yi;
}
}
/** Forced pitch delay and gain */
int forced_pitch_quant(spx_word16_t target[], /* Target vector */
spx_word16_t * sw, spx_coef_t ak[], /* LPCs for this subframe */
spx_coef_t awk1[], /* Weighted LPCs #1 for this subframe */
spx_coef_t awk2[], /* Weighted LPCs #2 for this subframe */
spx_sig_t exc[], /* Excitation */
const void *par, int start, /* Smallest pitch value allowed */
int end, /* Largest pitch value allowed */
spx_word16_t pitch_coef, /* Voicing (pitch) coefficient */
int p, /* Number of LPC coeffs */
int nsf, /* Number of samples in subframe */
SpeexBits * bits,
char *stack,
spx_word16_t * exc2,
spx_word16_t * r,
int complexity,
int cdbk_offset,
int plc_tuning, spx_word32_t * cumul_gain)
{
(void)sw;
(void)par;
(void)end;
(void)bits;
(void)r;
(void)complexity;
(void)cdbk_offset;
(void)plc_tuning;
(void)cumul_gain;
int i;
spx_word16_t res[nsf];
#ifdef FIXED_POINT
if (pitch_coef > 63)
pitch_coef = 63;
#else
if (pitch_coef > .99)
pitch_coef = .99;
#endif
for (i = 0; i < nsf && i < start; i++) {
exc[i] = MULT16_16(SHL16(pitch_coef, 7), exc2[i - start]);
}
for (; i < nsf; i++) {
exc[i] = MULT16_32_Q15(SHL16(pitch_coef, 9), exc[i - start]);
}
for (i = 0; i < nsf; i++)
res[i] = EXTRACT16(PSHR32(exc[i], SIG_SHIFT - 1));
syn_percep_zero16(res, ak, awk1, awk2, res, nsf, p, stack);
for (i = 0; i < nsf; i++)
target[i] =
EXTRACT16(SATURATE
(SUB32(EXTEND32(target[i]), EXTEND32(res[i])),
32700));
return start;
}
//------------------------------------------------------------------------
void CGameRulesCommonDamageHandling::ClProcessHit(Vec3 dir, EntityId shooterId, EntityId weaponId, float damage, uint16 projectileClassId, uint8 hitTypeId)
{
const char* hit = g_pGame->GetGameRules()->GetHitType(hitTypeId);
if(hit)
{
IActor *pClientActor = gEnv->pGame->GetIGameFramework()->GetClientActor();
string hitSound;
hitSound.Format("ClientDamage%s", hit);
float maxHealth = pClientActor->GetMaxHealth();
float normalizedDamage = SATURATE(maxHealth > 0.0f ? damage / maxHealth : 0.0f);
CAudioSignalPlayer::JustPlay(hitSound.c_str(), pClientActor->GetEntityId(), "damage", normalizedDamage);
}
}
void LPSynthesisFilter (word16_t *excitationVector, word16_t *LPCoefficients, word16_t *reconstructedSpeech)
{
int i;
/* compute excitationVector[i] - Sum0-9(LPCoefficients[j]*reconstructedSpeech[i-j]) */
for (i=0; i<L_SUBFRAME; i++) {
word32_t acc = SHL(excitationVector[i],12); /* acc get the first term of the sum, in Q12 (excitationVector is in Q0)*/
int j;
for (j=0; j<NB_LSP_COEFF; j++) {
acc = MSU16_16(acc, LPCoefficients[j], reconstructedSpeech[i-j-1]);
}
reconstructedSpeech[i] = (word16_t)SATURATE(PSHR(acc, 12), MAXINT16); /* shift right acc to get it back in Q0 and check overflow on 16 bits */
}
return;
}
virtual void DrawParticle(const render_context_t &context, particle_t &particle)
{
if (img[0] == nullptr)
{
img[0] = MaterialPrecache::GetInstance()->GetMaterial(material[0].c_str());
for (int i = 1; i < materialCount; i++)
{
if (material[i].length() == 0)
break;
img[i] = MaterialPrecache::GetInstance()->GetMaterial(material[i].c_str());
}
}
Vector2D delta(particle.data_runtime.size, particle.data_runtime.size);
if (!IsScreenspaceScale())
{
Camera::GetInstance()->Scale(delta);
}
float size = delta.x * 2.0f;
Vector2D origin = particle.data_runtime.origin;
if (IsScreenspacePosition())
{
VECTOR_PROPORTIONAL_SCREENSPACE(origin);
}
else
{
Camera::GetInstance()->ToScreen(origin);
}
float oldOpacity = context.painter->opacity();
context.painter->setOpacity(oldOpacity * particle.data_runtime.color[3]);
int materialIndex = 0;
if (materialCount > 1)
{
materialIndex = SATURATE((float)particle.seed / (float)USHRT_MAX)
* materialCount;
materialIndex = CLAMP(materialIndex, 0, materialCount - 1);
}
img[materialIndex]->Render(origin, particle.data_runtime.angle,
size, context);
context.painter->setOpacity(oldOpacity);
}
void CMelee::ApplyMeleeDamage(const Vec3& point, const Vec3& dir, const Vec3& normal, IPhysicalEntity* physicalEntity,
EntityId entityID, int partId, int ipart, int surfaceIdx, bool remote, int iPrim)
{
const float blend = m_slideKick ? 0.4f : SATURATE(m_pMeleeParams->meleeparams.impulse_up_percentage);
Vec3 impulseDir = Vec3(0, 0, 1) * blend + dir * (1.0f - blend);
impulseDir.normalize();
int hitTypeID = Hit(point, dir, normal, physicalEntity, entityID, partId, ipart, surfaceIdx, remote);
IActor* pActor = gEnv->bMultiplayer && entityID ? g_pGame->GetIGameFramework()->GetIActorSystem()->GetActor(entityID) : NULL;
if(!gEnv->bMultiplayer || !pActor) //MP handles melee impulses to actors in GameRulesClientServer.cpp. See: ClApplyActorMeleeImpulse)
{
Impulse(point, impulseDir, normal, physicalEntity, entityID, partId, ipart, surfaceIdx, hitTypeID, iPrim);
}
}
Color Object::sample_lamb(const Vector &wo, Vector &wi, const Vector &n, const Point &p, unsigned short *Xi, double &pdf)
{
if(wi.length() > 1.1)
wi = randomHemisphereVector(n, wo, Xi);
double cos_i = SATURATE(Vector::dot(wi, n));
pdf = cos_i * M_1_PI;
if(pdf <= 0.0)
{
wi = randomHemisphereVector(n, wo, Xi);
return sample_lamb(wo, wi, n, p, Xi, pdf);
}
// albedo * cdiff/PI * cos_i
return get_color(p) * material.kD * M_1_PI * cos_i;
}
Color Object::sample_blinnphong(const Vector &wo, Vector &wi, const Vector &n, const Point &p, unsigned short *Xi, double &pdf)
{
if(wi.length() > 1.1)
wi = randomImportanceSampling(n, wo, Xi);
else // bug fix for direct illumination
return sample_lamb(wo, wi, n, p, Xi, pdf);
// halfway vector
Vector eye = (wo * (-1)).normalize();
Vector wh = (wi + eye).normalize();
double cos_h = SATURATE(Vector::dot(wh, n));
// pdf
pdf = ((material.alpha + 2.0) / (2.0 * M_PI)) * pow(cos_h, material.alpha);
// if pdf 0, resampling
if(pdf <= 0.0) return sample_blinnphong(wo, wi, n, p, Xi, pdf);
// albedo * m+8/8PI * cspec * (h . n)^m
return get_color(p) * (((material.alpha+8)/(8*M_PI)) * material.kS * pow(cos_h, material.alpha));
}
SWeaponOffset CWeaponOffsetState::ComputeCurrentOffset()
{
SWeaponOffset result;
if (m_transitiontime > 0.0f)
{
float t = Hermite2(SATURATE(m_time / m_transitiontime));
result.m_position = LERP(m_last.m_position, m_current.m_position, t);
result.m_rotation = Lerp(m_last.m_rotation, m_current.m_rotation, t);
}
else
{
result = m_current;
}
return result;
}
void CLightingManager::CullLights()
{
Assert( m_hRenderLights.Count() == 0 );
FOR_EACH_VEC_FAST( def_light_t*, m_hDeferredLights, l )
{
if ( !m_bDrawWorldLights && l->bWorldLight )
continue;
if ( !render->AreAnyLeavesVisible( l->iLeaveIDs, l->iNumLeaves ) )
continue;
// if the optimized bounds cause popping for you, use the naive ones or
// ...improve the optimization code
//if( !engine->IsBoxInViewCluster( l->bounds_min_naive, l->bounds_max_naive ) )
// continue;
if ( engine->CullBox( l->bounds_min_naive, l->bounds_max_naive ) )
//if ( engine->CullBox( l->bounds_min, l->bounds_max ) )
continue;
if ( l->IsSpot() && l->HasShadow() )
{
if ( IntersectFrustumWithFrustum( m_matScreenToWorld, l->spotMVPInv ) )
continue;
}
Vector veclightDelta = l->boundsCenter - m_vecViewOrigin;
if ( veclightDelta.LengthSqr() > l->flMaxDistSqr )
continue;
l->flDistance_ViewOrigin = veclightDelta.Length();
l->flShadowFade = l->HasShadow() ?
( SATURATE( ( l->flDistance_ViewOrigin - l->iShadow_Dist ) / l->iShadow_Range ) )
: 1.0f;
m_hRenderLights.AddToTail( l );
}
FOR_EACH_VEC_FAST_END
}
void FillSplotch( const DrawUtils::DrawInfos& i, Pixel& P )
{
Noise& N = *((Noise*) i.pData);
NjFloat2 UV = i.UV;
UV.x -= 0.5f;
UV.y -= 0.5f;
float Scale = 1.0f + 1.0f * N.Perlin( NjFloat2( 0.005f * i.x / i.w, 0.005f * i.y / i.h ) );
UV.x *= Scale;
UV.y *= Scale;
float Distance2Center = UV.Length();
float C = 1.2f * (1.0f - 2.0f * Distance2Center);
C = SATURATE( C );
float A = C * C * i.Coverage;
NjFloat4 Color( C, C, C, A );
P.Blend( Pixel( Color ), Color.w );
}
void LowPass2::Filter(signed short int *Buff, int Tam)
{
int a;
int Tmp, Tmp2;
for (a = 0; a < Tam*2; a += 2)
{
Tmp = (b0*Buff[a] + b1*i1 + b2*i2
- a1*o1 - a2*o2) / (1 << FixBits);
Tmp2 = (b0b*Buff[a] + b1b*i1 + b2b*i2
- a1b*o1b - a2b*o2b) / (1 << FixBits);
i2 = i1;
i1 = Buff[a];
o2 = o1;
o1 = Tmp;
o2b = o1b;
o1b = Tmp2;
Buff[a] = (short)SATURATE(-32768, 32767, Tmp + Tmp2);
}
}
void handleCommMessage(void){
char buffer[COMM_BUFFER_SIZE];
unsigned char function_code = 0;
long i;
buffer[64]=0;
buffer[79]=0;
UARTgets((uint8*)buffer, COMM_BUFFER_SIZE);
//UARTgetMessage(buffer);
//UARTprintf("MESSAGE GET!!%s\r\n", buffer);
if(isValidMessage(buffer))
{
for(i = 0; i < CODE_LENGTH; i++) //generate hash of function code
{
function_code ^= buffer[CODE_START + i];
}
switch(function_code)
{
case SPLM: //JoystickXOut(SATURATE(atoi(&buffer[DATA_START]),0,255));
//UARTprintf("GOOD MESSAGE- SVXA: %s\r\n", buffer);
SetLeftMotor(SATURATE(atoi(&buffer[DATA_START]),-128,127));
VelCtrlRunning = 0;
ResetWatchdog(); //we got a valid message, so they are still talking to us
break;
case SPRM: //JoystickYOut(SATURATE(atoi(&buffer[DATA_START]),0,255));
//UARTprintf("GOOD MESSAGE- SVYA: %s\r\n", buffer);
SetRightMotor(SATURATE(atoi(&buffer[DATA_START]),-128,127));
VelCtrlRunning = 0;
ResetWatchdog(); //we got a valid message, so they are still talking to us
break;
case SAHS: SetHokuyoServo(SATURATE(atoi(&buffer[DATA_START]),-128,127));
break;
case SVLX: UpdateLinearX(atoi(&buffer[DATA_START]));
ResetWatchdog();
VelCtrlRunning = 1;
break;
case SVAZ: UpdateAngularZ(atoi(&buffer[DATA_START]));
VelCtrlRunning = 1;
ResetWatchdog();
break;
case ETFM: EnableSensorFeedbackMessages = 1;
break;
case DTFM: EnableSensorFeedbackMessages = 0;
break;
case RSTE: ResetEncoders();
break;
case STMR: if(1000 % atoi(&buffer[DATA_START]) == 0)
SetMessageRate(atoi(&buffer[DATA_START]));
else
UARTprintf("INVALID MESSAGE RATE (1000%rate must = 0): %s", buffer);
break;
case SETT: SetTime(atoi(&buffer[DATA_START]));
break;
case EHST: EnableHokuyoTilting();
break;
case DHST: DisableHokuyoTilting();
break;
case SHTR: if(1000 % atoi(&buffer[DATA_START]) == 0)
SetHokuyoTiltRate(atoi(&buffer[DATA_START]));
else
UARTprintf("INVALID RATE (1000%rate must = 0): %s", buffer);
break;
case SACD: SetAccelDivisor(atoi(&buffer[DATA_START]));
break;
/*case FLPC: Servo_0_WriteCompare1(atoi(&buffer[DATA_START]));
break;
case FRPC: Servo_0_WriteCompare2(atoi(&buffer[DATA_START]));
break;*/
default: UARTprintf("UNRECOGNIZED MESSAGE: %s\r\n", buffer);
break;
}
}
else
{
UARTprintf("INVALID START CHARACTER: %s\r\n", buffer);
}
}
static int opus_decode_frame(OpusDecoder *st, const unsigned char *data,
opus_int32 len, opus_val16 *pcm, int frame_size, int decode_fec)
{
void *silk_dec;
CELTDecoder *celt_dec;
int i, silk_ret=0, celt_ret=0;
ec_dec dec;
opus_int32 silk_frame_size;
int pcm_silk_size;
VARDECL(opus_int16, pcm_silk);
int pcm_transition_silk_size;
VARDECL(opus_val16, pcm_transition_silk);
int pcm_transition_celt_size;
VARDECL(opus_val16, pcm_transition_celt);
opus_val16 *pcm_transition=NULL;
int redundant_audio_size;
VARDECL(opus_val16, redundant_audio);
int audiosize;
int mode;
int transition=0;
int start_band;
int redundancy=0;
int redundancy_bytes = 0;
int celt_to_silk=0;
int c;
int F2_5, F5, F10, F20;
const opus_val16 *window;
opus_uint32 redundant_rng = 0;
int celt_accum;
ALLOC_STACK;
silk_dec = (char*)st+st->silk_dec_offset;
celt_dec = (CELTDecoder*)((char*)st+st->celt_dec_offset);
F20 = st->Fs/50;
F10 = F20>>1;
F5 = F10>>1;
F2_5 = F5>>1;
if (frame_size < F2_5)
{
RESTORE_STACK;
return OPUS_BUFFER_TOO_SMALL;
}
/* Limit frame_size to avoid excessive stack allocations. */
frame_size = IMIN(frame_size, st->Fs/25*3);
/* Payloads of 1 (2 including ToC) or 0 trigger the PLC/DTX */
if (len<=1)
{
data = NULL;
/* In that case, don't conceal more than what the ToC says */
frame_size = IMIN(frame_size, st->frame_size);
}
if (data != NULL)
{
audiosize = st->frame_size;
mode = st->mode;
ec_dec_init(&dec,(unsigned char*)data,len);
} else {
audiosize = frame_size;
mode = st->prev_mode;
if (mode == 0)
{
/* If we haven't got any packet yet, all we can do is return zeros */
for (i=0; i<audiosize*st->channels; i++)
pcm[i] = 0;
RESTORE_STACK;
return audiosize;
}
/* Avoids trying to run the PLC on sizes other than 2.5 (CELT), 5 (CELT),
10, or 20 (e.g. 12.5 or 30 ms). */
if (audiosize > F20)
{
do {
int ret = opus_decode_frame(st, NULL, 0, pcm, IMIN(audiosize, F20), 0);
if (ret<0)
{
RESTORE_STACK;
return ret;
}
pcm += ret*st->channels;
audiosize -= ret;
} while (audiosize > 0);
RESTORE_STACK;
return frame_size;
} else if (audiosize < F20)
{
if (audiosize > F10)
audiosize = F10;
else if (mode != MODE_SILK_ONLY && audiosize > F5 && audiosize < F10)
audiosize = F5;
}
}
/* In fixed-point, we can tell CELT to do the accumulation on top of the
SILK PCM buffer. This saves some stack space. */
#ifdef FIXED_POINT
celt_accum = (mode != MODE_CELT_ONLY) && (frame_size >= F10);
#else
celt_accum = 0;
#endif
pcm_transition_silk_size = ALLOC_NONE;
pcm_transition_celt_size = ALLOC_NONE;
if (data!=NULL && st->prev_mode > 0 && (
(mode == MODE_CELT_ONLY && st->prev_mode != MODE_CELT_ONLY && !st->prev_redundancy)
|| (mode != MODE_CELT_ONLY && st->prev_mode == MODE_CELT_ONLY) )
)
{
transition = 1;
/* Decide where to allocate the stack memory for pcm_transition */
if (mode == MODE_CELT_ONLY)
pcm_transition_celt_size = F5*st->channels;
else
pcm_transition_silk_size = F5*st->channels;
}
ALLOC(pcm_transition_celt, pcm_transition_celt_size, opus_val16);
if (transition && mode == MODE_CELT_ONLY)
{
pcm_transition = pcm_transition_celt;
opus_decode_frame(st, NULL, 0, pcm_transition, IMIN(F5, audiosize), 0);
}
if (audiosize > frame_size)
{
/*fprintf(stderr, "PCM buffer too small: %d vs %d (mode = %d)\n", audiosize, frame_size, mode);*/
RESTORE_STACK;
return OPUS_BAD_ARG;
} else {
frame_size = audiosize;
}
/* Don't allocate any memory when in CELT-only mode */
pcm_silk_size = (mode != MODE_CELT_ONLY && !celt_accum) ? IMAX(F10, frame_size)*st->channels : ALLOC_NONE;
ALLOC(pcm_silk, pcm_silk_size, opus_int16);
/* SILK processing */
if (mode != MODE_CELT_ONLY)
{
int lost_flag, decoded_samples;
opus_int16 *pcm_ptr;
#ifdef FIXED_POINT
if (celt_accum)
pcm_ptr = pcm;
else
#endif
pcm_ptr = pcm_silk;
if (st->prev_mode==MODE_CELT_ONLY)
silk_InitDecoder( silk_dec );
/* The SILK PLC cannot produce frames of less than 10 ms */
st->DecControl.payloadSize_ms = IMAX(10, 1000 * audiosize / st->Fs);
if (data != NULL)
{
st->DecControl.nChannelsInternal = st->stream_channels;
if( mode == MODE_SILK_ONLY ) {
if( st->bandwidth == OPUS_BANDWIDTH_NARROWBAND ) {
st->DecControl.internalSampleRate = 8000;
} else if( st->bandwidth == OPUS_BANDWIDTH_MEDIUMBAND ) {
st->DecControl.internalSampleRate = 12000;
} else if( st->bandwidth == OPUS_BANDWIDTH_WIDEBAND ) {
st->DecControl.internalSampleRate = 16000;
} else {
st->DecControl.internalSampleRate = 16000;
silk_assert( 0 );
}
} else {
/* Hybrid mode */
st->DecControl.internalSampleRate = 16000;
}
}
lost_flag = data == NULL ? 1 : 2 * decode_fec;
decoded_samples = 0;
do {
/* Call SILK decoder */
int first_frame = decoded_samples == 0;
silk_ret = silk_Decode( silk_dec, &st->DecControl,
lost_flag, first_frame, &dec, pcm_ptr, &silk_frame_size );
if( silk_ret ) {
if (lost_flag) {
/* PLC failure should not be fatal */
silk_frame_size = frame_size;
for (i=0; i<frame_size*st->channels; i++)
pcm_ptr[i] = 0;
} else {
RESTORE_STACK;
return OPUS_INTERNAL_ERROR;
}
}
pcm_ptr += silk_frame_size * st->channels;
decoded_samples += silk_frame_size;
} while( decoded_samples < frame_size );
}
start_band = 0;
if (!decode_fec && mode != MODE_CELT_ONLY && data != NULL
&& ec_tell(&dec)+17+20*(st->mode == MODE_HYBRID) <= 8*len)
{
/* Check if we have a redundant 0-8 kHz band */
if (mode == MODE_HYBRID)
redundancy = ec_dec_bit_logp(&dec, 12);
else
redundancy = 1;
if (redundancy)
{
celt_to_silk = ec_dec_bit_logp(&dec, 1);
/* redundancy_bytes will be at least two, in the non-hybrid
case due to the ec_tell() check above */
redundancy_bytes = mode==MODE_HYBRID ?
(opus_int32)ec_dec_uint(&dec, 256)+2 :
len-((ec_tell(&dec)+7)>>3);
len -= redundancy_bytes;
/* This is a sanity check. It should never happen for a valid
packet, so the exact behaviour is not normative. */
if (len*8 < ec_tell(&dec))
{
len = 0;
redundancy_bytes = 0;
redundancy = 0;
}
/* Shrink decoder because of raw bits */
dec.storage -= redundancy_bytes;
}
}
if (mode != MODE_CELT_ONLY)
start_band = 17;
{
int endband=21;
switch(st->bandwidth)
{
case OPUS_BANDWIDTH_NARROWBAND:
endband = 13;
break;
case OPUS_BANDWIDTH_MEDIUMBAND:
case OPUS_BANDWIDTH_WIDEBAND:
endband = 17;
break;
case OPUS_BANDWIDTH_SUPERWIDEBAND:
endband = 19;
break;
case OPUS_BANDWIDTH_FULLBAND:
endband = 21;
break;
}
celt_decoder_ctl(celt_dec, CELT_SET_END_BAND(endband));
celt_decoder_ctl(celt_dec, CELT_SET_CHANNELS(st->stream_channels));
}
if (redundancy)
{
transition = 0;
pcm_transition_silk_size=ALLOC_NONE;
}
ALLOC(pcm_transition_silk, pcm_transition_silk_size, opus_val16);
if (transition && mode != MODE_CELT_ONLY)
{
pcm_transition = pcm_transition_silk;
opus_decode_frame(st, NULL, 0, pcm_transition, IMIN(F5, audiosize), 0);
}
/* Only allocation memory for redundancy if/when needed */
redundant_audio_size = redundancy ? F5*st->channels : ALLOC_NONE;
ALLOC(redundant_audio, redundant_audio_size, opus_val16);
/* 5 ms redundant frame for CELT->SILK*/
if (redundancy && celt_to_silk)
{
celt_decoder_ctl(celt_dec, CELT_SET_START_BAND(0));
celt_decode_with_ec(celt_dec, data+len, redundancy_bytes,
redundant_audio, F5, NULL, 0);
celt_decoder_ctl(celt_dec, OPUS_GET_FINAL_RANGE(&redundant_rng));
}
/* MUST be after PLC */
celt_decoder_ctl(celt_dec, CELT_SET_START_BAND(start_band));
if (mode != MODE_SILK_ONLY)
{
int celt_frame_size = IMIN(F20, frame_size);
/* Make sure to discard any previous CELT state */
if (mode != st->prev_mode && st->prev_mode > 0 && !st->prev_redundancy)
celt_decoder_ctl(celt_dec, OPUS_RESET_STATE);
/* Decode CELT */
celt_ret = celt_decode_with_ec(celt_dec, decode_fec ? NULL : data,
len, pcm, celt_frame_size, &dec, celt_accum);
} else {
unsigned char silence[2] = {0xFF, 0xFF};
if (!celt_accum)
{
for (i=0; i<frame_size*st->channels; i++)
pcm[i] = 0;
}
/* For hybrid -> SILK transitions, we let the CELT MDCT
do a fade-out by decoding a silence frame */
if (st->prev_mode == MODE_HYBRID && !(redundancy && celt_to_silk && st->prev_redundancy) )
{
celt_decoder_ctl(celt_dec, CELT_SET_START_BAND(0));
celt_decode_with_ec(celt_dec, silence, 2, pcm, F2_5, NULL, celt_accum);
}
}
if (mode != MODE_CELT_ONLY && !celt_accum)
{
#ifdef FIXED_POINT
for (i=0; i<frame_size*st->channels; i++)
pcm[i] = SAT16(ADD32(pcm[i], pcm_silk[i]));
#else
for (i=0; i<frame_size*st->channels; i++)
pcm[i] = pcm[i] + (opus_val16)((1.f/32768.f)*pcm_silk[i]);
#endif
}
{
const CELTMode *celt_mode;
celt_decoder_ctl(celt_dec, CELT_GET_MODE(&celt_mode));
window = celt_mode->window;
}
/* 5 ms redundant frame for SILK->CELT */
if (redundancy && !celt_to_silk)
{
celt_decoder_ctl(celt_dec, OPUS_RESET_STATE);
celt_decoder_ctl(celt_dec, CELT_SET_START_BAND(0));
celt_decode_with_ec(celt_dec, data+len, redundancy_bytes, redundant_audio, F5, NULL, 0);
celt_decoder_ctl(celt_dec, OPUS_GET_FINAL_RANGE(&redundant_rng));
smooth_fade(pcm+st->channels*(frame_size-F2_5), redundant_audio+st->channels*F2_5,
pcm+st->channels*(frame_size-F2_5), F2_5, st->channels, window, st->Fs);
}
if (redundancy && celt_to_silk)
{
for (c=0; c<st->channels; c++)
{
for (i=0; i<F2_5; i++)
pcm[st->channels*i+c] = redundant_audio[st->channels*i+c];
}
smooth_fade(redundant_audio+st->channels*F2_5, pcm+st->channels*F2_5,
pcm+st->channels*F2_5, F2_5, st->channels, window, st->Fs);
}
if (transition)
{
if (audiosize >= F5)
{
for (i=0; i<st->channels*F2_5; i++)
pcm[i] = pcm_transition[i];
smooth_fade(pcm_transition+st->channels*F2_5, pcm+st->channels*F2_5,
pcm+st->channels*F2_5, F2_5,
st->channels, window, st->Fs);
} else {
/* Not enough time to do a clean transition, but we do it anyway
This will not preserve amplitude perfectly and may introduce
a bit of temporal aliasing, but it shouldn't be too bad and
that's pretty much the best we can do. In any case, generating this
transition it pretty silly in the first place */
smooth_fade(pcm_transition, pcm,
pcm, F2_5,
st->channels, window, st->Fs);
}
}
if(st->decode_gain)
{
opus_val32 gain;
gain = celt_exp2(MULT16_16_P15(QCONST16(6.48814081e-4f, 25), st->decode_gain));
for (i=0; i<frame_size*st->channels; i++)
{
opus_val32 x;
x = MULT16_32_P16(pcm[i],gain);
pcm[i] = SATURATE(x, 32767);
}
}
if (len <= 1)
st->rangeFinal = 0;
else
st->rangeFinal = dec.rng ^ redundant_rng;
st->prev_mode = mode;
st->prev_redundancy = redundancy && !celt_to_silk;
if (celt_ret>=0)
{
if (OPUS_CHECK_ARRAY(pcm, audiosize*st->channels))
OPUS_PRINT_INT(audiosize);
}
RESTORE_STACK;
return celt_ret < 0 ? celt_ret : audiosize;
}
