void Controller::findMinimumAllowedVergence()
{
iKinChain cl(*chainEyeL), cr(*chainEyeR);
Vector zeros(cl.getDOF(),0.0);
cl.setAng(zeros); cr.setAng(zeros);
double minVer=startupMinVer;
double maxVer=lim(nJointsHead-1,1);
for (double ver=minVer; ver<maxVer; ver+=0.5*CTRL_DEG2RAD)
{
cl(cl.getDOF()-1).setAng(ver/2.0);
cr(cr.getDOF()-1).setAng(-ver/2.0);
Vector fp(4);
fp[3]=1.0; // impose homogeneous coordinates
if (CartesianHelper::computeFixationPointData(cl,cr,fp))
{
// if the component along eye's z-axis is positive
// then this means that the fixation point is ok,
// being in front of the robot
Vector fpe=SE3inv(cl.getH())*fp;
if (fpe[2]>0.0)
{
minVer=ver;
break;
}
}
}
yInfo("### computed minimum allowed vergence = %g [deg]",minVer*CTRL_RAD2DEG);
commData->get_minAllowedVergence()=minVer;
}
TEST(DynamicMemory, LimitedPoolAllocator)
{
uavcan::PoolAllocator<128, 32> pool32;
uavcan::LimitedPoolAllocator lim(pool32, 2);
EXPECT_EQ(2, lim.getNumBlocks());
const void* ptr1 = lim.allocate(1);
const void* ptr2 = lim.allocate(1);
const void* ptr3 = lim.allocate(1);
EXPECT_TRUE(ptr1);
EXPECT_TRUE(ptr2);
EXPECT_FALSE(ptr3);
lim.deallocate(ptr2);
const void* ptr4 = lim.allocate(1);
lim.deallocate(ptr1);
const void* ptr5 = lim.allocate(1);
const void* ptr6 = lim.allocate(1);
EXPECT_TRUE(ptr4);
EXPECT_TRUE(ptr5);
EXPECT_FALSE(ptr6);
}
void pref(char prefi[78],char exp[78])
{
struct stack st1;
struct stack st2;
struct stack st3;
int cn=0;
int i, apil=0;
st1.top=0;
st2.top=0;
st3.top=0;
lim(&st1);
exp[0]='\0';
for(i=strlen(prefi)-1;i>=0;i--)
{
if( oper(prefi[i]))
ins(&st2,prefi[i]);
else
{
ins(&st1,prefi[i]);
cn++;
}
if (cn==2)
{
vaciar(&st1,&st3);
ins(&st3,elim(&st2,st2.pila[st2.top]));
cn=0;
}
}
//mostrarmul(&st1,&st2,&st3);
copytexp(&st2,exp);
copytexp(&st1,exp);
copytexp(&st3,exp);
}
void EyePinvRefGen::minAllowedVergenceChanged()
{
LockGuard guard(mutex);
lim(2,0)=commData->minAllowedVergence;
I->setLim(lim);
orig_lim=lim;
}
void small_sinh_series(T x, T& result)
{
typedef typename boost::multiprecision::detail::canonical<unsigned, T>::type ui_type;
bool neg = eval_get_sign(x) < 0;
if(neg)
x.negate();
T p(x);
T mult(x);
eval_multiply(mult, x);
result = x;
ui_type k = 1;
T lim(x);
eval_ldexp(lim, lim, 1 - boost::multiprecision::detail::digits2<number<T, et_on> >::value);
do
{
eval_multiply(p, mult);
eval_divide(p, ++k);
eval_divide(p, ++k);
eval_add(result, p);
}while(p.compare(lim) >= 0);
if(neg)
result.negate();
}
t_vec3d lambert_alg(t_obj *obj, t_env *e)
{
t_vec3d form;
t_vec3d light;
double value;
double d;
light = a_moin_b(&obj->pos, &(e->pos));
ajust(&light);
d = sqrt((e->pos.x - obj->pos.x) * (e->pos.x - obj->pos.x) + \
(e->pos.y - obj->pos.y) * (e->pos.y - obj->pos.y) + (e->pos.z - obj->pos.z) * (e->pos.z - obj->pos.z));
d = lim(sqrt(1 / (d * (1.0 - obj->intens))), 0, 1);
value = lim(mult(&(e->effect.vec), &light), 0, 1);
if (e->effect.vec.x == 0 && e->effect.vec.y == 1 && e->effect.vec.z == 0)
value = 1;
form = mult_value(&obj->color, value);
form = mult_value(&form, obj->intens);
form = mult_value(&form, d);
return (form);
}
double phong_alg(t_obj *obj, t_env *e)
{
double form;
t_vec3d calc;
t_vec3d light;
double keep;
double keep2;
keep = mult(&(e->effect.vec), &(e->ray_dir));
calc.x = e->ray_dir.x - 2 * keep * e->effect.vec.x;
calc.y = e->ray_dir.y - 2 * keep * e->effect.vec.y;
calc.z = e->ray_dir.z - 2 * keep * e->effect.vec.z;
light = a_moin_b(&obj->pos, &(e->pos));
ajust(&light);
ajust(&calc);
keep2 = pow(lim(mult(&calc, &light), 0, 1), 40);
form = lim(keep2, 0, 1);
form *= pow(obj->intens, 0.5);
return (form);
}
void Controller::doSaccade(Vector &ang, Vector &vel)
{
if (ctrlInhibited)
return;
mutexCtrl.lock();
posHead->setRefSpeed(eyesJoints[0],vel[0]);
posHead->setRefSpeed(eyesJoints[1],vel[1]);
posHead->setRefSpeed(eyesJoints[2],vel[2]);
// enforce joints bounds
ang[0]=std::min(std::max(lim(eyesJoints[0],0),ang[0]),lim(eyesJoints[0],1));
ang[1]=std::min(std::max(lim(eyesJoints[1],0),ang[1]),lim(eyesJoints[1],1));
ang[2]=std::min(std::max(lim(eyesJoints[2],0),ang[2]),lim(eyesJoints[2],1));
posHead->positionMove(eyesJoints[0],CTRL_RAD2DEG*ang[0]);
posHead->positionMove(eyesJoints[1],CTRL_RAD2DEG*ang[1]);
posHead->positionMove(eyesJoints[2],CTRL_RAD2DEG*ang[2]);
saccadeStartTime=Time::now();
tiltDone=panDone=verDone=false;
commData->get_isSaccadeUnderway()=true;
unplugCtrlEyes=true;
notifyEvent("saccade-onset");
mutexCtrl.unlock();
}
void CCookie::fromString( const char* cookieStr )
{
CAnsiStrGen fullGen(cookieStr);
CAnsiStrGen highLevelLimitGen(kHighLevelLimit);
CAnsiStrGen lowLevelLimitGen(kLowLevelLimit);
CAnsiStrGen spacesGen(kSpaces);
CAnsiTokens highLevelTokens(tokenizeSequnce(fullGen, highLevelLimitGen));
//first part is for value
if (not highLevelTokens.getLimits().empty()) {
CAnsiTokens::CTokenLimit cookiePart(gTrimSequence( highLevelTokens.getLimits().front()
, spacesGen ));
m_sValue.assign(cookiePart.m_Begin, cookiePart.m_End);
}
for (unsigned int i = 1; i < highLevelTokens.getLimits().size(); ++i) {
CAnsiTokens::CTokenLimit lim(gTrimSequence( highLevelTokens.getLimits()[i]
, spacesGen ));
CAnsiPartStrGen lowLevelGen(lim.m_Begin, lim.m_End);
CAnsiTokens lowLevelTokens(tokenizeSequnce( lowLevelGen
, lowLevelLimitGen ));
const CAnsiTokens::CTokenLimit& firstPart(lowLevelTokens.getLimits().front());
if ( (kUsualEqualityPartCounter == lowLevelTokens.getLimits().size())
and
(strncmp(firstPart.m_Begin, kPathPart, firstPart.size()) == 0) ) {
//path={uri}
const CAnsiTokens::CTokenLimit& secondPart(lowLevelTokens.getLimits().back());
m_sAppliedPath.assign(secondPart.m_Begin, secondPart.size());
}
else if ( (kUsualEqualityPartCounter == lowLevelTokens.getLimits().size())
and
(strncmp(firstPart.m_Begin, kDomainPart, firstPart.size()) == 0) ) {
const CAnsiTokens::CTokenLimit& secondPart(lowLevelTokens.getLimits().back());
m_sDomain.assign(secondPart.m_Begin, secondPart.size());
}
else if ( (kUsualEqualityPartCounter == lowLevelTokens.getLimits().size())
and
(strncmp(firstPart.m_Begin, kExpiresPart, firstPart.size()) == 0) ) {
//expires={Date}
const CAnsiTokens::CTokenLimit& secondPart(lowLevelTokens.getLimits().back());
m_sValidDate.assign(secondPart.m_Begin, secondPart.size());
}
}
}
int test_parallel(int num_threads) {
// test puts with no decrements
for ( int i = 0; i < L; ++i ) {
tbb::graph g;
tbb::limiter_node< T > lim(g, i);
parallel_receiver<T> r;
tbb::atomic<int> accept_count;
accept_count = 0;
lim.register_successor( r );
// test puts with no decrements
NativeParallelFor( num_threads, put_body<T>(lim, accept_count) );
g.wait_for_all();
int c = accept_count;
ASSERT( c == i, NULL );
}
// test puts with decrements
for ( int i = 1; i < L; ++i ) {
tbb::graph g;
tbb::limiter_node< T > lim(g, i);
parallel_receiver<T> r;
empty_sender< tbb::continue_msg > s;
tbb::atomic<int> accept_count;
accept_count = 0;
lim.register_successor( r );
lim.decrement.register_predecessor( s );
// test puts with no decrements
NativeParallelFor( num_threads, put_dec_body<T>(lim, accept_count) );
int c = accept_count;
ASSERT( c == N*num_threads, NULL );
ASSERT( r.my_count == N*num_threads, NULL );
}
return 0;
}
void trait_obj(char *here, t_obj *obj)
{
char *here2;
char *temp;
temp = chaine(here);
here[0] = 'W';
if (temp == NULL)
return ;
if ((here2 = ft_strstr(temp, "pos(")) && here2 != NULL)
{
here2 = chaine2(here2);
obj->pos = *vec3d(here2);
}
if ((here2 = ft_strstr(temp, "color(")) && here2 != NULL)
{
here2 = chaine2(here2);
obj->color = *vec3d(here2);
obj->color.x = lim(obj->color.x / 256, 0, 1);
obj->color.y = lim(obj->color.y / 256, 0, 1);
obj->color.z = lim(obj->color.z / 256, 0, 1);
}
cut_obj(temp, obj);
}
void ParticleDistribution::init(long ncoord,const Vector_3 *Coordarr,long *inda){
indarr= new long[ncoord];
if(!indarr)fatal_error("ParticleDistribution: MAE!\n");
long i;
if(!inda){
for(i=0;i<ncoord;i++)indarr[i]=i;
}
else{
for(i=0;i<ncoord;i++)indarr[i]=inda[i];
}
Vector_3 pmin, pmax;
GetIScope(Coordarr,indarr,ncoord,&pmin,&pmax);
SpaceRegion lim(pmin,pmax);
SetLimitRegion(&lim);
Coords=Coordarr;
nc=ncoord;
}
double give_shadow(t_env *e)
{
t_obj *obj;
double my_shadow;
double give;
obj = e->obj;
my_shadow = 1;
while (obj)
{
if (obj->type == 8)
{
give = inter_shadow(e, &obj->pos);
if (give == 1)
my_shadow -= (obj->intens + e->nl) / 2;
}
obj = obj->next;
}
return (lim(my_shadow, 0, 1));
}
void CFilterATMApp::FilterContinue(void)
{
// update progress counter
DisplayTileProgress(0);
// do the processing for this tile
libbase::matrix<double> in, out;
GetPixelMatrix(in);
atmfilter<double>::process(in, out);
if(m_sData->bKeepNoise)
{
out *= -1;
out += in;
out += 0.5;
libimage::limiter<double> lim(0,1);
lim.process(out);
}
SetPixelMatrix(out);
// select the next rectangle based on the given tile suggestions
CPSPlugIn::FilterContinue();
}
void Controller::doSaccade(const Vector &ang, const Vector &vel)
{
LockGuard guard(mutexCtrl);
if (ctrlInhibited)
return;
setJointsCtrlMode();
// enforce joints bounds
Vector ang_(3);
ang_[0]=CTRL_RAD2DEG*sat(ang[0],lim(eyesJoints[0],0),lim(eyesJoints[0],1));
ang_[1]=CTRL_RAD2DEG*sat(ang[1],lim(eyesJoints[1],0),lim(eyesJoints[1],1));
ang_[2]=CTRL_RAD2DEG*sat(ang[2],lim(eyesJoints[2],0),lim(eyesJoints[2],1));
posHead->setRefSpeeds(eyesJoints.size(),eyesJoints.getFirst(),vel.data());
posHead->positionMove(eyesJoints.size(),eyesJoints.getFirst(),ang_.data());
if (commData->debugInfoEnabled && (port_debug.getOutputCount()>0))
{
Bottle info;
for (size_t i=0; i<ang_.length(); i++)
{
ostringstream ss;
ss<<"pos_"<<eyesJoints[i];
info.addString(ss.str().c_str());
info.addDouble(ang_[i]);
}
port_debug.prepare()=info;
txInfo_debug.update(q_stamp);
port_debug.setEnvelope(txInfo_debug);
port_debug.writeStrict();
}
saccadeStartTime=Time::now();
commData->saccadeUnderway=true;
unplugCtrlEyes=true;
notifyEvent("saccade-onset");
}
void EyePinvRefGen::run()
{
if (genOn)
{
LockGuard guard(mutex);
double timeStamp;
// read encoders
getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData,&timeStamp);
updateTorsoBlockedJoints(chainNeck,fbTorso);
updateTorsoBlockedJoints(chainEyeL,fbTorso);
updateTorsoBlockedJoints(chainEyeR,fbTorso);
// get current target
Vector xd=commData->port_xd->get_xd();
// update neck chain
chainNeck->setAng(nJointsTorso+0,fbHead[0]);
chainNeck->setAng(nJointsTorso+1,fbHead[1]);
chainNeck->setAng(nJointsTorso+2,fbHead[2]);
// ask for saccades (if possible)
if (commData->saccadesOn && (saccadesRxTargets!=commData->port_xd->get_rx()) &&
!commData->saccadeUnderway && (Time::now()-saccadesClock>commData->saccadesInhibitionPeriod))
{
Vector fph=xd; fph.push_back(1.0);
fph=SE3inv(chainNeck->getH())*fph; fph[3]=0.0;
double rot=CTRL_RAD2DEG*acos(fph[2]/norm(fph)); fph[3]=1.0;
// estimate geometrically the target tilt and pan of the eyes
Vector ang(3,0.0);
ang[0]=-atan2(fph[1],fabs(fph[2]));
ang[1]=atan2(fph[0],fph[2]);
// enforce joints bounds
ang[0]=sat(ang[0],lim(0,0),lim(0,1));
ang[1]=sat(ang[1],lim(1,0),lim(1,1));
// favor the smooth-pursuit in case saccades are small
if (rot>commData->saccadesActivationAngle)
{
// init vergence
ang[2]=fbHead[5];
// get rid of eyes tilt
Vector axis(4);
axis[0]=1.0; axis[1]=0.0; axis[2]=0.0; axis[3]=-ang[0];
fph=axis2dcm(axis)*fph;
// go on iff the point is in front of us
if (fph[2]>0.0)
{
double L,R;
// estimate geometrically the target vergence Vg=L-R
if (fph[0]>=eyesHalfBaseline)
{
L=M_PI/2.0-atan2(fph[2],fph[0]+eyesHalfBaseline);
R=M_PI/2.0-atan2(fph[2],fph[0]-eyesHalfBaseline);
}
else if (fph[0]>-eyesHalfBaseline)
{
L=M_PI/2.0-atan2(fph[2],fph[0]+eyesHalfBaseline);
R=-(M_PI/2.0-atan2(fph[2],eyesHalfBaseline-fph[0]));
}
else
{
L=-(M_PI/2.0-atan2(fph[2],-fph[0]-eyesHalfBaseline));
R=-(M_PI/2.0-atan2(fph[2],eyesHalfBaseline-fph[0]));
}
ang[2]=L-R;
}
// enforce joints bounds
ang[2]=sat(ang[2],lim(2,0),lim(2,1));
commData->set_qd(3,ang[0]);
commData->set_qd(4,ang[1]);
commData->set_qd(5,ang[2]);
Vector vel(3,SACCADES_VEL);
ctrl->doSaccade(ang,vel);
saccadesClock=Time::now();
}
}
// update eyes chains for convergence purpose
updateNeckBlockedJoints(chainEyeL,fbHead); updateNeckBlockedJoints(chainEyeR,fbHead);
chainEyeL->setAng(nJointsTorso+3,qd[0]); chainEyeR->setAng(nJointsTorso+3,qd[0]);
chainEyeL->setAng(nJointsTorso+4,qd[1]+qd[2]/2.0); chainEyeR->setAng(nJointsTorso+4,qd[1]-qd[2]/2.0);
// converge on target
if (CartesianHelper::computeFixationPointData(*chainEyeL,*chainEyeR,fp,eyesJ))
{
Vector v=EYEPINVREFGEN_GAIN*(pinv(eyesJ)*(xd-fp));
// update eyes chains in actual configuration for velocity compensation
chainEyeL->setAng(nJointsTorso+3,fbHead[3]); chainEyeR->setAng(nJointsTorso+3,fbHead[3]);
chainEyeL->setAng(nJointsTorso+4,fbHead[4]+fbHead[5]/2.0); chainEyeR->setAng(nJointsTorso+4,fbHead[4]-fbHead[5]/2.0);
// compensate neck rotation at eyes level
if ((commData->eyesBoundVer>=0.0) || !CartesianHelper::computeFixationPointData(*chainEyeL,*chainEyeR,fp,eyesJ))
commData->set_counterv(zeros(qd.length()));
else
commData->set_counterv(getEyesCounterVelocity(eyesJ,fp));
// reset eyes controller and integral upon saccades transition on=>off
if (saccadeUnderWayOld && !commData->saccadeUnderway)
{
ctrl->resetCtrlEyes();
qd[0]=fbHead[3];
qd[1]=fbHead[4];
qd[2]=fbHead[5];
I->reset(qd);
}
// update reference
qd=I->integrate(v+commData->get_counterv());
}
else
commData->set_counterv(zeros(qd.length()));
// set a new target position
commData->set_xd(xd);
commData->set_x(fp,timeStamp);
commData->set_fpFrame(chainNeck->getH());
if (!commData->saccadeUnderway)
{
commData->set_qd(3,qd[0]);
commData->set_qd(4,qd[1]);
commData->set_qd(5,qd[2]);
}
// latch the saccades status
saccadeUnderWayOld=commData->saccadeUnderway;
saccadesRxTargets=commData->port_xd->get_rx();
}
}
void route_edges2(Layouter &state,plugin& pg, double scale, int iter, double temp, int debug){
VR nodes;
double d=state.avgsize/5;
int n=state.nw.nodes.size();
for (int i=0;i<n;i++){
nodes.push_back(state.nw.nodes[i].rect());
nodes.back().extend(d);
//debugrect(nodes.back(),0,0,255);
}
for (int i=0,m=state.nw.edges.size();i<m;i++){
//if (i!=iter) continue;
Edge &e=state.nw.edges[i];
e.splinehandles.clear();
e.splinepoints.clear();
int n1=e.from;
int n2=e.to;
Point vec,p1,p2;
double dir;
switch(e.type){
case substrate:
dir=lim(state.nw.nodes[n1].dir+PI/2);
vec=Point(dir);
swap(n1,n2);
p1=state.nw.nodes[n1];
vec=state.nw.nodes[n2].rect().border_vec(vec);
p2=state.nw.nodes[n2]+vec;
break;
case product:
dir=lim(state.nw.nodes[n1].dir-PI/2);
vec=Point(dir);
vec=state.nw.nodes[n1].rect().border_vec(vec);
p1=state.nw.nodes[n1]+vec;
p2=state.nw.nodes[n2];
break;
case activator:
case inhibitor:
case catalyst:
swap(n1,n2);
default:
p1=state.nw.nodes[n1];
p2=state.nw.nodes[n2];
}
vector<Segment> vs;
vs.push_back(Segment(ParamEdge(p1,p2),true,true));
split_route(vs,nodes,0,n1,n2);
Point vec1=state.nw.nodes[n1].rect().border_vec(vs.front().edge.to()-vs.front().edge.from());
Point vec2=state.nw.nodes[n2].rect().border_vec(vs.back().edge.from()-vs.back().edge.to());
switch(e.type){
case activator:
case inhibitor:
case catalyst:
dir=lim(state.nw.nodes[n2].dir);
if (scalar(Point(dir),vs.back().edge.from()-vs.back().edge.to())<0) dir=lim(dir+PI);
vec=Point(dir);
vec=state.nw.nodes[n2].rect().border_vec(vec);
p2=state.nw.nodes[n2]+vec;
case substrate:
e.splinehandles.push_back(vec1+d);
e.splinehandles.push_back(vec+d);
break;
case product:
e.splinehandles.push_back(vec+d);
e.splinehandles.push_back(vec2+d);
break;
default:
e.splinehandles.push_back(vec1+d);
e.splinehandles.push_back(vec2+d);
}
for (int j=1,s=vs.size();j<s;j++){
Point before=vs[j-1].edge.from();
Point cur=vs[j-1].edge.to();
//if (cur!=vs[j].edge.from()) throw "ups: edge segments not continious";
Point after=vs[j].edge.to();
Point dv=unit(before-cur)*d+unit(after-cur)*d;
debugline(cur,cur+dv,0,255,0,true);
e.splinehandles.insert(--e.splinehandles.end(),unit(to_left(dv,PI/2))*sign(scalar(to_left(dv,PI/2),before-cur))*d);
e.splinepoints.push_back(cur+dv);
}
}
}
int main()
{
int N = 31;
for(int l1 = 0; l1 <= N; l1++)
for(int l2 = 0; l2 <= N; l2++)
for(int i = 0; i <= l1*l2; i++) {
int &at = D[l1][l2][i];
if(i == 0)// || l1 == 0 || l2 == 0)
at = 1;
else if(l1 == 0 || l2 == 0)
at = 0;
else {
at = D[l1-1][l2][i];
if(l1 <= i)
at += D[l1][l2-1][i-l1];
at %= MOD;
}
}
memset(A, 0, sizeof A);
A[0][0][0] = A[1][0][0] = 1;
for(int n = 2; n <= N; n++) {
fprintf(stderr, "Resolvendo n = %d... ", n);
for(int k = 0; k < n; k++) {
int l1 = k;
int l2 = n-k-1;
for(int q1 = 0; q1 <= lim(l1); q1++) {
for(int q2 = 0; q2 <= lim(l2); q2++) {
int q = q1 + q2 + n-1;
memset(S, 0, sizeof S);
for(int i1 = 0; i1 <= lim(l1); i1++)
for(int i2 = 0; i2 <= lim(l2); i2++)
S[i1+i2] = (((ll)S[i1+i2]+((ll)A[l1][q1][i1]*A[l2][q2][i2]))%MOD);
for(int s = 0; s <= lim(l1) + lim(l2); s++) {
for(int ic = 0; ic <= l1*l2; ic++) {
int i = s+ic+k;
int &at = A[n][q][i];
at = (((ll)at + (ll)S[s] * D[l1][l2][ic])%MOD);
}
}
}
}
}
fprintf(stderr, "done!\n");
}
int casos;
scanf(" %d", &casos);
while(casos--) {
int n, x;
scanf(" %d %d", &n, &x);
int r = 0;
for(int q = 0; q <= lim(n); q++) {
for(int i = 0; i+n-1 <= q*x && i <= lim(n); i++) {
r += A[n][q][i];
r %= MOD;
}
}
printf("%d\n", r);
}
return 0;
}
int main()
{
int N = 31; //limite
memset(D, 0, sizeof D);
memset(A, 0, sizeof A);
for(int l1 = 0; l1 <= N; l1++)
for(int l2 = 0; l1+l2 <= N; l2++)
for(int i = 0; i <= l1*l2; i++){
int &at = D[l1][l2][i];
if(i == 0)
at = 1;
else if(l1 == 0 || l2 == 0)
at = 0;
else{
at = D[l1-1][l2][i];
if(i >= l1){
at += D[l1][l2-1][i - l1];
at %= MOD;
}
}
}
A[0][0][0] = A[1][0][0] = 1;
for(int n = 2; n <= N; n++){
printf("Processando n = %d...", n);
fflush(stdout);
for(int k = 0; k < n; k++){
int l1 = k;
int l2 = n - l1 - 1;
for(int q1 = 0; q1 <= lim(l1); q1++){
for(int q2 = 0; q2 <= lim(l2); q2++){
int q = q1 + q2 + (n-1);
memset(S, 0, sizeof S);
for(int i1 = 0; i1 <= lim(l1); i1++){
for(int i2 = 0; i2 <= lim(l2); i2++){
S[i1+i2] += ((ll)A[l1][q1][i1] * A[l2][q2][i2])%MOD;
S[i1+i2] %= MOD;
}
}
for(int ic = 0; ic <= l1*l2; ic++){
for(int s = 0; s <= lim(l1)+lim(l2); s++){
int i = s+ic+k;
int &resp = A[n][q][i];
resp += ((ll)S[s]*D[l1][l2][ic])%MOD;
resp %= MOD;
}
}
}
}
}
printf("done!\n");
}
char base[20] = "-sorting.";
for(int i = 0; i <= 9; i++){
char ent[20], sai[20];
sprintf(ent, "%d%sin", i, base);
sprintf(sai, "%d%sout", i, base);
FILE *fin = fopen(ent, "r");
FILE *fout = fopen(sai, "w");
int n, x;
fscanf(fin, " %d %d", &n, &x);
ll r = 0;
for(int q = 0; q <= lim(n); q++)
for(int i = 0; i+n-1 <= x*q && i <= lim(n); i++){
r += A[n][q][i];
r %= MOD;
}
fprintf(fout, "%lld\n", r);
fclose(fin);
fclose(fout);
}
return 0;
}
void Controller::minAllowedVergenceChanged()
{
lim(nJointsHead-1,0)=commData->get_minAllowedVergence();
IntState->setLim(lim);
}
Controller::Controller(PolyDriver *_drvTorso, PolyDriver *_drvHead, exchangeData *_commData,
const bool _neckPosCtrlOn, const double _neckTime, const double _eyesTime,
const double _minAbsVel, const unsigned int _period) :
RateThread(_period), drvTorso(_drvTorso), drvHead(_drvHead),
commData(_commData), neckPosCtrlOn(_neckPosCtrlOn), neckTime(_neckTime),
eyesTime(_eyesTime), minAbsVel(_minAbsVel), period(_period),
Ts(_period/1000.0), printAccTime(0.0)
{
// Instantiate objects
neck=new iCubHeadCenter(commData->head_version>1.0?"right_v2":"right");
eyeL=new iCubEye(commData->head_version>1.0?"left_v2":"left");
eyeR=new iCubEye(commData->head_version>1.0?"right_v2":"right");
// release links
neck->releaseLink(0); eyeL->releaseLink(0); eyeR->releaseLink(0);
neck->releaseLink(1); eyeL->releaseLink(1); eyeR->releaseLink(1);
neck->releaseLink(2); eyeL->releaseLink(2); eyeR->releaseLink(2);
// Get the chain objects
chainNeck=neck->asChain();
chainEyeL=eyeL->asChain();
chainEyeR=eyeR->asChain();
// add aligning matrices read from configuration file
getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain());
// overwrite aligning matrices iff specified through tweak values
if (commData->tweakOverwrite)
{
getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
}
// read number of joints
if (drvTorso!=NULL)
{
IEncoders *encTorso; drvTorso->view(encTorso);
encTorso->getAxes(&nJointsTorso);
}
else
nJointsTorso=3;
IEncoders *encHead; drvHead->view(encHead);
encHead->getAxes(&nJointsHead);
drvHead->view(modHead);
drvHead->view(posHead);
drvHead->view(velHead);
// if requested check if position control is available
if (neckPosCtrlOn)
{
neckPosCtrlOn=drvHead->view(posNeck);
yInfo("### neck control - requested POSITION mode: IPositionDirect [%s] => %s mode selected",
neckPosCtrlOn?"available":"not available",neckPosCtrlOn?"POSITION":"VELOCITY");
}
else
yInfo("### neck control - requested VELOCITY mode => VELOCITY mode selected");
// joints bounds alignment
lim=alignJointsBounds(chainNeck,drvTorso,drvHead,commData->eyeTiltMin,commData->eyeTiltMax);
// read starting position
fbTorso.resize(nJointsTorso,0.0);
fbHead.resize(nJointsHead,0.0);
// exclude acceleration constraints by fixing
// thresholds at high values
Vector a_robHead(nJointsHead,1e9);
velHead->setRefAccelerations(a_robHead.data());
copyJointsBounds(chainNeck,chainEyeL);
copyJointsBounds(chainEyeL,chainEyeR);
// find minimum allowed vergence
startupMinVer=lim(nJointsHead-1,0);
findMinimumAllowedVergence();
// reinforce vergence min bound
lim(nJointsHead-1,0)=commData->get_minAllowedVergence();
getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);
fbNeck=fbHead.subVector(0,2);
fbEyes=fbHead.subVector(3,5);
qdNeck.resize(3,0.0); qdEyes.resize(3,0.0);
vNeck.resize(3,0.0); vEyes.resize(3,0.0);
// Set the task execution time
setTeyes(eyesTime);
setTneck(neckTime);
mjCtrlNeck=new minJerkVelCtrlForIdealPlant(Ts,fbNeck.length());
mjCtrlEyes=new minJerkVelCtrlForIdealPlant(Ts,fbEyes.length());
IntState=new Integrator(Ts,fbHead,lim);
IntPlan=new Integrator(Ts,fbNeck,lim.submatrix(0,2,0,1));
v.resize(nJointsHead,0.0);
neckJoints.resize(3);
eyesJoints.resize(3);
neckJoints[0]=0;
neckJoints[1]=1;
neckJoints[2]=2;
eyesJoints[0]=3;
eyesJoints[1]=4;
eyesJoints[2]=5;
qd=fbHead;
q0deg=CTRL_RAD2DEG*qd;
qddeg=q0deg;
vdeg =CTRL_RAD2DEG*v;
port_xd=NULL;
ctrlActiveRisingEdgeTime=0.0;
saccadeStartTime=0.0;
unplugCtrlEyes=false;
ctrlInhibited=false;
}
void limit_for_vec(t_vec3d *vec, double a, double b)
{
vec->x = lim(vec->x, a, b);
vec->y = lim(vec->y, a, b);
vec->z = lim(vec->z, a, b);
}
int main(int argc, char *argv[]){
printf("This program will check for min and max values for different primitives (1/0 = signed/unsigned), (1/0 = ) and a character(c = char, s = short, d = int, l = long int, z = long long int)\n");
if (argc < 4) {
printf("\nGive two integer arguments and a character\n");
return -1;
}
int s, max;
char t;
sscanf(argv[1], "%d", &s);
sscanf(argv[2], "%d", &max);
sscanf(argv[3], "%c", &t);
if (s == 1){
printf("\n1 = signed, 0 = unsigned\nsign? = %d\n\n1 = maximum, 0 = minimum\nMax? = %d\n\n'c' = char, 's' = short int, 'd' = int, 'l' = long int, 'z' = long long int\ntype = %c\n\nlim is: \n%lld\n", s , max, t, lim(s,max,t)); //print results signed
}else{
printf("\n1 = signed, 0 = unsigned\nsign? = %d\n\n1 = maximum, 0 = minimum\nMax? = %d\n\n'c' = char, 's' = short int, 'd' = int, 'l' = long int, 'z' = long long int\ntype = %c\n\nlim is: \n%lld\n", s , max, t, lim(s,max,t)); //print results signed
}
return 0;
}
// Отладочная ведомость отступлений
std::wstring OvrHTML(const VFileName & fileName,
const VFileName & reportFile)
{
VDataSet dataSet;
if (!dataSet.OpenFile(fileName))
{
return L"Не доступен локальный файл " + fileName.GetPath();
}
/// Отступления версии 3
if (dataSet.Load(PRM_OVRS3))
{
const VOvr3List & overs = dataSet.GetOvrs3();
std::ofstream report(reportFile.GetPath().c_str());
if (report.bad())
{
return L"Не удалось открыть файл отчета";
}
// фиксированное число знаков после запятой
report << std::fixed;
report << std::setprecision(0);
std::string title = "Отступления (v.3)";
report << VHtml::GetHeader(title);
report << "<h3>" << dataSet.GetFileName().GetPath() << "<br>" << title << "</h3>" << std::endl;
report << "<p><strong>Дата поездки:</strong> " << dataSet.GetRailDate().ToStringLocale() << "<br>" << std::endl;
report << "<strong>Границы файла:</strong> " << dataSet.GetSection().start << " - ";
report << dataSet.GetSection().end << "</p>" << std::endl << std::endl;
report << "<table><tr><th>к-та</th><th>тип</th><th>величина</th>" << std::endl;
report << "<th>длина/№ элем</th><th>степень</th><!--<th>баллы</th>-->" << std::endl;
report << "<th>скор.прк</th><th>скор.огр</th><th>примечания</th>" << std::endl;
report << "</thead><tbody>" << std::endl;
for (size_t i = 0; i < overs.GetCount(); ++i)
{
if (i > 0 && overs.GetItem(i).km != overs.GetItem(i-1).km)
{
report << "</tbody><tbody>" << std::endl;
}
report << "<tr><td>" << VWayCoord(overs.GetItem(i).km, overs.GetItem(i).m) << "</td>" << std::endl;
report << "<td>" << wstring_to_string(overs.GetItem(i).type) << "</td>" << std::endl;
report << "<td>" << overs.GetItem(i).value << "</td>" << std::endl;
report << "<td>" << overs.GetItem(i).length;
if (std::wstring(overs.GetItem(i).type) == WIDEN ||
std::wstring(overs.GetItem(i).type) == NARROW ||
std::wstring(overs.GetItem(i).type) == LEVEL)
{
report << " (" << overs.GetItem(i).amount << "";
}
report << "</td>" << std::endl;
report << "<td>" << overs.GetItem(i).degree << "</td>" << std::endl;
VSpeeds prk(overs.GetItem(i).spd1, overs.GetItem(i).spd2, overs.GetItem(i).spd3);
report << "<td>" << prk << "</td>" << std::endl;
VSpeeds lim(overs.GetItem(i).rst1, overs.GetItem(i).rst2, overs.GetItem(i).rst3);
if (lim < prk) report << "<td>" << lim << "</td>" << std::endl;
else report << "<td>" << VSpeeds() << "</td>" << std::endl;
report << "<td>" << wstring_to_string(overs.GetItem(i).comment) << "</td></tr>" << std::endl;
}
report << std::endl << "</tbody></table>" << std::endl;
report << VHtml::GetFooter();
report.close();
return L"";
}
/// Отступления версии 3
else if (dataSet.Load(PRM_OVRS2))
{
const VOvr2List & overs = dataSet.GetOvrs2();
std::ofstream report(reportFile.GetPath().c_str());
if (report.bad())
{
return L"Не удалось открыть файл отчета";
}
// фиксированное число знаков после запятой
report << std::fixed;
report << std::setprecision(0);
std::string title = "Отступления (v.2)";
report << VHtml::GetHeader(title);
report << "<h3>" << dataSet.GetFileName().GetPath() << "<br>" << title << "</h3>" << std::endl;
report << "<p><strong>Дата поездки:</strong> " << dataSet.GetRailDate().ToStringLocale() << "<br>" << std::endl;
report << "<strong>Границы файла:</strong> " << dataSet.GetSection().start << " - ";
report << dataSet.GetSection().end << "</p>" << std::endl << std::endl;
report << "<table><thead><tr>" << std::endl;
report << "<th>к-та</th><th>тип</th><th>величина</th>" << std::endl;
report << "<th>длина/№ элем</th><th>степень</th><!--<th>баллы</th>-->" << std::endl;
report << "<th>скор.прк</th><th>скор.огр</th><th>примечания</th>" << std::endl;
report << "</thead><tbody>" << std::endl;
for (size_t i = 0; i < overs.GetCount(); ++i)
{
if (i > 0 && overs.GetItem(i).wayCoord.km != overs.GetItem(i-1).wayCoord.km)
{
report << "</tbody><tbody>" << std::endl;
}
if (GetOverstepName(overs.GetItem(i).type) == "" ||
overs.GetItem(i).degree < 2 || overs.GetItem(i).degree > 4)
{
continue;
}
report << "<tr><td>" << overs.GetItem(i).wayCoord << "</td>" << std::endl;
report << "<td>" << GetOverstepName(overs.GetItem(i).type) << "</td>" << std::endl;
report << "<td>" << overs.GetItem(i).value << "</td>" << std::endl;
report << "<td>" << overs.GetItem(i).len;
if (overs.GetItem(i).type == ovrWidthPl || overs.GetItem(i).type == ovrWidthMn ||
overs.GetItem(i).type == ovrLevel)
{
report << " (" << overs.GetItem(i).numElem << "";
}
report << "</td>" << std::endl;
report << "<td>" << overs.GetItem(i).degree << "</td>" << std::endl;
// report << "<td>" << overs.GetItem(i).GetPenalty() << "</td>" << std::endl;
report << "<td>" << overs.GetItem(i).setSpd << "</td>" << std::endl;
report << "<td>";
if (overs.GetItem(i).IsLimit())
{
report << overs.GetItem(i).limSpd;
}
report << "</td>" << std::endl;
report << "<td>";
if (overs.GetItem(i).code.C) report << "С ";
if (overs.GetItem(i).code.PP) report << "РР ";
if (overs.GetItem(i).code.ThreePr) report << "3Пр ";
if (overs.GetItem(i).code.PrjRich) report << "пржР ";
if (overs.GetItem(i).code.PrjP) report << "пржП ";
if (overs.GetItem(i).code.PrjPitR) report << "пржПр.п ";
if (overs.GetItem(i).code.PrjPitL) report << "пржПр.л ";
if (overs.GetItem(i).code.PrjRP) report << "пржР+П ";
if (overs.GetItem(i).code.PrjRPitR) report << "пржР+Пр.л ";
if (overs.GetItem(i).code.PrjRPitL) report << "пржР+Пр.п ";
if (overs.GetItem(i).code.Bridge) report << "мост ";
if (overs.GetItem(i).code.Tonnel) report << "тоннель ";
if (overs.GetItem(i).code.Switch) report << "стрелка ";
if (overs.GetItem(i).type == ovrWidthPl || overs.GetItem(i).type == ovrWidthMn)
{
report << "НШК=" << overs.GetItem(i).norma;
}
report << "</td></tr>" << std::endl;
}
report << std::endl << "</tbody></table>" << std::endl;
report << VHtml::GetFooter();
report.close();
return L"";
}
else
{
return L"В файле отсутствуют отступления";
}
}
EyePinvRefGen::EyePinvRefGen(PolyDriver *_drvTorso, PolyDriver *_drvHead,
ExchangeData *_commData, Controller *_ctrl,
const Vector &_counterRotGain, const unsigned int _period) :
RateThread(_period), drvTorso(_drvTorso), drvHead(_drvHead),
commData(_commData), ctrl(_ctrl), period(_period),
Ts(_period/1000.0), counterRotGain(_counterRotGain)
{
// Instantiate objects
neck=new iCubHeadCenter(commData->head_version>1.0?"right_v2":"right");
eyeL=new iCubEye(commData->head_version>1.0?"left_v2":"left");
eyeR=new iCubEye(commData->head_version>1.0?"right_v2":"right");
imu=new iCubInertialSensor(commData->head_version>1.0?"v2":"v1");
// remove constraints on the links: logging purpose
imu->setAllConstraints(false);
// block neck dofs
eyeL->blockLink(3,0.0); eyeR->blockLink(3,0.0);
eyeL->blockLink(4,0.0); eyeR->blockLink(4,0.0);
eyeL->blockLink(5,0.0); eyeR->blockLink(5,0.0);
// Get the chain objects
chainNeck=neck->asChain();
chainEyeL=eyeL->asChain();
chainEyeR=eyeR->asChain();
// add aligning matrices read from configuration file
getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain());
// overwrite aligning matrices iff specified through tweak values
if (commData->tweakOverwrite)
{
getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
}
if (commData->tweakOverwrite)
{
getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
}
// get the length of the half of the eyes baseline
eyesHalfBaseline=0.5*norm(eyeL->EndEffPose().subVector(0,2)-eyeR->EndEffPose().subVector(0,2));
// read number of joints
if (drvTorso!=NULL)
{
IEncoders *encTorso; drvTorso->view(encTorso);
encTorso->getAxes(&nJointsTorso);
}
else
nJointsTorso=3;
IEncoders *encHead; drvHead->view(encHead);
encHead->getAxes(&nJointsHead);
// joints bounds alignment
lim=alignJointsBounds(chainNeck,drvTorso,drvHead,commData->eyeTiltLim);
copyJointsBounds(chainNeck,chainEyeL);
copyJointsBounds(chainEyeL,chainEyeR);
// just eye part is required
lim=lim.submatrix(3,5,0,1);
// reinforce vergence min bound
lim(2,0)=commData->minAllowedVergence;
// read starting position
fbTorso.resize(nJointsTorso,0.0);
fbHead.resize(nJointsHead,0.0);
getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);
// save original eyes tilt and pan bounds
orig_eye_tilt_min=(*chainEyeL)[nJointsTorso+3].getMin();
orig_eye_tilt_max=(*chainEyeL)[nJointsTorso+3].getMax();
orig_eye_pan_min=(*chainEyeL)[nJointsTorso+4].getMin();
orig_eye_pan_max=(*chainEyeL)[nJointsTorso+4].getMax();
orig_lim=lim;
// Instantiate integrator
qd.resize(3);
qd[0]=fbHead[3];
qd[1]=fbHead[4];
qd[2]=fbHead[5];
I=new Integrator(Ts,qd,lim);
fp.resize(3,0.0);
eyesJ.resize(3,3);
eyesJ.zero();
genOn=false;
saccadeUnderWayOld=false;
}
main ()
{
term x, n;
type t;
term plus_omega, omega_2, plus_omega_2, plus_omega_n, omega_n, omega_omega,
plus_omega_omega;
term f, p;
param_out.fd = stdout;
param_err.fd = stderr;
init ();
/* Exemple : construction de l'ordinal omega * 2 */
x = ap (LIM, ap (ap (S(ORD,ORD,ORD),
ap (ap (S(ORD,fnc(ORD,ORD),fnc(ORD,ORD)),rep(ORD)),
ap(K(fnc(ORD,ORD),ORD),SUC))),
ap(K(ORD,ORD),ap(LIM,I(ORD)))));
sput ("x = ", out);
write_term (x, out);
t = type_term (x);
sput ("\nt = ", out);
write_type (t, out);
sput ("\n", out);
n = var ("n", ORD);
/*
x = ap (LIM, lambda (n,
ap (ap (ap (rep(ORD), n), SUC), ap (LIM, I(ORD)))
));
*/
x = ap (ap (ap (rep(ORD), n), SUC), ap (LIM, I(ORD)));
/* x = ap (rep(ORD), n); */
sput ("x = ", out);
write_term (x, out);
t = type_term (x);
sput ("\nt = ", out);
write_type (t, out);
sput ("\n", out);
x = lambda (n, x);
sput ("x = ", out);
write_term (x, out);
t = type_term (x);
sput ("\nt = ", out);
write_type (t, out);
sput ("\n", out);
x = ap (LIM, x);
sput ("x = ", out);
write_term (x, out);
t = type_term (x);
sput ("\nt = ", out);
write_type (t, out);
sput ("\n", out);
x = var ("x", ORD);
plus_omega = lambda (x, lim (lambda (n,
rpt (ORD, n, SUC, x)
)));
omega_2 = lim (lambda (n, rpt (ORD, n, plus_omega, ZERO)));
t = type_term (omega_2);
sput ("Type of omega_2 = ", out);
write_type (t, out);
sput ("\n", out);
plus_omega_2 = lambda (x, lim (lambda (n,
rpt (ORD, n, plus_omega, x))));
t = type_term (plus_omega_2);
sput ("Type of plus_omega_2 is ", out);
write_type (t, out);
f = var ("f", fnc(ORD,ORD));
p = var ("p", ORD);
plus_omega_n = lambda (n, rpt (fnc(ORD,ORD), n, lambda (f,
lambda (x, lim (lambda (p, rpt (ORD, p, f, x)))) ),
SUC));
/*
next_power = lambda (f,
lambda (x, lim (lambda (p, rpt (ORD, p, f, x)))));
plus_omega_n = lambda (n, rpt (fnc(ORD,ORD), n,
*/
t = type_term (plus_omega_n);
sput ("\nType of plus_omega_n is ", out);
write_type (t, out);
omega_n = lambda (n, ap (ap (plus_omega_n, n), ZERO));
t = type_term (omega_n);
sput ("\nType of omega_n is ", out);
write_type (t, out);
omega_omega = lim (omega_n);
t = type_term (omega_omega);
sput ("\nType of omega_omega is ", out);
write_type (t, out);
plus_omega_omega = lambda (x, lim (lambda (n,
ap (ap (plus_omega_n, n), x) )));
t = type_term (plus_omega_omega);
sput ("\nType of plus_omega_omega is ", out);
write_type (t, out);
sput ("\n", out);
}