void Reservoir::linear_activation(VectorXd &inputs, VectorXd &results) { unsigned int num_elems = (inputs.rows() > inputs.cols()) ? inputs.rows() : inputs.cols(); results.resize(num_elems, 1); for (unsigned int i=0; i<num_elems; i++) { results(i) = inputs(i); } }
void MultivariateFNormalSufficientSparse::set_FM(const VectorXd& FM) { if (FM.rows() != FM_.rows() || FM.cols() != FM_.cols() || FM != FM_){ if (FM.rows() != M_) { IMP_THROW("size mismatch for FM: got " <<FM.rows() << " instead of " << M_, ModelException); } FM_=FM; IMP_LOG(TERSE, "MVNsparse: set FM to new vector" << std::endl); compute_epsilon(); } }
void MultivariateFNormalSufficientSparse::set_Fbar(const VectorXd& Fbar) { if (Fbar.rows() != Fbar_.rows() || Fbar.cols() != Fbar_.cols() || Fbar != Fbar_){ if (Fbar.rows() != M_) { IMP_THROW("size mismatch for Fbar: got " << Fbar.rows() << " instead of " << M_, ModelException); } Fbar_=Fbar; IMP_LOG(TERSE, "MVNsparse: set Fbar to new vector" << std::endl); compute_epsilon(); } }
void MultivariateFNormalSufficient::set_FM(const VectorXd& FM) { if (FM.rows() != FM_.rows() || FM.cols() != FM_.cols() || FM != FM_){ CHECK(FM.rows() == M_, "size mismatch for FM: got " <<FM.rows() << " instead of " << M_); FM_=FM; LOG( "MVN: set FM to new vector" << std::endl); flag_epsilon_ = false; flag_Peps_ = false; } flag_FM_ = true; }
bool LocallyWeightedRegression::updateThetas(const VectorXd &deltaThetas) { if (!initialized_) { printf("ERROR: LWR model not initialized.\n"); return initialized_; } assert(deltaThetas.cols() == thetas_.cols()); assert(deltaThetas.rows() == thetas_.rows()); thetas_ += deltaThetas; return true; }
void MultivariateFNormalSufficient::set_FM(const VectorXd& FM) { if (FM.rows() != FM_.rows() || FM.cols() != FM_.cols() || FM != FM_){ if (FM.rows() != M_) { IMP_THROW("size mismatch for FM: got " <<FM.rows() << " instead of " << M_, ModelException); } FM_=FM; IMP_LOG(TERSE, "MVN: set FM to new vector" << std::endl); flag_epsilon_ = false; flag_Peps_ = false; } flag_FM_ = true; }
bool LocallyWeightedRegression::getThetas(VectorXd &thetas) { if (!initialized_) { printf("ERROR: LWR model not initialized.\n"); return initialized_; } assert(thetas.cols() == thetas_.cols()); assert(thetas.rows() == thetas_.rows()); thetas = thetas_; return true; }
void MultivariateFNormalSufficient::set_Fbar(const VectorXd& Fbar) { if (Fbar.rows() != Fbar_.rows() || Fbar.cols() != Fbar_.cols() || Fbar != Fbar_){ CHECK(Fbar.rows() == M_, "size mismatch for Fbar: got " << Fbar.rows() << " instead of " << M_); Fbar_=Fbar; LOG( "MVN: set Fbar to new vector" << std::endl); flag_epsilon_ = false; flag_W_ = false; flag_PW_ = false; flag_Peps_ = false; } flag_Fbar_ = true; }
std::tuple<double, VectorXd, SparseMatrix> kantorovich_2(const Density_2 &pl, const MatrixXd &X, const VectorXd &w) { size_t N = X.rows(); assert(X.cols() == 2); assert(w.cols() == 1); assert(w.rows() == N); VectorXd g(N); SparseMatrix h; double res = MA::kantorovich(pl._t, pl._functions, X, w, g, h); return std::make_tuple(res, g, h); }
void MultivariateFNormalSufficient::set_Fbar(const VectorXd& Fbar) { if (Fbar.rows() != Fbar_.rows() || Fbar.cols() != Fbar_.cols() || Fbar != Fbar_){ if (Fbar.rows() != M_) { IMP_THROW("size mismatch for Fbar: got " << Fbar.rows() << " instead of " << M_, ModelException); } Fbar_=Fbar; IMP_LOG(TERSE, "MVN: set Fbar to new vector" << std::endl); flag_epsilon_ = false; flag_W_ = false; flag_PW_ = false; flag_Peps_ = false; } flag_Fbar_ = true; }
std::tuple<VectorXd, MatrixXd, MatrixXd> moments_2(const Density_2 &pl, const MatrixXd &X, const VectorXd &w) { size_t N = X.rows(); assert(X.cols() == 2); assert(w.cols() == 1); assert(w.rows() == N); // create some room for return values: masses, centroids and inertia VectorXd m(N); MatrixXd c(N, 2); MatrixXd I(N, 3); MA::second_moment(pl._t, pl._functions, X, w, m, c, I); return std::make_tuple(m, c, I); }
Density_2(const MatrixXd& X, const VectorXd& f, const MatrixXi& tri) : gen(clock()) { size_t N = X.rows(); assert(X.cols() == 2); assert(f.cols() == 1); assert(f.rows() == N); assert(tri.cols() == 3); CGAL::Triangulation_incremental_builder_2<T> builder(_t); builder.begin_triangulation(); // add vertices std::vector<T::Vertex_handle> vertices(N); for (size_t i = 0; i < N; ++i) { Point p(X(i,0),X(i,1)); vertices[i] = builder.add_vertex(Point(X(i,0), X(i,1))); vertices[i]->info() = i; } // add faces size_t Nt = tri.rows(); for (size_t i = 0; i < Nt; ++i) { int a = tri(i,0), b = tri(i,1), c = tri(i,2); builder.add_face(vertices[a], vertices[b], vertices[c]); } builder.end_triangulation(); // compute functions for (T::Finite_faces_iterator it = _t.finite_faces_begin (); it != _t.finite_faces_end(); ++it) { size_t a = it->vertex(0)->info(); size_t b = it->vertex(1)->info(); size_t c = it->vertex(2)->info(); _functions[it] = Function(vertices[a]->point(), f[a], vertices[b]->point(), f[b], vertices[c]->point(), f[c]); } }
void python_to_delaunay_2(const MatrixXd& X, const VectorXd& w, RT &dt) { size_t N = X.rows(); assert(X.cols() == 2); assert(w.cols() == 1); assert(w.rows() == N); // insert points with indices in the regular triangulation std::vector<std::pair<Weighted_point,size_t> > Xw(N); for (size_t i = 0; i < N; ++i) { Xw[i] = std::make_pair(Weighted_point(Point(X(i,0), X(i,1)), w(i)), i); } dt.clear(); dt.insert(Xw.begin(), Xw.end()); dt.infinite_vertex()->info() = -1; }
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { // This is useful for debugging whether Matlab is caching the mex binary //mexPrintf("%s %s\n",__TIME__,__DATE__); igl::MexStream mout; std::streambuf *outbuf = std::cout.rdbuf(&mout); using namespace std; using namespace Eigen; using namespace igl; MatrixXd V,P,N; VectorXd S; MatrixXi F; int num_samples; parse_rhs(nrhs,prhs,V,F,P,N,num_samples); // Prepare left-hand side nlhs = 1; //read_triangle_mesh("../shared/cheburashka.off",V,F); //P = V; //per_vertex_normals(V,F,N); ambient_occlusion(V,F,P,N,num_samples,S); //MatlabWorkspace mw; //mw.save(V,"V"); //mw.save(P,"P"); //mw.save(N,"N"); //mw.save_index(F,"F"); //mw.save(S,"S"); //mw.write("out.mat"); plhs[0] = mxCreateDoubleMatrix(S.rows(),S.cols(), mxREAL); copy(S.data(),S.data()+S.size(),mxGetPr(plhs[0])); // Restore the std stream buffer Important! std::cout.rdbuf(outbuf); }
// template <typename tA, typename tB, typename tC, typename tD, typename tE, // typename tF, typename tG> // int fastQPThatTakesQinv(vector< MatrixBase<tA>* > QinvblkDiag, const // MatrixBase<tB>& f, const MatrixBase<tC>& Aeq, const MatrixBase<tD>& beq, // const MatrixBase<tE>& Ain, const MatrixBase<tF>& bin, set<int>& active, // MatrixBase<tG>& x) int fastQPThatTakesQinv(vector<MatrixXd*> QinvblkDiag, const VectorXd& f, const MatrixXd& Aeq, const VectorXd& beq, const MatrixXd& Ain, const VectorXd& bin, set<int>& active, VectorXd& x) { int i, d; int iterCnt = 0; int M_in = bin.size(); int M = Aeq.rows(); int N = Aeq.cols(); if (f.rows() != N) { cerr << "size of f (" << f.rows() << " by " << f.cols() << ") doesn't match cols of Aeq (" << Aeq.rows() << " by " << Aeq.cols() << ")" << endl; return 2; } if (beq.rows() != M) { cerr << "size of beq doesn't match rows of Aeq" << endl; return 2; } if (Ain.cols() != N) { cerr << "cols of Ain doesn't match cols of Aeq" << endl; return 2; }; if (bin.rows() != Ain.rows()) { cerr << "bin rows doesn't match Ain rows" << endl; return 2; }; if (x.rows() != N) { cerr << "x doesn't match Aeq" << endl; return 2; } int n_active = active.size(); MatrixXd Aact = MatrixXd(n_active, N); VectorXd bact = VectorXd(n_active); MatrixXd QinvAteq(N, M); VectorXd minusQinvf(N); // calculate a bunch of stuff that is constant during each iteration int startrow = 0; // for (typename vector< MatrixBase<tA>* >::iterator // iterQinv=QinvblkDiag.begin(); iterQinv!=QinvblkDiag.end(); iterQinv++) { // MatrixBase<tA> *thisQinv = *iterQinv; for (vector<MatrixXd*>::iterator iterQinv = QinvblkDiag.begin(); iterQinv != QinvblkDiag.end(); iterQinv++) { MatrixXd* thisQinv = *iterQinv; int numRow = thisQinv->rows(); int numCol = thisQinv->cols(); if (numRow == 1 || numCol == 1) { // it's a vector d = numRow * numCol; if (M > 0) QinvAteq.block(startrow, 0, d, M) = thisQinv->asDiagonal() * Aeq.block(0, startrow, M, d) .transpose(); // Aeq.transpoODse().block(startrow, 0, d, N) minusQinvf.segment(startrow, d) = -thisQinv->cwiseProduct(f.segment(startrow, d)); startrow = startrow + d; } else { // potentially dense matrix d = numRow; if (numRow != numCol) { cerr << "Q is not square! " << numRow << "x" << numCol << "\n"; return -2; } if (M > 0) QinvAteq.block(startrow, 0, d, M) = thisQinv->operator*( Aeq.block(0, startrow, M, d) .transpose()); // Aeq.transpose().block(startrow, 0, d, N) minusQinvf.segment(startrow, d) = -thisQinv->operator*(f.segment(startrow, d)); startrow = startrow + d; } if (startrow > N) { cerr << "Q is too big!" << endl; return -2; } } if (startrow != N) { cerr << "Q is the wrong size. Got " << startrow << "by" << startrow << " but needed " << N << "by" << N << endl; return -2; } MatrixXd A; VectorXd b; MatrixXd QinvAt; VectorXd lam, lamIneq; VectorXd violated(M_in); VectorXd violation; while (1) { iterCnt++; n_active = active.size(); Aact.resize(n_active, N); bact.resize(n_active); i = 0; for (set<int>::iterator iter = active.begin(); iter != active.end(); iter++) { if (*iter < 0 || *iter >= Ain.rows()) { return -3; // active set is invalid. exit quietly, because this is // expected behavior in normal operation (e.g. it means I // should immediately kick out to gurobi) } Aact.row(i) = Ain.row(*iter); bact(i++) = bin(*iter); } A.resize(Aeq.rows() + Aact.rows(), N); b.resize(beq.size() + bact.size()); A << Aeq, Aact; b << beq, bact; if (A.rows() > 0) { // Solve H * [x;lam] = [-f;b] using Schur complements, H = [Q, At';A, 0]; QinvAt.resize(QinvAteq.rows(), QinvAteq.cols() + Aact.rows()); if (n_active > 0) { int startrow = 0; for (vector<MatrixXd*>::iterator iterQinv = QinvblkDiag.begin(); iterQinv != QinvblkDiag.end(); iterQinv++) { MatrixXd* thisQinv = (*iterQinv); d = thisQinv->rows(); int numCol = thisQinv->cols(); if (numCol == 1) { // it's a vector QinvAt.block(startrow, 0, d, M + n_active) << QinvAteq.block(startrow, 0, d, M), thisQinv->asDiagonal() * Aact.block(0, startrow, n_active, d).transpose(); } else { // it's a matrix QinvAt.block(startrow, 0, d, M + n_active) << QinvAteq.block(startrow, 0, d, M), thisQinv->operator*( Aact.block(0, startrow, n_active, d).transpose()); } startrow = startrow + d; } } else { QinvAt = QinvAteq; } lam.resize(QinvAt.cols()); lam = -(A * QinvAt).ldlt().solve(b + (f.transpose() * QinvAt).transpose()); x = minusQinvf - QinvAt * lam; lamIneq = lam.tail(lam.size() - M); } else { x = minusQinvf; lamIneq.resize(0); } if (Ain.rows() == 0) { active.clear(); break; } set<int> new_active; violation = Ain * x - bin; for (i = 0; i < M_in; i++) if (violation(i) >= 1e-6) new_active.insert(i); bool all_pos_mults = true; for (i = 0; i < n_active; i++) { if (lamIneq(i) < 0) { all_pos_mults = false; break; } } if (new_active.empty() && all_pos_mults) { // existing active was AOK break; } i = 0; set<int>::iterator iter = active.begin(), tmp; while (iter != active.end()) { // to accomodating inloop erase tmp = iter++; if (lamIneq(i++) < 0) { active.erase(tmp); } } active.insert(new_active.begin(), new_active.end()); if (iterCnt > MAX_ITER) { // Default to calling this method // cout << "FastQP max iter reached." << endl; // mexErrMsgIdAndTxt("Drake:approximateIKmex:Error", "Max iter // reached. Problem is likely infeasible"); return -1; } } return iterCnt; }
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { // This is useful for debugging whether Matlab is caching the mex binary //mexPrintf("%s %s\n",__TIME__,__DATE__); igl::matlab::MexStream mout; std::streambuf *outbuf = std::cout.rdbuf(&mout); using namespace std; using namespace Eigen; using namespace igl; using namespace igl::matlab; MatrixXd P,V,C; VectorXi I; VectorXd sqrD; MatrixXi F; if(nrhs < 3) { mexErrMsgTxt("nrhs < 3"); } parse_rhs_double(prhs,P); parse_rhs_double(prhs+1,V); parse_rhs_index(prhs+2,F); mexErrMsgTxt(P.cols()==3 || P.cols()==2,"P must be #P by (3|2)"); mexErrMsgTxt(V.cols()==3 || V.cols()==2,"V must be #V by (3|2)"); mexErrMsgTxt(V.cols()==P.cols(),"dim(V) must be dim(P)"); mexErrMsgTxt(F.cols()==3 || F.cols()==2 || F.cols()==1,"F must be #F by (3|2|1)"); point_mesh_squared_distance(P,V,F,sqrD,I,C); // Prepare left-hand side switch(nlhs) { case 3: { // Treat indices as reals plhs[2] = mxCreateDoubleMatrix(C.rows(),C.cols(), mxREAL); double * Cp = mxGetPr(plhs[2]); copy(&C.data()[0],&C.data()[0]+C.size(),Cp); // Fallthrough } case 2: { // Treat indices as reals plhs[1] = mxCreateDoubleMatrix(I.rows(),I.cols(), mxREAL); double * Ip = mxGetPr(plhs[1]); VectorXd Id = (I.cast<double>().array()+1).matrix(); copy(&Id.data()[0],&Id.data()[0]+Id.size(),Ip); // Fallthrough } case 1: { plhs[0] = mxCreateDoubleMatrix(sqrD.rows(),sqrD.cols(), mxREAL); double * sqrDp = mxGetPr(plhs[0]); copy(&sqrD.data()[0],&sqrD.data()[0]+sqrD.size(),sqrDp); break; } default:break; } // Restore the std stream buffer Important! std::cout.rdbuf(outbuf); }
//#define IGL_LINPROG_VERBOSE IGL_INLINE bool igl::linprog( const Eigen::VectorXd & c, const Eigen::MatrixXd & _A, const Eigen::VectorXd & b, const int k, Eigen::VectorXd & x) { // This is a very literal translation of // http://www.mathworks.com/matlabcentral/fileexchange/2166-introduction-to-linear-algebra/content/strang/linprog.m using namespace Eigen; using namespace std; bool success = true; // number of constraints const int m = _A.rows(); // number of original variables const int n = _A.cols(); // number of iterations int it = 0; // maximum number of iterations //const int MAXIT = 10*m; const int MAXIT = 100*m; // residual tolerance const double tol = 1e-10; const auto & sign = [](const Eigen::VectorXd & B) -> Eigen::VectorXd { Eigen::VectorXd Bsign(B.size()); for(int i = 0;i<B.size();i++) { Bsign(i) = B(i)>0?1:(B(i)<0?-1:0); } return Bsign; }; // initial (inverse) basis matrix VectorXd Dv = sign(sign(b).array()+0.5); Dv.head(k).setConstant(1.); MatrixXd D = Dv.asDiagonal(); // Incorporate slack variables MatrixXd A(_A.rows(),_A.cols()+D.cols()); A<<_A,D; // Initial basis VectorXi B = igl::colon<int>(n,n+m-1); // non-basis, may turn out that vector<> would be better here VectorXi N = igl::colon<int>(0,n-1); int j; double bmin = b.minCoeff(&j); int phase; VectorXd xb; VectorXd s; VectorXi J; if(k>0 && bmin<0) { phase = 1; xb = VectorXd::Ones(m); // super cost s.resize(n+m+1); s<<VectorXd::Zero(n+k),VectorXd::Ones(m-k+1); N.resize(n+1); N<<igl::colon<int>(0,n-1),B(j); J.resize(B.size()-1); // [0 1 2 3 4] // ^ // [0 1] // [3 4] J.head(j) = B.head(j); J.tail(B.size()-j-1) = B.tail(B.size()-j-1); B(j) = n+m; MatrixXd AJ; igl::slice(A,J,2,AJ); const VectorXd a = b - AJ.rowwise().sum(); { MatrixXd old_A = A; A.resize(A.rows(),A.cols()+a.cols()); A<<old_A,a; } D.col(j) = -a/a(j); D(j,j) = 1./a(j); }else if(k==m) { phase = 2; xb = b; s.resize(c.size()+m); // cost function s<<c,VectorXd::Zero(m); }else //k = 0 or bmin >=0 { phase = 1; xb = b.array().abs(); s.resize(n+m); // super cost s<<VectorXd::Zero(n+k),VectorXd::Ones(m-k); } while(phase<3) { double df = -1; int t = std::numeric_limits<int>::max(); // Lagrange mutipliers fro Ax=b VectorXd yb = D.transpose() * igl::slice(s,B); while(true) { if(MAXIT>0 && it>=MAXIT) { #ifdef IGL_LINPROG_VERBOSE cerr<<"linprog: warning! maximum iterations without convergence."<<endl; #endif success = false; break; } // no freedom for minimization if(N.size() == 0) { break; } // reduced costs VectorXd sN = igl::slice(s,N); MatrixXd AN = igl::slice(A,N,2); VectorXd r = sN - AN.transpose() * yb; int q; // determine new basic variable double rmin = r.minCoeff(&q); // optimal! infinity norm if(rmin>=-tol*(sN.array().abs().maxCoeff()+1)) { break; } // increment iteration count it++; // apply Bland's rule to avoid cycling if(df>=0) { if(MAXIT == -1) { #ifdef IGL_LINPROG_VERBOSE cerr<<"linprog: warning! degenerate vertex"<<endl; #endif success = false; } igl::find((r.array()<0).eval(),J); double Nq = igl::slice(N,J).minCoeff(); // again seems like q is assumed to be a scalar though matlab code // could produce a vector for multiple matches (N.array()==Nq).cast<int>().maxCoeff(&q); } VectorXd d = D*A.col(N(q)); VectorXi I; igl::find((d.array()>tol).eval(),I); if(I.size() == 0) { #ifdef IGL_LINPROG_VERBOSE cerr<<"linprog: warning! solution is unbounded"<<endl; #endif // This seems dubious: it=-it; success = false; break; } VectorXd xbd = igl::slice(xb,I).array()/igl::slice(d,I).array(); // new use of r int p; { double r; r = xbd.minCoeff(&p); p = I(p); // apply Bland's rule to avoid cycling if(df>=0) { igl::find((xbd.array()==r).eval(),J); double Bp = igl::slice(B,igl::slice(I,J)).minCoeff(); // idiotic way of finding index in B of Bp // code down the line seems to assume p is a scalar though the matlab // code could find a vector of matches) (B.array()==Bp).cast<int>().maxCoeff(&p); } // update x xb -= r*d; xb(p) = r; // change in f df = r*rmin; } // row vector RowVectorXd v = D.row(p)/d(p); yb += v.transpose() * (s(N(q)) - d.transpose()*igl::slice(s,B)); d(p)-=1; // update inverse basis matrix D = D - d*v; t = B(p); B(p) = N(q); if(t>(n+k-1)) { // remove qth entry from N VectorXi old_N = N; N.resize(N.size()-1); N.head(q) = old_N.head(q); N.head(q) = old_N.head(q); N.tail(old_N.size()-q-1) = old_N.tail(old_N.size()-q-1); }else { N(q) = t; } } // iterative refinement xb = (xb+D*(b-igl::slice(A,B,2)*xb)).eval(); // must be due to rounding VectorXi I; igl::find((xb.array()<0).eval(),I); if(I.size()>0) { // so correct VectorXd Z = VectorXd::Zero(I.size(),1); igl::slice_into(Z,I,xb); } // B, xb,n,m,res=A(:,B)*xb-b if(phase == 2 || it<0) { break; } if(xb.transpose()*igl::slice(s,B) > tol) { it = -it; #ifdef IGL_LINPROG_VERBOSE cerr<<"linprog: warning, no feasible solution"<<endl; #endif success = false; break; } // re-initialize for Phase 2 phase = phase+1; s*=1e6*c.array().abs().maxCoeff(); s.head(n) = c; } x.resize(std::max(B.maxCoeff()+1,n)); igl::slice_into(xb,B,x); x = x.head(n).eval(); return success; }
//TBD: J -> adfda, e should take data as input too. //PatchFit::lm(MatrixXd (*J)(VectorXd) ,VectorXd (*e)(VectorXd), VectorXd a_Init) VectorXd PatchFit::lm(VectorXd a_Init) { // Options bool normalize (false); double l_Init = 0.001; double nu = 10; VectorXd a (a_Init.rows(), a_Init.cols()); a = a_Init; double l; l = l_Init; int i=0; //iteration //bool cc = false; // whether the last iteration committed a change to a double r; //residual //calculate initial error and residual if (normalize) { a = a/a.norm(); //cc = true; } VectorXd ee; ee = PatchFit::parab(cloud_vec,a); r = ee.norm(); //double r_Init = r; double dr = 0.0; MatrixXd jj; double lastr; // define matrices MatrixXd jtj, jtj_diag, aa; VectorXd b, da, lasta, lastee; while (1) { // terminating due to 0 residual at iteration i if (fabs(r) < EPS) break; // terminating due to minad at iteration i //TBD // terminating due to minrd at iteration i if ((i>0) && (this->minrd_>0.0) && (dr<0.0) && (fabs(dr)<this->minrd_*lastr)) break; // terminating due to minar at iteration i //TBD // terminating due to minrr at iteration i //TBD // terminating due to minada at iteration i //TBD // terminating due to minrda at iteration i //TBD // terminating due to max iterations if (i==this->maxi_) break; i = i+1; // update Jacobian if necessary if ((i==1)||(dr<0.0)) jj = PatchFit::parab_dfda(a); // calculate da jtj = jj.adjoint()*jj; jtj_diag = jtj.diagonal().asDiagonal(); aa = jtj + (l*jtj_diag); b = -jj.adjoint()*ee; // solve LLS system da = lls(aa,b); // update a (will back out the update if residual increased) lasta = a; a = a+da; if (normalize) a = a/a.norm(); // update error and residual lastr = r; lastee = ee; ee = PatchFit::parab(cloud_vec,a); r = ee.norm(); dr = r-lastr; if (dr<0) // converging { l = l/nu; //cc = true; } else // diverging { l = l*nu; a = lasta; ee = lastee; r = lastr; //cc = false; } } if (normalize && (i==0)) a = a/a.norm(); return (a); }