void IPM
( const Matrix<Real>& A,
const Matrix<Real>& b,
Real lambda,
Matrix<Real>& x,
const qp::affine::Ctrl<Real>& ctrl )
{
EL_DEBUG_CSE
const Int m = A.Height();
const Int n = A.Width();
const Range<Int> uInd(0,n), vInd(n,2*n), rInd(2*n,2*n+m);
Matrix<Real> Q, c, AHat, G, h;
// Q := | 0 0 0 |
// | 0 0 0 |
// | 0 0 I |
// ==============
Zeros( Q, 2*n+m, 2*n+m );
auto Qrr = Q( rInd, rInd );
FillDiagonal( Qrr, Real(1) );
// c := lambda*[1;1;0]
// ===================
Zeros( c, 2*n+m, 1 );
auto cuv = c( IR(0,2*n), ALL );
Fill( cuv, lambda );
// \hat A := [A, -A, I]
// ====================
Zeros( AHat, m, 2*n+m );
auto AHatu = AHat( IR(0,m), uInd );
auto AHatv = AHat( IR(0,m), vInd );
auto AHatr = AHat( IR(0,m), rInd );
AHatu = A;
AHatv -= A;
FillDiagonal( AHatr, Real(1) );
// G := | -I 0 0 |
// | 0 -I 0 |
// ================
Zeros( G, 2*n, 2*n+m );
FillDiagonal( G, Real(-1) );
// h := 0
// ======
Zeros( h, 2*n, 1 );
// Solve the affine QP
// ===================
Matrix<Real> xHat, y, z, s;
QP( Q, AHat, G, b, c, h, xHat, y, z, s, ctrl );
// x := u - v
// ==========
x = xHat( uInd, ALL );
x -= xHat( vInd, ALL );
}
VectorXd MotionModel::Qprod(VectorXd Q, VectorXd P)
{
double a, x;
VectorXd QP(4), QP2;
Vector3d u, v;
a = Q(0); x = P(0);
v = Q.segment(1,3);
u = P.segment(1,3);
QP2 = a * u + x * v + v.cross(u); // cross of v and u
QP << a * x - v.transpose() * u, QP2;
return QP; // QP should be a 4x1 vector
}
int ControlT(const float* P1, const float* P2, const float* MechPara, const float* IMUdata, const float* TensionT)
{
float R1[9],R2[9];
float l1[3],l2[3];
l1[0]=l1[1]=0;
l1[2]=MechPara[0];
l2[0]=l1[2]=0;
l2[2]=MechPara[1];
if(IMUdata[0]==4||IMUdata[1]==4||IMUdata[2]==4||IMUdata[3]==4||IMUdata[4]==4||IMUdata[5]==4)//IMU数据错误
return -1;
EA2SO3(*(IMUdata+2),*(IMUdata+1),*(IMUdata),R1);//求矩阵R1，R2
EA2SO3(*(IMUdata+5),*(IMUdata+4),*(IMUdata+3),R2);
float P[3];
SolveP(R1, R2, l1, l2, P);//求腕部轨迹P
float d;
float fp[3];
d = FootPoint(P1, P2, P, fp);//求P到轨迹P1P2的距离d与垂足fp
const float an = 1;//构造末端力的参数
float F[3];
MakeF(P, fp, d, an, F);//构造末端所需的力F
float taos[3],taoe[3];
TransformF(R1, R2, l1, l2, F, taos, taoe);//末端力转化到关节力矩
float jacob_s[12],jacob_e[6];
SolveJacob(MechPara, R1, R2, jacob_s, jacob_e);//求Jacob矩阵
float ts[4];
if(QP(jacob_s,taos,ts) == -1)//求优发散了
{
return -1;
}
else
{
return 1;
// if(QP(jacob_e,taoe,te == -1))//求优发散了
// return -1;
// else//终于求出来最优解了
// {
//
// }
}
}
Z H1(ts){A z;Z C *t[]={"int","float","char","null","box","sym","func",
"unknown"};
W(gs(Et))*z->p=MS(si(t[QA(a)?(Et<a->t?6:Et>a->t?a->t:!a->n?3:
Et==a->t?(QA(a=(A)*a->p)&&a->t<Xt?4:QS(a)?5:6):6):
QP(a)?6:QX(a)?6:7]));R(I)z;}
void IPM
( const DistSparseMatrix<Real>& A,
const DistMultiVec<Real>& b,
Real lambda,
DistMultiVec<Real>& x,
const qp::affine::Ctrl<Real>& ctrl )
{
DEBUG_CSE
const Int m = A.Height();
const Int n = A.Width();
mpi::Comm comm = A.Comm();
DistSparseMatrix<Real> Q(comm), AHat(comm), G(comm);
DistMultiVec<Real> c(comm), h(comm);
// Q := | 0 0 0 |
// | 0 0 0 |
// | 0 0 I |
// ==============
Zeros( Q, 2*n+m, 2*n+m );
{
Int numLocalUpdates = 0;
for( Int iLoc=0; iLoc<Q.LocalHeight(); ++iLoc )
if( Q.GlobalRow(iLoc) >= 2*n )
++numLocalUpdates;
Q.Reserve( numLocalUpdates );
for( Int iLoc=0; iLoc<Q.LocalHeight(); ++iLoc )
if( Q.GlobalRow(iLoc) >= 2*n )
Q.QueueLocalUpdate( iLoc, Q.GlobalRow(iLoc), Real(1) );
Q.ProcessLocalQueues();
}
// c := lambda*[1;1;0]
// ===================
Zeros( c, 2*n+m, 1 );
auto& cLoc = c.Matrix();
for( Int iLoc=0; iLoc<c.LocalHeight(); ++iLoc )
if( c.GlobalRow(iLoc) < 2*n )
cLoc(iLoc) = lambda;
// \hat A := [A, -A, I]
// ====================
// NOTE: Since A and \hat A are the same height and each distributed within
// columns, it is possible to form \hat A from A without communication
const Int numLocalEntriesA = A.NumLocalEntries();
Zeros( AHat, m, 2*n+m );
AHat.Reserve( 2*numLocalEntriesA+AHat.LocalHeight() );
for( Int e=0; e<numLocalEntriesA; ++e )
{
AHat.QueueUpdate( A.Row(e), A.Col(e), A.Value(e) );
AHat.QueueUpdate( A.Row(e), A.Col(e)+n, -A.Value(e) );
}
for( Int iLoc=0; iLoc<AHat.LocalHeight(); ++iLoc )
{
const Int i = AHat.GlobalRow(iLoc);
AHat.QueueLocalUpdate( iLoc, i+2*n, Real(1) );
}
AHat.ProcessLocalQueues();
// G := | -I 0 0 |
// | 0 -I 0 |
// ================
Zeros( G, 2*n, 2*n+m );
G.Reserve( G.LocalHeight() );
for( Int iLoc=0; iLoc<G.LocalHeight(); ++iLoc )
{
const Int i = G.GlobalRow(iLoc);
G.QueueLocalUpdate( iLoc, i, Real(-1) );
}
G.ProcessLocalQueues();
// h := 0
// ======
Zeros( h, 2*n, 1 );
// Solve the affine QP
// ===================
DistMultiVec<Real> xHat(comm), y(comm), z(comm), s(comm);
QP( Q, AHat, G, b, c, h, xHat, y, z, s, ctrl );
// x := u - v
// ==========
Zeros( x, n, 1 );
Int numRemoteUpdates = 0;
for( Int iLoc=0; iLoc<xHat.LocalHeight(); ++iLoc )
if( xHat.GlobalRow(iLoc) < 2*n )
++numRemoteUpdates;
else
break;
x.Reserve( numRemoteUpdates );
auto& xHatLoc = xHat.LockedMatrix();
for( Int iLoc=0; iLoc<xHat.LocalHeight(); ++iLoc )
{
const Int i = xHat.GlobalRow(iLoc);
if( i < n )
x.QueueUpdate( i, 0, xHatLoc(iLoc) );
else if( i < 2*n )
x.QueueUpdate( i-n, 0, -xHatLoc(iLoc) );
else
break;
}
x.ProcessQueues();
}
void IPM
( const SparseMatrix<Real>& A,
const Matrix<Real>& b,
Real lambda,
Matrix<Real>& x,
const qp::affine::Ctrl<Real>& ctrl )
{
DEBUG_CSE
const Int m = A.Height();
const Int n = A.Width();
const Range<Int> uInd(0,n), vInd(n,2*n), rInd(2*n,2*n+m);
SparseMatrix<Real> Q, AHat, G;
Matrix<Real> c, h;
// Q := | 0 0 0 |
// | 0 0 0 |
// | 0 0 I |
// ==============
Zeros( Q, 2*n+m, 2*n+m );
Q.Reserve( m );
for( Int e=0; e<m; ++e )
Q.QueueUpdate( 2*n+e, 2*n+e, Real(1) );
Q.ProcessQueues();
// c := lambda*[1;1;0]
// ===================
Zeros( c, 2*n+m, 1 );
auto cuv = c( IR(0,2*n), ALL );
Fill( cuv, lambda );
// \hat A := [A, -A, I]
// ====================
const Int numEntriesA = A.NumEntries();
Zeros( AHat, m, 2*n+m );
AHat.Reserve( 2*numEntriesA+m );
for( Int e=0; e<numEntriesA; ++e )
{
AHat.QueueUpdate( A.Row(e), A.Col(e), A.Value(e) );
AHat.QueueUpdate( A.Row(e), A.Col(e)+n, -A.Value(e) );
}
for( Int e=0; e<m; ++e )
AHat.QueueUpdate( e, e+2*n, Real(1) );
AHat.ProcessQueues();
// G := | -I 0 0 |
// | 0 -I 0 |
// ================
Zeros( G, 2*n, 2*n+m );
G.Reserve( 2*m );
for( Int e=0; e<2*m; ++e )
G.QueueUpdate( e, e, Real(-1) );
G.ProcessQueues();
// h := 0
// ======
Zeros( h, 2*n, 1 );
// Solve the affine QP
// ===================
Matrix<Real> xHat, y, z, s;
QP( Q, AHat, G, b, c, h, xHat, y, z, s, ctrl );
// x := u - v
// ==========
x = xHat( uInd, ALL );
x -= xHat( vInd, ALL );
}
/*
* Q P r o b l e m _ q p O A S E S
*/
int_t QProblem_qpOASES( int_t nV, int_t nC, HessianType hessianType, int_t nP,
SymmetricMatrix* H, double* g, Matrix* A,
double* lb, double* ub,
double* lbA, double* ubA,
int_t nWSRin, real_t maxCpuTimeIn,
const double* const x0, Options* options,
int_t nOutputs, mxArray* plhs[],
const double* const guessedBounds, const double* const guessedConstraints,
const double* const _R
)
{
int_t nWSRout;
real_t maxCpuTimeOut;
/* 1) Setup initial QP. */
QProblem QP( nV,nC,hessianType );
QP.setOptions( *options );
/* 2) Solve initial QP. */
returnValue returnvalue;
Bounds bounds(nV);
Constraints constraints(nC);
if (guessedBounds != 0) {
for (int_t i = 0; i < nV; i++) {
if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
bounds.setupBound(i, ST_LOWER);
} else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
bounds.setupBound(i, ST_UPPER);
} else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
bounds.setupBound(i, ST_INACTIVE);
} else {
char msg[MAX_STRING_LENGTH];
snprintf(msg, MAX_STRING_LENGTH,
"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
myMexErrMsgTxt(msg);
return -1;
}
}
}
if (guessedConstraints != 0) {
for (int_t i = 0; i < nC; i++) {
if ( isEqual(guessedConstraints[i],-1.0) == BT_TRUE ) {
constraints.setupConstraint(i, ST_LOWER);
} else if ( isEqual(guessedConstraints[i],1.0) == BT_TRUE ) {
constraints.setupConstraint(i, ST_UPPER);
} else if ( isEqual(guessedConstraints[i],0.0) == BT_TRUE ) {
constraints.setupConstraint(i, ST_INACTIVE);
} else {
char msg[MAX_STRING_LENGTH];
snprintf(msg, MAX_STRING_LENGTH,
"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of constraints!");
myMexErrMsgTxt(msg);
return -1;
}
}
}
nWSRout = nWSRin;
maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
returnvalue = QP.init( H,g,A,lb,ub,lbA,ubA,
nWSRout,&maxCpuTimeOut,
x0,0,
(guessedBounds != 0) ? &bounds : 0, (guessedConstraints != 0) ? &constraints : 0,
_R
);
/* 3) Solve remaining QPs and assign lhs arguments. */
/* Set up pointers to the current QP vectors */
real_t* g_current = g;
real_t* lb_current = lb;
real_t* ub_current = ub;
real_t* lbA_current = lbA;
real_t* ubA_current = ubA;
/* Loop through QP sequence. */
for ( int_t k=0; k<nP; ++k )
{
if ( k > 0 )
{
/* update pointers to the current QP vectors */
g_current = &(g[k*nV]);
if ( lb != 0 )
lb_current = &(lb[k*nV]);
if ( ub != 0 )
ub_current = &(ub[k*nV]);
if ( lbA != 0 )
lbA_current = &(lbA[k*nC]);
if ( ubA != 0 )
ubA_current = &(ubA[k*nC]);
nWSRout = nWSRin;
maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
returnvalue = QP.hotstart( g_current,lb_current,ub_current,lbA_current,ubA_current, nWSRout,&maxCpuTimeOut );
}
/* write results into output vectors */
obtainOutputs( k,&QP,returnvalue,nWSRout,maxCpuTimeOut,
nOutputs,plhs,nV,nC );
}
//QP.writeQpDataIntoMatFile( "qpDataMat0.mat" );
return 0;
}
void IPM
( const AbstractDistMatrix<Real>& A,
const AbstractDistMatrix<Real>& d,
Real lambda,
AbstractDistMatrix<Real>& x,
const qp::affine::Ctrl<Real>& ctrl )
{
EL_DEBUG_CSE
const Int m = A.Height();
const Int n = A.Width();
const Grid& g = A.Grid();
const Range<Int> wInd(0,n), betaInd(n,n+1), zInd(n+1,n+m+1);
DistMatrix<Real> Q(g), c(g), AHat(g), b(g), G(g), h(g);
// Q := | I 0 0 |
// | 0 0 0 |
// | 0 0 0 |
// ==============
Zeros( Q, n+m+1, n+m+1 );
auto Qww = Q( wInd, wInd );
FillDiagonal( Qww, Real(1) );
// c := [0;0;lambda]
// ================
Zeros( c, n+m+1, 1 );
auto cz = c( zInd, ALL );
Fill( cz, lambda );
// AHat = []
// =========
Zeros( AHat, 0, n+m+1 );
// b = []
// ======
Zeros( b, 0, 1 );
// G := |-diag(d) A, -d, -I|
// | 0, 0, -I|
// =========================
Zeros( G, 2*m, n+m+1 );
auto G0w = G( IR(0,m), wInd );
auto G0beta = G( IR(0,m), betaInd );
auto G0z = G( IR(0,m), zInd );
auto G1z = G( IR(m,2*m), zInd );
G0w = A; G0w *= -1; DiagonalScale( LEFT, NORMAL, d, G0w );
G0beta = d; G0beta *= -1;
FillDiagonal( G0z, Real(-1) );
FillDiagonal( G1z, Real(-1) );
// h := [-ones(m,1); zeros(m,1)]
// =============================
Zeros( h, 2*m, 1 );
auto h0 = h( IR(0,m), ALL );
Fill( h0, Real(-1) );
// Solve the affine QP
// ===================
DistMatrix<Real> y(g), z(g), s(g);
QP( Q, AHat, G, b, c, h, x, y, z, s, ctrl );
}
void IPM
( const DistSparseMatrix<Real>& A,
const DistMultiVec<Real>& d,
Real lambda,
DistMultiVec<Real>& x,
const qp::affine::Ctrl<Real>& ctrl )
{
EL_DEBUG_CSE
const Int m = A.Height();
const Int n = A.Width();
mpi::Comm comm = A.Comm();
DistSparseMatrix<Real> Q(comm), AHat(comm), G(comm);
DistMultiVec<Real> c(comm), b(comm), h(comm);
auto& dLoc = d.LockedMatrix();
auto& cLoc = c.Matrix();
auto& hLoc = h.Matrix();
// Q := | I 0 0 |
// | 0 0 0 |
// | 0 0 0 |
// ==============
Zeros( Q, n+m+1, n+m+1 );
{
// Count the number of local entries in the top-left I
// ---------------------------------------------------
Int numLocalUpdates = 0;
for( Int iLoc=0; iLoc<Q.LocalHeight(); ++iLoc )
if( Q.GlobalRow(iLoc) < n )
++numLocalUpdates;
else
break;
Q.Reserve( numLocalUpdates );
for( Int iLoc=0; iLoc<Q.LocalHeight(); ++iLoc )
if( Q.GlobalRow(iLoc) < n )
Q.QueueLocalUpdate( iLoc, Q.GlobalRow(iLoc), Real(1) );
Q.ProcessLocalQueues();
}
// c := [0;0;lambda]
// =================
Zeros( c, n+m+1, 1 );
for( Int iLoc=0; iLoc<c.LocalHeight(); ++iLoc )
if( c.GlobalRow(iLoc) > n )
cLoc(iLoc) = lambda;
// AHat = []
// =========
Zeros( AHat, 0, n+m+1 );
// b = []
// ======
Zeros( b, 0, 1 );
// G := |-diag(d) A, -d, -I|
// | 0, 0, -I|
// =========================
Zeros( G, 2*m, n+m+1 );
G.Reserve
( A.NumLocalEntries()+d.LocalHeight()+G.LocalHeight(),
A.NumLocalEntries()+d.LocalHeight() );
for( Int e=0; e<A.NumLocalEntries(); ++e )
{
const Int i = A.Row(e);
const Int j = A.Col(e);
const Int iLoc = A.LocalRow(i);
const Real value = -dLoc(iLoc)*A.Value(e);
G.QueueUpdate( i, j, value );
}
for( Int iLoc=0; iLoc<d.LocalHeight(); ++iLoc )
{
const Int i = d.GlobalRow(iLoc);
G.QueueUpdate( i, n, -dLoc(iLoc) );
}
for( Int iLoc=0; iLoc<G.LocalHeight(); ++iLoc )
{
const Int i = G.GlobalRow(iLoc);
if( i < m )
G.QueueLocalUpdate( iLoc, i+n+1, Real(-1) );
else
G.QueueLocalUpdate( iLoc, (i-m)+n+1, Real(-1) );
}
G.ProcessQueues();
// h := [-ones(m,1); zeros(m,1)]
// =============================
Zeros( h, 2*m, 1 );
for( Int iLoc=0; iLoc<h.LocalHeight(); ++iLoc )
if( h.GlobalRow(iLoc) < m )
hLoc(iLoc) = Real(-1);
else
break;
// Solve the affine QP
// ===================
DistMultiVec<Real> y(comm), z(comm), s(comm);
QP( Q, AHat, G, b, c, h, x, y, z, s, ctrl );
}
void IPM
( const SparseMatrix<Real>& A,
const Matrix<Real>& d,
Real lambda,
Matrix<Real>& x,
const qp::affine::Ctrl<Real>& ctrl )
{
EL_DEBUG_CSE
const Int m = A.Height();
const Int n = A.Width();
const Range<Int> wInd(0,n), betaInd(n,n+1), zInd(n+1,n+m+1);
SparseMatrix<Real> Q, AHat, G;
Matrix<Real> c, b, h;
// Q := | I 0 0 |
// | 0 0 0 |
// | 0 0 0 |
// ==============
Zeros( Q, n+m+1, n+m+1 );
Q.Reserve( n );
for( Int e=0; e<n; ++e )
Q.QueueUpdate( e, e, Real(1) );
Q.ProcessQueues();
// c := [0;0;lambda]
// =================
Zeros( c, n+m+1, 1 );
auto cz = c( zInd, ALL );
Fill( cz, lambda );
// AHat = []
// =========
Zeros( AHat, 0, n+m+1 );
// b = []
// ======
Zeros( b, 0, 1 );
// G := |-diag(d) A, -d, -I|
// | 0, 0, -I|
// =========================
Zeros( G, 2*m, n+m+1 );
const Int numEntriesA = A.NumEntries();
G.Reserve( numEntriesA+3*m );
for( Int e=0; e<numEntriesA; ++e )
G.QueueUpdate( A.Row(e), A.Col(e), -d(A.Row(e))*A.Value(e) );
for( Int e=0; e<m; ++e )
G.QueueUpdate( e, n, -d(e) );
for( Int e=0; e<m; ++e )
{
G.QueueUpdate( e, e+n+1, Real(-1) );
G.QueueUpdate( e+m, e+n+1, Real(-1) );
}
G.ProcessQueues();
// h := [-ones(m,1); zeros(m,1)]
// =============================
Zeros( h, 2*m, 1 );
auto h0 = h( IR(0,m), ALL );
Fill( h0, Real(-1) );
// Solve the affine QP
// ===================
Matrix<Real> y, z, s;
QP( Q, AHat, G, b, c, h, x, y, z, s, ctrl );
}
void IPM
( const AbstractDistMatrix<Real>& APre,
const AbstractDistMatrix<Real>& b,
Real lambda,
AbstractDistMatrix<Real>& xPre,
const qp::affine::Ctrl<Real>& ctrl )
{
EL_DEBUG_CSE
DistMatrixReadProxy<Real,Real,MC,MR> AProx( APre );
const auto& A = AProx.GetLocked();
DistMatrixWriteProxy<Real,Real,MC,MR> xProx( xPre );
auto& x = xProx.Get();
const Int m = A.Height();
const Int n = A.Width();
const Grid& g = A.Grid();
const Range<Int> uInd(0,n), vInd(n,2*n), rInd(2*n,2*n+m);
DistMatrix<Real> Q(g), c(g), AHat(g), G(g), h(g);
// Q := | 0 0 0 |
// | 0 0 0 |
// | 0 0 I |
// ==============
Zeros( Q, 2*n+m, 2*n+m );
auto Qrr = Q( rInd, rInd );
FillDiagonal( Qrr, Real(1) );
// c := lambda*[1;1;0]
// ===================
Zeros( c, 2*n+m, 1 );
auto cuv = c( IR(0,2*n), ALL );
Fill( cuv, lambda );
// \hat A := [A, -A, I]
// ====================
Zeros( AHat, m, 2*n+m );
auto AHatu = AHat( IR(0,m), uInd );
auto AHatv = AHat( IR(0,m), vInd );
auto AHatr = AHat( IR(0,m), rInd );
AHatu = A;
AHatv -= A;
FillDiagonal( AHatr, Real(1) );
// G := | -I 0 0 |
// | 0 -I 0 |
// ================
Zeros( G, 2*n, 2*n+m );
FillDiagonal( G, Real(-1) );
// h := 0
// ======
Zeros( h, 2*n, 1 );
// Solve the affine QP
// ===================
DistMatrix<Real> xHat(g), y(g), z(g), s(g);
QP( Q, AHat, G, b, c, h, xHat, y, z, s, ctrl );
// x := u - v
// ==========
x = xHat( uInd, ALL );
x -= xHat( vInd, ALL );
}
I rk(I f,A r,A a,A w)
{
A z=0,*p=0;
if(w)ND2 else ND1;
{
XA;
C *pp=0,*ap,*wp;
I wt=0,wr=0,wn=0,*wd=0;
I n=0,t=0,i,j,k,d[9],rw,ra,ri,ir,iw=0,ia=0,ii=0,
e=!w&&f==MP(9),h=QP(f)&&f!=MP(71)&&!e;
Q(!QA(r),9);
Q(r->t,6);
Q(r->n<1||r->n>3,8);
ar-=ra=raw(ar,*r->p);
if(!w) mv(d,ad,ra),ir=tr(ra,ad),ad+=ra;
else
{
wt=w->t,wr=w->r,wd=w->d;
wr-=rw=raw(wr,r->p[r->n>1]),ri=r->n>2?r->p[2]:9;
Q(ri<0,9);
if(ri>ra)ri=ra;
if(ri>rw)ri=rw;
mv(d,ad,ra-=ri);
ia=tr(ra,ad),mv(d+ra,wd,rw),iw=tr(rw-=ri,wd);
Q(cm(ad+=ra,wd+=rw,ri),11);
ii=tr(ri,ad),ra+=rw+ri,ir=ia*iw*ii,wn=tr(wr,wd+=ri),ad+=ri;
if(h&&ir>iw&&(f==MP(21)||f==MP(25)||f==MP(26)||f==MP(32)||f==MP(33)))
h=0;
}
an=tr(ar,ad);
if(h)
{
g=0;
aw_c[0]=a->c;
aw_c[1]=w&&w->c;
r=(A)fa(f,gC(at,ar,an,ad,a->n?a->p:0),w?gC(wt,wr,wn,wd,w->n?w->p:0):0);
aw_c[0]=aw_c[1]=1;
if(!r)R 0;
r=un(&r);
mv(d+ra,r->d,j=r->r);
if((j+=ra)>MAXR)R q=13,(I)r;
n=r->n;t=r->t;
if(ir<2) R mv(r->d,d,r->r=j),r->n*=ir,(I)r;
dc(r);
if(g==(I (*)())rsh) R rsh(w?w:a,j,d);
if(!g){h=0;}
else
{
if(at=atOnExit,w) wt=wtOnExit;
if(at!=a->t&&!(a=at?ep_cf(1):ci(1))) R 0;
if(w&&wt!=w->t&&!(w=wt?ep_cf(2):ci(2))) R 0;
OF(i,ir,n);
W(ga(t,j,i,d));
pp=(C*)z->p;
}
}
if(!h)
{
W(ga(Et,ra,ir,d));
*--Y=zr(z),p=(A*)z->p;
}
if(!w)
{
for(ap=(C*)a->p;ir--;ap+=Tt(at,an))
if(h) (*(I(*)(C*,C*,I))g)(pp,ap,an),pp+=Tt(t,n);
else a=gc(at,ar,an,ad,(I*)ap),*p++=e?a:(A)fa(f,(I)a,0);
}
else
{
for(i=0;i<ia;++i)for(j=0;j<iw;++j)
for(k=0;k<ii;++k){
ap=(C*)a->p+Tt(at,(i*ii+k)*an);
wp=(C*)w->p+Tt(wt,(j*ii+k)*wn);
if(h)
{
(*(I(*)(C*,C*,C*,I))g)(pp,ap,wp,n),pp+=Tt(t,n);
if(q==1)*--Y=(I)z,aplus_err(q,(A)Y[1]),++Y;
}
else
{
*p++=(A)fa(f,(I)gc(at,ar,an,ad,(I*)ap),(I)gc(wt,wr,wn,wd,(I*)wp));
}
}
}
if(h)R(I)z;
if(!e)z=(A)dis(r=z),dc(r);
R ++Y,(I)z;
}
}
