static int
moveview( /* move our view */
int dx,
int dy,
int mov,
int orb
)
{
VIEW nv;
FVECT odir, v1;
double d;
int li;
/* start with old view */
nv = odev.v;
/* change view direction */
if (mov | orb) {
if ((li = qtFindLeaf(dx, dy)) < 0)
return(0); /* not on window */
VSUM(odir, qtL.wp[li], nv.vp, -1.);
} else {
if (viewray(nv.vp, nv.vdir, &odev.v,
(dx+.5)/odev.hres, (dy+.5)/odev.vres) < -FTINY)
return(0); /* outside view */
}
if (orb && mov) { /* orbit left/right */
spinvector(odir, odir, nv.vup, d=MOVDEG*PI/180.*mov);
VSUM(nv.vp, qtL.wp[li], odir, -1.);
spinvector(nv.vdir, nv.vdir, nv.vup, d);
} else if (orb) { /* orbit up/down */
if (geodesic(odir, odir, nv.vup,
d=MOVDEG*PI/180.*orb, GEOD_RAD) == 0.0)
return(0);
VSUM(nv.vp, qtL.wp[li], odir, -1.);
geodesic(nv.vdir, nv.vdir, nv.vup, d, GEOD_RAD);
} else if (mov) { /* move forward/backward */
d = MOVPCT/100. * mov;
VSUM(nv.vp, nv.vp, odir, d);
}
if (!mov ^ !orb && headlocked) { /* restore head height */
VSUM(v1, odev.v.vp, nv.vp, -1.);
d = DOT(v1, odev.v.vup);
VSUM(nv.vp, nv.vp, odev.v.vup, d);
}
if (setview(&nv) != NULL)
return(0); /* illegal view */
dev_view(&nv);
inpresflags |= DFL(DC_SETVIEW);
return(1);
}
/* Compute average DSF value at the given radius from central vector */
static double
eval_DSFsurround(const RBFNODE *rbf, const FVECT outvec, const double rad)
{
const int ninc = 12;
const double phinc = 2.*M_PI/ninc;
double sum = 0;
int n = 0;
FVECT tvec;
int i;
/* compute initial vector */
if (output_orient*outvec[2] >= 1.-FTINY) {
tvec[0] = tvec[2] = 0;
tvec[1] = 1;
} else {
tvec[0] = tvec[1] = 0;
tvec[2] = 1;
}
geodesic(tvec, outvec, tvec, rad, GEOD_RAD);
/* average surrounding DSF */
for (i = 0; i < ninc; i++) {
if (i) spinvector(tvec, tvec, outvec, phinc);
if (tvec[2] > 0 ^ output_orient > 0)
continue;
sum += eval_rbfrep(rbf, tvec) * COSF(tvec[2]);
++n;
}
if (n < 2) /* should never happen! */
return(sum);
return(sum/(double)n);
}
static int
moveview( /* move our view */
int dx,
int dy,
int mov,
int orb
)
{
VIEW nv;
FVECT odir, v1, wp;
double d;
/* start with old view */
nv = thisview;
/* change view direction */
if ((d = viewray(v1, odir, &thisview,
(dx+.5)/hres, (dy+.5)/vres)) < -FTINY)
return(0); /* outside view */
if (mov | orb) {
if (!getintersect(wp, v1, odir, d))
return(0);
VSUM(odir, wp, nv.vp, -1.);
} else
VCOPY(nv.vdir, odir);
if (orb && mov) { /* orbit left/right */
spinvector(odir, odir, nv.vup, d=MOVDEG*PI/180.*mov);
VSUM(nv.vp, wp, odir, -1.);
spinvector(nv.vdir, nv.vdir, nv.vup, d);
} else if (orb) { /* orbit up/down */
if (geodesic(odir, odir, nv.vup,
d=MOVDEG*PI/180.*orb, GEOD_RAD) == 0.0)
return(0);
VSUM(nv.vp, wp, odir, -1.);
geodesic(nv.vdir, nv.vdir, nv.vup, d, GEOD_RAD);
} else if (mov) { /* move forward/backward */
d = MOVPCT/100. * mov;
VSUM(nv.vp, nv.vp, odir, d);
}
if (!mov ^ !orb && headlocked) { /* restore head height */
VSUM(v1, thisview.vp, nv.vp, -1.);
d = DOT(v1, thisview.vup);
VSUM(nv.vp, nv.vp, thisview.vup, d);
}
if (setview(&nv) != NULL)
return(0); /* illegal view */
dev_view(&nv);
return(1);
}
void
getrotate( /* rotate camera */
char *s
)
{
VIEW nv = ourview;
double angle, elev, zfact;
elev = 0.0; zfact = 1.0;
if (sscanf(s, "%lf %lf %lf", &angle, &elev, &zfact) < 1) {
error(COMMAND, "missing angle");
return;
}
spinvector(nv.vdir, ourview.vdir, ourview.vup, angle*(PI/180.));
if (elev != 0.0)
geodesic(nv.vdir, nv.vdir, nv.vup, elev*(PI/180.), GEOD_RAD);
zoomview(&nv, zfact);
newview(&nv);
}
void SmoothDiscretizePath(Robot& robot,const vector<Config>& oldconfigs,int n,vector<Real>& times,vector<Config>& configs)
{
times.resize(n);
configs.resize(n);
RobotCSpace space(robot);
RobotGeodesicManifold geodesic(robot);
vector<GeneralizedCubicBezierCurve> curves;
SPLINE_INTERPOLATE_FUNC(oldconfigs,curves,&space,&geodesic);
times[0] = 0;
for(int i=1;i<n;i++)
times[i] = Real(i)/Real(n-1);
configs[0] = oldconfigs[0];
configs.back() = oldconfigs.back();
Vector temp;
for(int i=1;i+1<n;i++) {
Real u0 = Floor(times[i]*curves.size());
int segind = (int)u0;
Real u=times[i]*curves.size()-u0;
curves[segind].Eval(u,configs[i]);
}
}
Word ThLeftNormalForm::getReducedWord2() const {
if (theOmegaPower >= 0 || theDecomposition.size() == 0)
return getWord();
const auto p = -theOmegaPower;
const auto d = theDecomposition.size();
auto a = p < d ? p : d;
Word result;
// 1. Process omega
const Permutation omega = Permutation::getHalfTwistPermutation(theRank);
Word omegaWord = Word(omega.geodesicWord());
omegaWord = -omegaWord;
result = omegaWord.power(p - a);
// 2. Sort permutations and find the cut_value
vector<int> sizes;
for (const auto &perm : getDecomposition())
sizes.push_back(perm.geodesic().size());
sort(sizes.begin(), sizes.end(), [](const int &lhs, const int &rhs) { return lhs > rhs; });
const int cut_value = sizes[a - 1];
// 3. Cancel omega^-1 with positive permutations
for (auto it = theDecomposition.begin(); it != theDecomposition.end(); ++it) {
auto perm = *it;
if (a > 0 && perm.geodesic().size() >= cut_value) {
perm = (-perm) * omega;
if ((--a) % 2 != 0)
perm = perm.flip();
result *= -Word(perm.geodesicWord());
} else {
if (a % 2 != 0)
perm = perm.flip();
result *= Word(perm.geodesicWord());
}
}
return result;
}
void
moveview( /* move viewpoint */
double angle,
double elev,
double mag,
FVECT vc
)
{
double d;
VIEW nv = ourview;
int i;
spinvector(nv.vdir, ourview.vdir, ourview.vup, angle*(PI/180.));
if (elev != 0.0)
geodesic(nv.vdir, nv.vdir, nv.vup, elev*(-PI/180.), GEOD_RAD);
if (nv.type == VT_PAR) {
nv.horiz /= mag;
nv.vert /= mag;
d = 0.0; /* don't move closer */
for (i = 0; i < 3; i++)
d += (vc[i] - ourview.vp[i])*ourview.vdir[i];
} else {
d = sqrt(dist2(ourview.vp, vc)) / mag;
if (nv.vfore > FTINY) {
nv.vfore += d - d*mag;
if (nv.vfore < 0.0) nv.vfore = 0.0;
}
if (nv.vaft > FTINY) {
nv.vaft += d - d*mag;
if (nv.vaft <= nv.vfore) nv.vaft = 0.0;
}
nv.vdist /= mag;
}
for (i = 0; i < 3; i++)
nv.vp[i] = vc[i] - d*nv.vdir[i];
newview(&nv);
}