PlanningProblem::ExtendResult PlanningProblem::RRTStep(float extension_len, float max_len)
{
ExtendResult result;
try {
Station rand_st;
float toss = uni_rand(0, 1);
if(toss < GOAL_PROB)
rand_st.set(goal.goal_point);
else
{
if(max_len < INFINITY) {
Vector2D rand_point = randomSampleFromEllipse(initialState.getPosition().to2D(),
goal.goal_point.getPosition().to2D(),
max_len);
Station temp;
temp.setPosition(Vector3D(rand_point, uni_rand(-M_PI, M_PI)));
rand_st.set(temp);
}
else {
Station temp = SampleStateUniform();
rand_st.set(temp);
}
}
if(!rand_st.isValid())
throw "can not sampled!";
SpatialVertex* near_ver = randomTree.getNearestVertex(rand_st);
// if(near_ver->state.isValid())
if(near_ver == NULL)
throw "can not find nearest!";
Station new_st = RRTExtend((near_ver->state), rand_st, extension_len);
if(!new_st.isValid())
{
result = eTrapped;
throw "can not extend tree!";
}
if(goal.minDistTo(new_st) < agent->radius() * 1)
result = eReached;
else
result = eAdvanced;
randomTree.appendNewStation(near_ver, new_st);
} catch (const char* msg)
{
// cerr << "Exception in RRTStep: " << msg << endl;
}
return result;
}
Trajectory PlanningProblem::GRRTsolve()
{
this->planningResult = false;
randomTree.clear();
float start_time = currentTimeMSec();
int tryExtensionCounter = 0;
randomTree.appendNewStation(NULL, initialState);
for(int step=0; step < MAX_RRT_STEP_TRY/5; step++)
{
Station target;
float toss = uni_rand(0, 1);
if(toss < GOAL_PROB)
target.setPosition(goal.goal_point.getPosition());
else {
Station tempSt = SampleStateUniform();
target.setPosition(tempSt.getPosition());
}
if( !target.isValid() )
continue;
// throw "can not sampled!";
SpatialVertex* near_ver = randomTree.getNearestVertex(target);
if(near_ver == NULL)
continue;
int greedyCounter = 0;
while(greedyCounter < 5){
tryExtensionCounter ++;
Station extended = RRTExtend(near_ver->state, target, agent->radius() * 2);
if(!extended.isValid())
break;
randomTree.appendNewStation(near_ver, extended);
if(Station::dubinDistance(extended, target) < agent->radius() ) {
if((target.getPosition() - goal.goal_point.getPosition()).lenght2D() < agent->radius() /2)
planningResult = true;
break;
}
// if(target != goal.goal_point) break;
greedyCounter ++;
}
cout << "Step = " << step << endl;
}
if(planningResult)
{
float finish_time = currentTimeMSec();
this->planningTime = finish_time - start_time;
// cout << "Greedy RRT Planning succeed in " << planningTime << "mili seconds" << endl;
return buildTrajectoryFromRandomTree();
}
return Trajectory();
}
EXPORT void speex_decorrelate(SpeexDecorrState *st, const spx_int16_t *in, spx_int16_t *out, int strength)
{
int ch;
float amount;
if (strength<0)
strength = 0;
if (strength>100)
strength = 100;
amount = .01*strength;
for (ch=0;ch<st->channels;ch++)
{
int i;
int N=2*st->frame_size;
float beta, beta2;
float *x;
float max_alpha = 0;
float *buff;
float *ring;
int ringID;
int order;
float alpha;
buff = st->buff+ch*2*st->frame_size;
ring = st->ring[ch];
ringID = st->ringID[ch];
order = st->order[ch];
alpha = st->alpha[ch];
for (i=0;i<st->frame_size;i++)
buff[i] = buff[i+st->frame_size];
for (i=0;i<st->frame_size;i++)
buff[i+st->frame_size] = in[i*st->channels+ch];
x = buff+st->frame_size;
beta = 1.-.3*amount*amount;
if (amount>1)
beta = 1-sqrt(.4*amount);
else
beta = 1-0.63246*amount;
if (beta<0)
beta = 0;
beta2 = beta;
for (i=0;i<st->frame_size;i++)
{
st->y[i] = alpha*(x[i-ALLPASS_ORDER+order]-beta*x[i-ALLPASS_ORDER+order-1])*st->vorbis_win[st->frame_size+i+order]
+ x[i-ALLPASS_ORDER]*st->vorbis_win[st->frame_size+i]
- alpha*(ring[ringID]
- beta*ring[ringID+1>=order?0:ringID+1]);
ring[ringID++]=st->y[i];
st->y[i] *= st->vorbis_win[st->frame_size+i];
if (ringID>=order)
ringID=0;
}
order = order+(irand(&st->seed)%3)-1;
if (order < 5)
order = 5;
if (order > 10)
order = 10;
/*order = 5+(irand(&st->seed)%6);*/
max_alpha = pow(.96+.04*(amount-1),order);
if (max_alpha > .98/(1.+beta2))
max_alpha = .98/(1.+beta2);
alpha = alpha + .4*uni_rand(&st->seed);
if (alpha > max_alpha)
alpha = max_alpha;
if (alpha < -max_alpha)
alpha = -max_alpha;
for (i=0;i<ALLPASS_ORDER;i++)
ring[i] = 0;
ringID = 0;
for (i=0;i<st->frame_size;i++)
{
float tmp = alpha*(x[i-ALLPASS_ORDER+order]-beta*x[i-ALLPASS_ORDER+order-1])*st->vorbis_win[i+order]
+ x[i-ALLPASS_ORDER]*st->vorbis_win[i]
- alpha*(ring[ringID]
- beta*ring[ringID+1>=order?0:ringID+1]);
ring[ringID++]=tmp;
tmp *= st->vorbis_win[i];
if (ringID>=order)
ringID=0;
st->y[i] += tmp;
}
#ifdef VORBIS_PSYCHO
float frame[N];
float scale = 1./N;
for (i=0;i<2*st->frame_size;i++)
frame[i] = buff[i];
//float coef = .5*0.78130;
float coef = M_PI*0.075063 * 0.93763 * amount * .8 * 0.707;
compute_curve(st->psy, buff, st->curve);
for (i=1;i<st->frame_size;i++)
{
float x1,x2;
float gain;
do {
x1 = uni_rand(&st->seed);
x2 = uni_rand(&st->seed);
} while (x1*x1+x2*x2 > 1.);
gain = coef*sqrt(.1+st->curve[i]);
frame[2*i-1] = gain*x1;
frame[2*i] = gain*x2;
}
frame[0] = coef*uni_rand(&st->seed)*sqrt(.1+st->curve[0]);
frame[2*st->frame_size-1] = coef*uni_rand(&st->seed)*sqrt(.1+st->curve[st->frame_size-1]);
spx_drft_backward(&st->lookup,frame);
for (i=0;i<2*st->frame_size;i++)
frame[i] *= st->vorbis_win[i];
#endif
for (i=0;i<st->frame_size;i++)
{
#ifdef VORBIS_PSYCHO
float tmp = st->y[i] + frame[i] + st->wola_mem[i];
st->wola_mem[i] = frame[i+st->frame_size];
#else
float tmp = st->y[i];
#endif
if (tmp>32767)
tmp = 32767;
if (tmp < -32767)
tmp = -32767;
out[i*st->channels+ch] = tmp;
}
st->ringID[ch] = ringID;
st->order[ch] = order;
st->alpha[ch] = alpha;
}
}
