real SocSystem_Analytical::getCosts(arr& R,arr& r,uint t,const arr& qt){
uint N=x.N;
R.resize(N,N); R.setZero();
r.resize(N); r.setZero();
real C=0.;
#ifndef USE_TRUNCATION //potentials for collision cost
arr J(1,qt.N),phiHatQ(1);
J.setZero();
phiHatQ.setZero();
for(uint i=0;i<obstacles.d0;i++){
real margin = .1;
real d = (1.-norm(x-obstacles[i])/margin);
if(d<0) continue;
phiHatQ(0) += d*d;
J += ((real)2.*d/margin)*(obstacles[i]-x)/norm(x-obstacles[i]);
}
J.reshape(1,J.N);
arr tJ,target(1);
target=(real)0.;
transpose(tJ,J);
real colprec = (real)5e2;
C += colprec*sqrDistance(target,phiHatQ);
R += colprec*tJ*J;
r += colprec*tJ*(target - phiHatQ + J*qt);
#endif
if(t!=T-1) return C;
R.setDiag(1.);
r = x1;
R *= prec;
r *= prec;
C += prec*sqrDistance(x1,x);
return C;
}
void soc::SocSystem_Ors::getqv0(arr& q_){
if(!WS->Qlin.N) q_.setBlockVector(WS->q0,WS->v0);
else{
arr q,qd;
getqv0(q,qd);
q_.setBlockVector(q,qd);
}
}
//initialization methods
void initKinematic(uint dim,uint trajectory_length, real w, real endPrec){
x0.resize(dim); x0.setZero();
x1=x0; x1(0)=1.;
x=x0;
W.setDiag(w,x.N);
prec=endPrec;
T=trajectory_length;
obstacles.resize(2,x.N);
obstacles(0,0)=.3; obstacles(0,1)=.05;
obstacles(1,0)=.7; obstacles(1,1)=-.05;
dynamic=false;
//os = &cout;
}
virtual double fs(arr& g, arr& H, const arr& x) {
double A=.5, f=A*x.N;
for(uint i=0; i<x.N; i++) f += x(i)*x(i) - A*::cos(10.*x(i));
if(&g) {
g.resize(x.N);
for(uint i=0; i<x.N; i++) g(i) = 2*x(i) + 10.*A*::sin(10.*x(i));
}
if(&H) {
H.resize(x.N,x.N); H.setZero();
for(uint i=0; i<x.N; i++) H(i,i) = 2 + 100.*A*::cos(10.*x(i));
}
return f;
}
arr rightminarray(const arr& hist)
{
arr out(hist.size());
stack<int> S;
for(int i=hist.size()-1 ;i>=0; --i)
{
while( !S.empty() && hist[S.top()] >= hist[i]) S.pop();
if(!S.empty())out[i] = S.top();
else out[i] = hist.size();
S.push(i);
}
return out;
}
arr leftminarray(const arr& hist)
{
arr out(hist.size());
stack<int> S;
for(int i=0;i<hist.size(); ++i)
{
while( !S.empty() && hist[S.top()] >= hist[i]) S.pop();
if(!S.empty())out[i] = S.top();
else out[i] = -1;
S.push(i);
}
return out;
}
void soc::SocSystem_Ors::getMF(arr& M,arr& F){
if(!WS->pseudoDynamic){
if(WS->Qlin.N) NIY;
ors->clearForces();
ors->gravityToForces();
//ors->frictionToForces(1.1);
ors->equationOfMotion(M,F,WS->v_act);
//M.setId(); F = .1;
//Minv *= .2;//1e-1;
}else{
uint n=qDim();
M.setId(n);
F.resize(n); F.setZero();
}
}
virtual double fs(arr& g, arr& H, const arr& x) {
double f=exp(-sumOfSqr(x));
if(&g) g=2.*f*x;
if(&H) { H.setDiag(2.*f, x.N); H -= 4.*f*(x^x); }
f = 1.-f;
return f;
}
virtual double fs(arr& g, arr& H, const arr& x) {
//initialize on first call
if(which==none){
which = (Which) MT::getParameter<int>("fctChoice");
}
if(!condition.N!=x.N){
condition.resize(x.N);
double cond = MT::getParameter<double>("condition");
for(uint i=0; i<x.N; i++) condition(i) = pow(cond,0.5*i/(x.N-1));
}
arr y = x;
y *= condition; //elem-wise product
double f;
switch(which) {
case sum: f = SumFunction.fs(g, H, y); break;
case square: f = SquareFunction.fs(g, H, y); break;
case hole: f = HoleFunction.fs(g, H, y); break;
case rosenbrock: f = RosenbrockFunction.fs(g, H, y); break;
case rastrigin: f = RastriginFunction.fs(g, H, y); break;
default: NIY;
}
if(&g) g *= condition; //elem-wise product
if(&H) H = condition%H%condition;
return f;
}
void getPhi(arr& phiq_i,uint i){ NIY;
if(i==1){
phiq_i.resize(1);
for(uint i=0;i<obstacles.N;i++){
phiq_i(0) += norm(x-obstacles[i]);
}
}
}
size_t lookup_by_name(arr &names, const char *name) {
for (size_t idx = 0; idx < names.size(); ++idx) {
if (strcmp(name, names[idx]) == 0) {
return idx;
}
}
return 0;
}
// Prepare weights used by map2alm. weight array must be allocated already:
void PrepRingWeights(int col, arr<double> & weight, int nside) {
double *tempweight;
int status;
long i;
if (USE_WEIGHTS==1) {
Announce("Loading Healpix map weights... ");
tempweight = vector<double>(0, 2*nside-1);
status = ReadHealpixWeights(col, nside, tempweight);
if (status==0) for (i=0; i<2*nside; i++) weight[i]=1.0+tempweight[i];
else {
warning("PrepRingWeights: could not load Healpix weights, using 1.0 instead.");
weight.fill(1);
}
free_vector(tempweight, 0, 2*nside-1);
Announce();
}
else weight.fill(1);
}
//Given an array and a value, remove all instances of that value in place and return the new length.
//The order of elements can be changed.It doesn't matter what you leave beyond the new length(超出新长度的元素可以任意).
int Array::removeElement(arr& nums, int val)
{
int i, j;
for (i = j = nums.size() - 1; i >= 0; i--)
{
if (nums[i] == val && i != j--)
nums[i] = nums[j + 1];
}
return j + 1;
}
void getTriNormals(const Mesh& m, arr& Tn) {
uint i;
ors::Vector a, b, c;
Tn.resize(m.T.d0, 3); //tri normals
for(i=0; i<m.T.d0; i++) {
a.set(&m.V(m.T(i, 0), 0)); b.set(&m.V(m.T(i, 1), 0)); c.set(&m.V(m.T(i, 2), 0));
b-=a; c-=a; a=b^c; a.normalize();
Tn(i, 0)=a.x; Tn(i, 1)=a.y; Tn(i, 2)=a.z;
}
}
int max_area(const arr& hist)
{
int max_area = INT_MIN;
arr left = leftminarray(hist);
arr right = rightminarray(hist);
for(int i=0; i < hist.size(); ++i)
{
int area = (right[i]-left[i]-1 )*hist[i];
if( area > max_area) max_area = area;
}
return max_area;
}
void generateConditionedRandomProjection(arr& M, uint n, double condition) {
uint i,j;
//let M be a ortho-normal matrix (=random rotation matrix)
M.resize(n,n);
rndUniform(M,-1.,1.,false);
//orthogonalize
for(i=0; i<n; i++) {
for(j=0; j<i; j++) M[i]()-=scalarProduct(M[i],M[j])*M[j];
M[i]()/=length(M[i]);
}
//we condition each column of M with powers of the condition
for(i=0; i<n; i++) M[i]() *= pow(condition, double(i) / (2.*double(n - 1)));
}
void sharp_sht::backward(const arr< dcomplex > &in_alm, arr< double > &out_map)
{
dsharpjob->alm2map(in_alm.begin(), out_map.begin(), false);
}
void sharp_sht::forward(const arr< double > &in_map, arr< dcomplex > &out_alm)
{
dsharpjob->map2alm(in_map.begin(), out_alm.begin(), false);
}
virtual double fs(arr& g, arr& H, const arr& x) {
if(&g) { g.resize(x.N); g=1.; }
if(&H) { H.resize(x.N,x.N); H.setZero(); }
return sum(x);
}
virtual double fs(arr& g, arr& H, const arr& x) {
if(&g) g=2.*x;
if(&H) H.setDiag(2., x.N);
return sumOfSqr(x);
}
void ParticleAroundWalls::phi_t(arr& phi, arr& J, uint t, const arr& x_bar){
uint T=get_T(), n=dim_x(), k=get_k();
//assert some dimensions
CHECK(x_bar.d0==k+1,"");
CHECK(x_bar.d1==n,"");
CHECK(t<=T,"");
//-- transition costs: append to phi
if(k==1) phi = x_bar[1]-x_bar[0]; //penalize velocity
if(k==2) phi = x_bar[2]-2.*x_bar[1]+x_bar[0]; //penalize acceleration
if(k==3) phi = x_bar[3]-3.*x_bar[2]+3.*x_bar[1]-x_bar[0]; //penalize jerk
//-- walls: append to phi
//Note: here we append to phi ONLY in certain time slices: the dimensionality of phi may very with time slices; see dim_phi(uint t)
double eps=.1, power=2.;
if(!hardConstrained){
//-- wall costs
for(uint i=0;i<n;i++){ //add barrier costs to each dimension
if(t==T/4) phi.append(MT::ineqConstraintCost(i+1.-x_bar(k,i), eps, power)); //middle factor: ``greater than i''
if(t==T/2) phi.append(MT::ineqConstraintCost(x_bar(k,i)+i+1., eps, power)); //last factor: ``lower than -i''
if(t==3*T/4) phi.append(MT::ineqConstraintCost(i+1.-x_bar(k,i), eps, power)); //middle factor: ``greater than i''
if(t==T) phi.append(MT::ineqConstraintCost(x_bar(k,i)+i+1., eps, power)); //last factor: ``lower than -i''
}
}else{
//-- wall constraints
for(uint i=0;i<n;i++){ //add barrier costs to each dimension
if(t==T/4) phi.append((i+1.-x_bar(k,i))); //middle factor: ``greater than i''
if(t==T/2) phi.append((x_bar(k,i)+i+1.)); //last factor: ``lower than -i''
if(t==3*T/4) phi.append((i+1.-x_bar(k,i))); //middle factor: ``greater than i''
if(t==T) phi.append((x_bar(k,i)+i+1.)); //last factor: ``lower than -i''
}
}
uint m=phi.N;
CHECK(m==dim_phi(t),"");
if(&J){ //we also need to return the Jacobian
J.resize(m,k+1,n).setZero();
//-- transition costs
for(uint i=0;i<n;i++){
if(k==1){ J(i,1,i) = 1.; J(i,0,i) = -1.; }
if(k==2){ J(i,2,i) = 1.; J(i,1,i) = -2.; J(i,0,i) = 1.; }
if(k==3){ J(i,3,i) = 1.; J(i,2,i) = -3.; J(i,1,i) = +3.; J(i,0,i) = -1.; }
}
//-- walls
if(!hardConstrained){
for(uint i=0;i<n;i++){
if(t==T/4) J(n+i,k,i) = -MT::d_ineqConstraintCost(i+1.-x_bar(k,i), eps, power);
if(t==T/2) J(n+i,k,i) = MT::d_ineqConstraintCost(x_bar(k,i)+i+1., eps, power);
if(t==3*T/4) J(n+i,k,i) = -MT::d_ineqConstraintCost(i+1.-x_bar(k,i), eps, power);
if(t==T) J(n+i,k,i) = MT::d_ineqConstraintCost(x_bar(k,i)+i+1., eps, power);
}
}else{
for(uint i=0;i<n;i++){
if(t==T/4) J(n+i,k,i) = -1.;
if(t==T/2) J(n+i,k,i) = +1.;
if(t==3*T/4) J(n+i,k,i) = -1.;
if(t==T) J(n+i,k,i) = +1.;
}
}
}
}
template<typename... Ix> size_t offset_at(const arr& a, Ix&&... idx) { return a.offset_at(idx...); }
template<typename... Ix> size_t index_at(const arr& a, Ix&&... idx) { return a.index_at(idx...); }
template<typename... Ix> arr data(const arr& a, Ix&&... index) {
return arr(a.nbytes() - a.offset_at(index...), (const uint8_t *) a.data(index...));
}
void SocSystem_Analytical::getProcess(arr& A,arr& a,arr& B){
uint N=x.N;
A.setDiag(1.,N);
B.setDiag(1.,N);
a.resize(N); a.setZero();
}
bool sRprop::step(arr& w, const arr& grad, uint *singleI) {
if(!stepSize.N) { //initialize
stepSize.resize(w.N);
lastGrad.resize(w.N);
lastGrad.setZero();
stepSize = delta0;
}
CHECK(grad.N==stepSize.N, "Rprop: gradient dimensionality changed!");
CHECK(w.N==stepSize.N , "Rprop: parameter dimensionality changed!");
uint i=0, I=w.N;
if(singleI) {
i=*(singleI);
I=i+1;
}
for(; i<I; i++) {
if(grad.elem(i) * lastGrad(i) > 0) { //same direction as last time
if(rMax) dMax=fabs(rMax*w.elem(i));
stepSize(i) = _mymin(dMax, incr * stepSize(i)); //increase step size
w.elem(i) += stepSize(i) * -_sgn(grad.elem(i)); //step in right direction
lastGrad(i) = grad.elem(i); //memorize gradient
} else if(grad.elem(i) * lastGrad(i) < 0) { //change of direction
stepSize(i) = _mymax(dMin, decr * stepSize(i)); //decrease step size
w.elem(i) += stepSize(i) * -_sgn(grad.elem(i)); //step in right direction (undo half the step)
lastGrad(i) = 0; //memorize to continue below next time
} else { //after change of direcion
w.elem(i) += stepSize(i) * -_sgn(grad.elem(i)); //step in right direction
lastGrad(i) = grad.elem(i); //memorize gradient
}
}
return stepSize.max() < incr*dMin;
}
void sharp_sht::forward(const arr< float > &in_map, arr< fcomplex > &out_alm)
{
fsharpjob->map2alm(in_map.begin(), out_alm.begin(), false);
}
void sharp_sht::backward(const arr< fcomplex > &in_alm, arr< float > &out_map)
{
fsharpjob->alm2map(in_alm.begin(), out_map.begin(), false);
}
void SocSystem_Analytical::getConstraints(arr& cdir,arr& coff,uint t,const arr& qt){
cdir.clear();
coff.clear();
#ifndef USE_TRUNCATION
return;
#endif
uint i,M=obstacles.d0;
arr d;
#if 0 //direct and clean way to do it -- but depends simple scenario
cdir.resize(M,x.N);
coff.resize(M);
for(i=0;i<M;i++){
cdir[i] = qt-obstacles[i];
coff(i) = scalarProduct(cdir[i],obstacles[i]);
}
#elif 1 //assume that the task vector is a list of scalars, each constrained >0
arr J,y;
for(i=0;i<M;i++){
real haty = norm(x-obstacles[i]);
if(haty>.5) continue; //that's good enough -> don't add the constraint
J = (x-obstacles[i])/norm(x-obstacles[i]);
coff.append(-haty + scalarProduct(J,x));
cdir.append(J);
}
cdir.reshape(coff.N,x.N);
coff.reshape(coff.N);
#else //messy: try to combine all constraints into a single scalar, doesn't really work...
//first compute squared collision meassure...
arr J(1,qt.N),phiHatQ(1);
J.setZero();
phiHatQ.setZero();
for(i=0;i<obstacles.d0;i++){
real margin = .25;
real d = 1.-norm(x-obstacles[i])/margin;
//if(d<0) continue;
//phiHatQ(0) += d*d;
//J += (2.*d/margin)*(obstacles[i]-x)/norm(x-obstacles[i]);
phiHatQ(0) += d;
J += (1./margin)*(obstacles[i]-x)/norm(x-obstacles[i]);
}
//...then add a single constraint
if(phiHatQ(0)>0.){ //potential violation, else discard
cdir.append(-J);
coff.append(phiHatQ-scalarProduct(J,x)-1.);
cdir.reshape(1,x.N);
coff.reshape(1);
}
#endif
}
