static PyObject * complex_add(PyObject *v, PyObject *w) { Py_complex result; Py_complex a, b; TO_COMPLEX(v, a); TO_COMPLEX(w, b); PyFPE_START_PROTECT("complex_add", return 0) result = c_sum(a, b); PyFPE_END_PROTECT(result) return PyComplex_FromCComplex(result); }
unsigned int open_write_close(int int_fifo, char* char_fifo, char* path){ char buffer[MAX_BUFFER]; ssize_t size; sprintf(buffer, "%d\n", c_sum(path)); if ( (int_fifo=open(char_fifo, O_WRONLY)) < 0 ){ perror("fifo open"); return EXIT_FAILURE; } size = write(int_fifo, buffer, sizeof(char)*(strlen(buffer)+1)); if( size != (strlen(buffer)+1) ){ printf("Can't write all string to FIFO\n"); return EXIT_FAILURE; } close(int_fifo); return EXIT_SUCCESS; }
void SFMViewer::update(std::vector<cv::Point3d> pcld, std::vector<cv::Vec3b> pcldrgb, std::vector<cv::Point3d> pcld_alternate, std::vector<cv::Vec3b> pcldrgb_alternate, std::vector<cv::Matx34d> cameras) { m_pcld = pcld; m_pcldrgb = pcldrgb; m_cameras = cameras; //get the scale of the result cloud using PCA { cv::Mat_<double> cldm(pcld.size(), 3); for (unsigned int i = 0; i < pcld.size(); i++) { cldm.row(i)(0) = pcld[i].x; cldm.row(i)(1) = pcld[i].y; cldm.row(i)(2) = pcld[i].z; } cv::Mat_<double> mean; //cv::reduce(cldm,mean,0,CV_REDUCE_AVG); cv::PCA pca(cldm, mean, CV_PCA_DATA_AS_ROW); scale_cameras_down = 1.0 / (3.0 * sqrt(pca.eigenvalues.at<double> (0))); // std::cout << "emean " << mean << std::endl; // m_global_transform = Eigen::Translation<double,3>(-Eigen::Map<Eigen::Vector3d>(mean[0])); } //compute transformation to place cameras in world m_cameras_transforms.resize(m_cameras.size()); Eigen::Vector3d c_sum(0,0,0); for (int i = 0; i < m_cameras.size(); ++i) { Eigen::Matrix<double, 3, 4> P = Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor> >(m_cameras[i].val); Eigen::Matrix3d R = P.block(0, 0, 3, 3); Eigen::Vector3d t = P.block(0, 3, 3, 1); Eigen::Vector3d c = -R.transpose() * t; c_sum += c; m_cameras_transforms[i] = Eigen::Translation<double, 3>(c) * Eigen::Quaterniond(R) * Eigen::UniformScaling<double>(scale_cameras_down) ; } m_global_transform = Eigen::Translation<double,3>(-c_sum / (double)(m_cameras.size())); // m_global_transform = m_cameras_transforms[0].inverse(); updateGL(); }
static Py_complex c_atanh(Py_complex x) { return c_prod(c_half,c_log(c_quot(c_sum(c_one,x),c_diff(c_one,x)))); }
static Py_complex c_acos(Py_complex x) { return c_neg(c_prodi(c_log(c_sum(x,c_prod(c_i, c_sqrt(c_diff(c_one,c_prod(x,x)))))))); }
/* * function [lp]=gaussmixp(y,m,v,w) * y = cat(1, testSamples(1).mfcc{:}); * m = gmm.M; * v = gmm.V; * w = gmm.W; */ void gaussmixp(const real_T y[2004], const real_T m[108], const real_T v[108], const real_T w[9], real_T lp[167]) { real_T b[108]; real_T dv0[108]; real_T b_b[9]; real_T lvm[9]; int32_T ix; real_T mx[167]; real_T kk[1503]; real_T km[1503]; static real_T b_y[18036]; int32_T iy; real_T dv1[18036]; real_T x[1503]; int32_T i; int32_T ixstart; int32_T ixstop; real_T mtmp; int32_T b_ix; boolean_T exitg1; real_T ps[167]; /* GAUSSMIXP calculate probability densities from a Gaussian mixture model */ /* */ /* Inputs: n data values, k mixtures, p parameters, q data vector size */ /* */ /* Y(n,q) = input data */ /* M(k,p) = mixture means for x(p) */ /* V(k,p) or V(p,p,k) variances (diagonal or full) */ /* W(k,1) = weights */ /* A(q,p), B(q) = transformation: y=x*a'+b' (where y and x are row vectors) */ /* if A is omitted, it is assumed to be the first q rows of the */ /* identity matrix. B defaults to zero. */ /* Note that most commonly, q=p and A and B are omitted entirely. */ /* */ /* Outputs */ /* */ /* LP(n,1) = log probability of each data point */ /* RP(n,k) = relative probability of each mixture */ /* KH(n,1) = highest probability mixture */ /* KP(n,1) = relative probability of highest probability mixture */ /* Copyright (C) Mike Brookes 2000-2009 */ /* Version: $Id: gaussmixp.m,v 1.3 2009/04/08 07:51:21 dmb Exp $ */ /* */ /* VOICEBOX is a MATLAB toolbox for speech processing. */ /* Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html */ /* */ /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */ /* This program is free software; you can redistribute it and/or modify */ /* it under the terms of the GNU General Public License as published by */ /* the Free Software Foundation; either version 2 of the License, or */ /* (at your option) any later version. */ /* */ /* This program is distributed in the hope that it will be useful, */ /* but WITHOUT ANY WARRANTY; without even the implied warranty of */ /* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the */ /* GNU General Public License for more details. */ /* */ /* You can obtain a copy of the GNU General Public License from */ /* http://www.gnu.org/copyleft/gpl.html or by writing to */ /* Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA. */ /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */ /* 'gaussmixp:49' [n,q]=size(y); */ /* 'gaussmixp:50' [k,p]=size(m); */ /* 'gaussmixp:51' memsize=voicebox('memsize'); */ voicebox(); /* set memory size to use */ /* 'gaussmixp:53' lp=zeros(n,1); */ memset((void *)&lp[0], 0, 167U * sizeof(real_T)); /* 'gaussmixp:54' wk=ones(k,1); */ /* 'gaussmixp:56' vi=-0.5*v.^(-1); */ power(v, b); /* data-independent scale factor in exponent */ /* 'gaussmixp:57' lvm=log(w)-0.5*sum(log(v),2); */ memcpy((void *)&dv0[0], (void *)&v[0], 108U * sizeof(real_T)); c_log(dv0); sum(dv0, b_b); memcpy((void *)&lvm[0], (void *)&w[0], 9U * sizeof(real_T)); b_log(lvm); for (ix = 0; ix < 9; ix++) { lvm[ix] -= 0.5 * b_b[ix]; } /* log of external scale factor (excluding -0.5*q*log(2pi) term) */ /* 'gaussmixp:58' ii=1:n; */ /* 'gaussmixp:59' wnj=ones(1,n); */ /* 'gaussmixp:60' kk=repmat(ii,k,1); */ for (ix = 0; ix < 167; ix++) { mx[ix] = 1.0 + (real_T)ix; } repmat(mx, kk); /* 'gaussmixp:61' km=repmat(1:k,1,n); */ for (ix = 0; ix < 9; ix++) { b_b[ix] = 1.0 + (real_T)ix; } b_repmat(b_b, km); /* 'gaussmixp:62' py=reshape(sum((y(kk(:),:)-m(km(:),:)).^2.*vi(km(:),:),2),k,n)+lvm(:,wnj); */ for (ix = 0; ix < 12; ix++) { for (iy = 0; iy < 1503; iy++) { b_y[iy + 1503 * ix] = y[((int32_T)kk[iy] + 167 * ix) - 1] - m[((int32_T) km[iy] + 9 * ix) - 1]; } } b_power(b_y, dv1); for (ix = 0; ix < 12; ix++) { for (iy = 0; iy < 1503; iy++) { b_y[iy + 1503 * ix] = dv1[iy + 1503 * ix] * (-0.5 * b[((int32_T)km[iy] + 9 * ix) - 1]); } } b_sum(b_y, x); memcpy((void *)&kk[0], (void *)&x[0], 1503U * sizeof(real_T)); for (ix = 0; ix < 167; ix++) { for (iy = 0; iy < 9; iy++) { km[iy + 9 * ix] = kk[iy + 9 * ix] + lvm[iy]; } } /* 'gaussmixp:63' mx=max(py,[],1); */ ix = -8; iy = -1; for (i = 0; i < 167; i++) { ix += 9; ixstart = ix; ixstop = ix + 8; mtmp = km[ix - 1]; if (rtIsNaN(km[ix - 1])) { b_ix = ix; exitg1 = 0U; while ((exitg1 == 0U) && (b_ix + 1 <= ixstop)) { ixstart = b_ix + 1; if (!rtIsNaN(km[b_ix])) { mtmp = km[b_ix]; exitg1 = 1U; } else { b_ix++; } } } if (ixstart < ixstop) { while (ixstart + 1 <= ixstop) { if (km[ixstart] > mtmp) { mtmp = km[ixstart]; } ixstart++; } } iy++; mx[iy] = mtmp; } /* find normalizing factor for each data point to prevent underflow when using exp() */ /* 'gaussmixp:64' px=exp(py-mx(wk,:)); */ for (ix = 0; ix < 167; ix++) { for (iy = 0; iy < 9; iy++) { kk[iy + 9 * ix] = km[iy + 9 * ix] - mx[ix]; } } b_exp(kk); /* find normalized probability of each mixture for each datapoint */ /* 'gaussmixp:65' ps=sum(px,1); */ c_sum(kk, ps); /* total normalized likelihood of each data point */ /* 'gaussmixp:66' lp(ii)=log(ps)+mx; */ d_log(ps); for (ix = 0; ix < 167; ix++) { /* 'gaussmixp:67' lp=lp-0.5*q*log(2*pi); */ lp[ix] = (ps[ix] + mx[ix]) - 11.027262398456072; } }
/* Function Definitions */ static void b_euclid(sammonStackData *SD, const real_T x[2000], const real_T y [2000], real_T d[1000000]) { real_T b_y[2000]; real_T c_y[2000]; int32_T k; real_T b[2000]; int32_T i2; real_T alpha1; real_T beta1; char_T TRANSB; char_T TRANSA; ptrdiff_t m_t; ptrdiff_t n_t; ptrdiff_t k_t; ptrdiff_t lda_t; ptrdiff_t ldb_t; ptrdiff_t ldc_t; double * alpha1_t; double * Aia0_t; double * Bib0_t; double * beta1_t; double * Cic0_t; real_T dv4[1000]; real_T dv5[1000]; /* all done */ emlrtPushRtStackR2012b(&h_emlrtRSI, emlrtRootTLSGlobal); for (k = 0; k < 2000; k++) { b_y[k] = x[k] * x[k]; c_y[k] = y[k] * y[k]; } for (k = 0; k < 1000; k++) { for (i2 = 0; i2 < 2; i2++) { b[i2 + (k << 1)] = y[k + 1000 * i2]; } } emlrtPushRtStackR2012b(&i_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&j_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&k_emlrtRSI, emlrtRootTLSGlobal); alpha1 = 1.0; beta1 = 0.0; TRANSB = 'N'; TRANSA = 'N'; memset(&SD->u1.f0.y[0], 0, 1000000U * sizeof(real_T)); emlrtPushRtStackR2012b(&l_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal); m_t = (ptrdiff_t)(1000); emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal); emlrtPopRtStackR2012b(&l_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&m_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal); n_t = (ptrdiff_t)(1000); emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal); emlrtPopRtStackR2012b(&m_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&n_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal); k_t = (ptrdiff_t)(2); emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal); emlrtPopRtStackR2012b(&n_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&o_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal); lda_t = (ptrdiff_t)(1000); emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal); emlrtPopRtStackR2012b(&o_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&p_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal); ldb_t = (ptrdiff_t)(2); emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal); emlrtPopRtStackR2012b(&p_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&q_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal); ldc_t = (ptrdiff_t)(1000); emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal); emlrtPopRtStackR2012b(&q_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&r_emlrtRSI, emlrtRootTLSGlobal); alpha1_t = (double *)(&alpha1); emlrtPopRtStackR2012b(&r_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&s_emlrtRSI, emlrtRootTLSGlobal); Aia0_t = (double *)(&x[0]); emlrtPopRtStackR2012b(&s_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&t_emlrtRSI, emlrtRootTLSGlobal); Bib0_t = (double *)(&b[0]); emlrtPopRtStackR2012b(&t_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&u_emlrtRSI, emlrtRootTLSGlobal); beta1_t = (double *)(&beta1); emlrtPopRtStackR2012b(&u_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&v_emlrtRSI, emlrtRootTLSGlobal); Cic0_t = (double *)(&SD->u1.f0.y[0]); emlrtPopRtStackR2012b(&v_emlrtRSI, emlrtRootTLSGlobal); emlrtPushRtStackR2012b(&w_emlrtRSI, emlrtRootTLSGlobal); dgemm(&TRANSA, &TRANSB, &m_t, &n_t, &k_t, alpha1_t, Aia0_t, &lda_t, Bib0_t, &ldb_t, beta1_t, Cic0_t, &ldc_t); emlrtPopRtStackR2012b(&w_emlrtRSI, emlrtRootTLSGlobal); emlrtPopRtStackR2012b(&k_emlrtRSI, emlrtRootTLSGlobal); emlrtPopRtStackR2012b(&j_emlrtRSI, emlrtRootTLSGlobal); emlrtPopRtStackR2012b(&i_emlrtRSI, emlrtRootTLSGlobal); c_sum(b_y, dv4); c_sum(c_y, dv5); for (k = 0; k < 1000; k++) { for (i2 = 0; i2 < 1000; i2++) { SD->u1.f0.dv6[k + 1000 * i2] = dv4[k]; SD->u1.f0.dv7[k + 1000 * i2] = dv5[i2]; } } for (k = 0; k < 1000; k++) { for (i2 = 0; i2 < 1000; i2++) { d[i2 + 1000 * k] = (SD->u1.f0.dv6[i2 + 1000 * k] + SD->u1.f0.dv7[i2 + 1000 * k]) - 2.0 * SD->u1.f0.y[i2 + 1000 * k]; } } b_sqrt(d); emlrtPopRtStackR2012b(&h_emlrtRSI, emlrtRootTLSGlobal); }
void sammon(const emxArray_real_T *x, emxArray_real_T *y) { emxArray_real_T *D; emxArray_real_T *delta; int32_T i0; int32_T b_D; int32_T br; emxArray_real_T *Dinv; emxArray_real_T *d; emxArray_real_T *dinv; emxArray_real_T *c_D; emxArray_real_T *b_delta; emxArray_real_T *b_x; real_T E; int32_T i; emxArray_real_T *deltaone; emxArray_real_T *g; emxArray_real_T *y2; emxArray_real_T *H; emxArray_real_T *s; emxArray_real_T *C; emxArray_real_T *b_C; emxArray_real_T *c_delta; emxArray_real_T *d_D; emxArray_real_T *b_deltaone; boolean_T exitg1; boolean_T guard2 = FALSE; uint32_T unnamed_idx_0; int32_T ic; int32_T ar; int32_T ib; int32_T ia; int32_T i1; boolean_T guard1 = FALSE; int32_T exitg3; boolean_T exitg2; int32_T b_y[2]; real_T E_new; real_T u1; emxInit_real_T(&D, 2); emxInit_real_T(&delta, 2); /* #codgen */ /* */ /* SAMMON - apply Sammon's nonlinear mapping */ /* */ /* Y = SAMMON(X) applies Sammon's nonlinear mapping procedure on */ /* multivariate data X, where each row represents a pattern and each column */ /* represents a feature. On completion, Y contains the corresponding */ /* co-ordinates of each point on the map. By default, a two-dimensional */ /* map is created. Note if X contains any duplicated rows, SAMMON will */ /* fail (ungracefully). */ /* */ /* [Y,E] = SAMMON(X) also returns the value of the cost function in E (i.e. */ /* the stress of the mapping). */ /* */ /* An N-dimensional output map is generated by Y = SAMMON(X,N) . */ /* */ /* A set of optimisation options can also be specified using a third */ /* argument, Y = SAMMON(X,N,OPTS) , where OPTS is a structure with fields: */ /* */ /* MaxIter - maximum number of iterations */ /* TolFun - relative tolerance on objective function */ /* MaxHalves - maximum number of step halvings */ /* Input - {'raw','distance'} if set to 'distance', X is */ /* interpreted as a matrix of pairwise distances. */ /* Display - {'off', 'on', 'iter'} */ /* Initialisation - {'pca', 'random'} */ /* */ /* The default options structure can be retrieved by calling SAMMON with */ /* no parameters. */ /* */ /* References : */ /* */ /* [1] Sammon, John W. Jr., "A Nonlinear Mapping for Data Structure */ /* Analysis", IEEE Transactions on Computers, vol. C-18, no. 5, */ /* pp 401-409, May 1969. */ /* */ /* See also : SAMMON_TEST */ /* */ /* File : sammon.m */ /* */ /* Date : Monday 12th November 2007. */ /* */ /* Author : Gavin C. Cawley and Nicola L. C. Talbot */ /* */ /* Description : Simple vectorised MATLAB implementation of Sammon's non-linear */ /* mapping algorithm [1]. */ /* */ /* References : [1] Sammon, John W. Jr., "A Nonlinear Mapping for Data */ /* Structure Analysis", IEEE Transactions on Computers, */ /* vol. C-18, no. 5, pp 401-409, May 1969. */ /* */ /* History : 10/08/2004 - v1.00 */ /* 11/08/2004 - v1.10 Hessian made positive semidefinite */ /* 13/08/2004 - v1.11 minor optimisation */ /* 12/11/2007 - v1.20 initialisation using the first n principal */ /* components. */ /* */ /* Thanks : Dr Nick Hamilton ([email protected]) for supplying the */ /* code for implementing initialisation using the first n */ /* principal components (introduced in v1.20). */ /* */ /* To do : The current version does not take advantage of the symmetry */ /* of the distance matrix in order to allow for easy */ /* vectorisation. This may not be a good choice for very large */ /* datasets, so perhaps one day I'll get around to doing a MEX */ /* version using the BLAS library etc. for very large datasets. */ /* */ /* Copyright : (c) Dr Gavin C. Cawley, November 2007. */ /* */ /* This program is free software; you can redistribute it and/or modify */ /* it under the terms of the GNU General Public License as published by */ /* the Free Software Foundation; either version 2 of the License, or */ /* (at your option) any later version. */ /* */ /* This program is distributed in the hope that it will be useful, */ /* but WITHOUT ANY WARRANTY; without even the implied warranty of */ /* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the */ /* GNU General Public License for more details. */ /* */ /* You should have received a copy of the GNU General Public License */ /* along with this program; if not, write to the Free Software */ /* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ /* */ /* use the default options structure */ /* create distance matrix unless given by parameters */ euclid(x, x, D); /* remaining initialisation */ eye(x->size[0], delta); i0 = D->size[0] * D->size[1]; emxEnsureCapacity((emxArray__common *)D, i0, (int32_T)sizeof(real_T)); b_D = D->size[0]; br = D->size[1]; br *= b_D; for (i0 = 0; i0 < br; i0++) { D->data[i0] += delta->data[i0]; } emxInit_real_T(&Dinv, 2); emxInit_real_T(&d, 2); rdivide(D, Dinv); randn(x->size[0], y); b_euclid(y, y, d); eye(x->size[0], delta); i0 = d->size[0] * d->size[1]; emxEnsureCapacity((emxArray__common *)d, i0, (int32_T)sizeof(real_T)); b_D = d->size[0]; br = d->size[1]; br *= b_D; for (i0 = 0; i0 < br; i0++) { d->data[i0] += delta->data[i0]; } emxInit_real_T(&dinv, 2); emxInit_real_T(&c_D, 2); rdivide(d, dinv); i0 = c_D->size[0] * c_D->size[1]; c_D->size[0] = D->size[0]; c_D->size[1] = D->size[1]; emxEnsureCapacity((emxArray__common *)c_D, i0, (int32_T)sizeof(real_T)); br = D->size[0] * D->size[1]; for (i0 = 0; i0 < br; i0++) { c_D->data[i0] = D->data[i0] - d->data[i0]; } emxInit_real_T(&b_delta, 2); power(c_D, delta); i0 = b_delta->size[0] * b_delta->size[1]; b_delta->size[0] = delta->size[0]; b_delta->size[1] = delta->size[1]; emxEnsureCapacity((emxArray__common *)b_delta, i0, (int32_T)sizeof(real_T)); br = delta->size[0] * delta->size[1]; emxFree_real_T(&c_D); for (i0 = 0; i0 < br; i0++) { b_delta->data[i0] = delta->data[i0] * Dinv->data[i0]; } emxInit_real_T(&b_x, 2); c_sum(b_delta, b_x); E = d_sum(b_x); /* get on with it */ i = 0; emxFree_real_T(&b_delta); emxInit_real_T(&deltaone, 2); emxInit_real_T(&g, 2); emxInit_real_T(&y2, 2); emxInit_real_T(&H, 2); b_emxInit_real_T(&s, 1); emxInit_real_T(&C, 2); emxInit_real_T(&b_C, 2); emxInit_real_T(&c_delta, 2); emxInit_real_T(&d_D, 2); b_emxInit_real_T(&b_deltaone, 1); exitg1 = FALSE; while ((exitg1 == FALSE) && (i < 500)) { /* compute gradient, Hessian and search direction (note it is actually */ /* 1/4 of the gradient and Hessian, but the step size is just the ratio */ /* of the gradient and the diagonal of the Hessian so it doesn't matter). */ i0 = delta->size[0] * delta->size[1]; delta->size[0] = dinv->size[0]; delta->size[1] = dinv->size[1]; emxEnsureCapacity((emxArray__common *)delta, i0, (int32_T)sizeof(real_T)); br = dinv->size[0] * dinv->size[1]; for (i0 = 0; i0 < br; i0++) { delta->data[i0] = dinv->data[i0] - Dinv->data[i0]; } guard2 = FALSE; if (delta->size[1] == 1) { guard2 = TRUE; } else { b_D = x->size[0]; if (b_D == 1) { guard2 = TRUE; } else { unnamed_idx_0 = (uint32_T)delta->size[0]; i0 = deltaone->size[0] * deltaone->size[1]; deltaone->size[0] = (int32_T)unnamed_idx_0; deltaone->size[1] = 2; emxEnsureCapacity((emxArray__common *)deltaone, i0, (int32_T)sizeof (real_T)); br = (int32_T)unnamed_idx_0 << 1; for (i0 = 0; i0 < br; i0++) { deltaone->data[i0] = 0.0; } if (delta->size[0] == 0) { } else { b_D = 0; while ((delta->size[0] > 0) && (b_D <= delta->size[0])) { i0 = b_D + delta->size[0]; for (ic = b_D; ic + 1 <= i0; ic++) { deltaone->data[ic] = 0.0; } b_D += delta->size[0]; } br = 0; b_D = 0; while ((delta->size[0] > 0) && (b_D <= delta->size[0])) { ar = 0; i0 = br + delta->size[1]; for (ib = br + 1; ib <= i0; ib++) { ia = ar; i1 = b_D + delta->size[0]; for (ic = b_D; ic + 1 <= i1; ic++) { ia++; deltaone->data[ic] += delta->data[ia - 1]; } ar += delta->size[0]; } br += delta->size[1]; b_D += delta->size[0]; } } } } if (guard2 == TRUE) { i0 = deltaone->size[0] * deltaone->size[1]; deltaone->size[0] = delta->size[0]; deltaone->size[1] = 2; emxEnsureCapacity((emxArray__common *)deltaone, i0, (int32_T)sizeof(real_T)); br = delta->size[0]; for (i0 = 0; i0 < br; i0++) { for (i1 = 0; i1 < 2; i1++) { deltaone->data[i0 + deltaone->size[0] * i1] = 0.0; ar = delta->size[1]; for (b_D = 0; b_D < ar; b_D++) { deltaone->data[i0 + deltaone->size[0] * i1] += delta->data[i0 + delta->size[0] * b_D]; } } } } if ((delta->size[1] == 1) || (y->size[0] == 1)) { i0 = g->size[0] * g->size[1]; g->size[0] = delta->size[0]; g->size[1] = 2; emxEnsureCapacity((emxArray__common *)g, i0, (int32_T)sizeof(real_T)); br = delta->size[0]; for (i0 = 0; i0 < br; i0++) { for (i1 = 0; i1 < 2; i1++) { g->data[i0 + g->size[0] * i1] = 0.0; ar = delta->size[1]; for (b_D = 0; b_D < ar; b_D++) { g->data[i0 + g->size[0] * i1] += delta->data[i0 + delta->size[0] * b_D] * y->data[b_D + y->size[0] * i1]; } } } } else { unnamed_idx_0 = (uint32_T)delta->size[0]; i0 = g->size[0] * g->size[1]; g->size[0] = (int32_T)unnamed_idx_0; g->size[1] = 2; emxEnsureCapacity((emxArray__common *)g, i0, (int32_T)sizeof(real_T)); br = (int32_T)unnamed_idx_0 << 1; for (i0 = 0; i0 < br; i0++) { g->data[i0] = 0.0; } if (delta->size[0] == 0) { } else { b_D = 0; while ((delta->size[0] > 0) && (b_D <= delta->size[0])) { i0 = b_D + delta->size[0]; for (ic = b_D; ic + 1 <= i0; ic++) { g->data[ic] = 0.0; } b_D += delta->size[0]; } br = 0; b_D = 0; while ((delta->size[0] > 0) && (b_D <= delta->size[0])) { ar = 0; i0 = br + delta->size[1]; for (ib = br; ib + 1 <= i0; ib++) { if (y->data[ib] != 0.0) { ia = ar; i1 = b_D + delta->size[0]; for (ic = b_D; ic + 1 <= i1; ic++) { ia++; g->data[ic] += y->data[ib] * delta->data[ia - 1]; } } ar += delta->size[0]; } br += delta->size[1]; b_D += delta->size[0]; } } } i0 = g->size[0] * g->size[1]; g->size[1] = 2; emxEnsureCapacity((emxArray__common *)g, i0, (int32_T)sizeof(real_T)); b_D = g->size[0]; br = g->size[1]; br *= b_D; for (i0 = 0; i0 < br; i0++) { g->data[i0] -= y->data[i0] * deltaone->data[i0]; } c_power(dinv, delta); b_power(y, y2); if ((delta->size[1] == 1) || (y2->size[0] == 1)) { i0 = H->size[0] * H->size[1]; H->size[0] = delta->size[0]; H->size[1] = 2; emxEnsureCapacity((emxArray__common *)H, i0, (int32_T)sizeof(real_T)); br = delta->size[0]; for (i0 = 0; i0 < br; i0++) { for (i1 = 0; i1 < 2; i1++) { H->data[i0 + H->size[0] * i1] = 0.0; ar = delta->size[1]; for (b_D = 0; b_D < ar; b_D++) { H->data[i0 + H->size[0] * i1] += delta->data[i0 + delta->size[0] * b_D] * y2->data[b_D + y2->size[0] * i1]; } } } } else { unnamed_idx_0 = (uint32_T)delta->size[0]; i0 = H->size[0] * H->size[1]; H->size[0] = (int32_T)unnamed_idx_0; H->size[1] = 2; emxEnsureCapacity((emxArray__common *)H, i0, (int32_T)sizeof(real_T)); br = (int32_T)unnamed_idx_0 << 1; for (i0 = 0; i0 < br; i0++) { H->data[i0] = 0.0; } if (delta->size[0] == 0) { } else { b_D = 0; while ((delta->size[0] > 0) && (b_D <= delta->size[0])) { i0 = b_D + delta->size[0]; for (ic = b_D; ic + 1 <= i0; ic++) { H->data[ic] = 0.0; } b_D += delta->size[0]; } br = 0; b_D = 0; while ((delta->size[0] > 0) && (b_D <= delta->size[0])) { ar = 0; i0 = br + delta->size[1]; for (ib = br; ib + 1 <= i0; ib++) { if (y2->data[ib] != 0.0) { ia = ar; i1 = b_D + delta->size[0]; for (ic = b_D; ic + 1 <= i1; ic++) { ia++; H->data[ic] += y2->data[ib] * delta->data[ia - 1]; } } ar += delta->size[0]; } br += delta->size[1]; b_D += delta->size[0]; } } } if ((delta->size[1] == 1) || (y->size[0] == 1)) { i0 = C->size[0] * C->size[1]; C->size[0] = delta->size[0]; C->size[1] = 2; emxEnsureCapacity((emxArray__common *)C, i0, (int32_T)sizeof(real_T)); br = delta->size[0]; for (i0 = 0; i0 < br; i0++) { for (i1 = 0; i1 < 2; i1++) { C->data[i0 + C->size[0] * i1] = 0.0; ar = delta->size[1]; for (b_D = 0; b_D < ar; b_D++) { C->data[i0 + C->size[0] * i1] += delta->data[i0 + delta->size[0] * b_D] * y->data[b_D + y->size[0] * i1]; } } } } else { unnamed_idx_0 = (uint32_T)delta->size[0]; i0 = C->size[0] * C->size[1]; C->size[0] = (int32_T)unnamed_idx_0; C->size[1] = 2; emxEnsureCapacity((emxArray__common *)C, i0, (int32_T)sizeof(real_T)); br = (int32_T)unnamed_idx_0 << 1; for (i0 = 0; i0 < br; i0++) { C->data[i0] = 0.0; } if (delta->size[0] == 0) { } else { b_D = 0; while ((delta->size[0] > 0) && (b_D <= delta->size[0])) { i0 = b_D + delta->size[0]; for (ic = b_D; ic + 1 <= i0; ic++) { C->data[ic] = 0.0; } b_D += delta->size[0]; } br = 0; b_D = 0; while ((delta->size[0] > 0) && (b_D <= delta->size[0])) { ar = 0; i0 = br + delta->size[1]; for (ib = br; ib + 1 <= i0; ib++) { if (y->data[ib] != 0.0) { ia = ar; i1 = b_D + delta->size[0]; for (ic = b_D; ic + 1 <= i1; ic++) { ia++; C->data[ic] += y->data[ib] * delta->data[ia - 1]; } } ar += delta->size[0]; } br += delta->size[1]; b_D += delta->size[0]; } } } guard1 = FALSE; if (delta->size[1] == 1) { guard1 = TRUE; } else { b_D = x->size[0]; if (b_D == 1) { guard1 = TRUE; } else { unnamed_idx_0 = (uint32_T)delta->size[0]; i0 = b_C->size[0] * b_C->size[1]; b_C->size[0] = (int32_T)unnamed_idx_0; b_C->size[1] = 2; emxEnsureCapacity((emxArray__common *)b_C, i0, (int32_T)sizeof(real_T)); br = (int32_T)unnamed_idx_0 << 1; for (i0 = 0; i0 < br; i0++) { b_C->data[i0] = 0.0; } if (delta->size[0] == 0) { } else { b_D = 0; while ((delta->size[0] > 0) && (b_D <= delta->size[0])) { i0 = b_D + delta->size[0]; for (ic = b_D; ic + 1 <= i0; ic++) { b_C->data[ic] = 0.0; } b_D += delta->size[0]; } br = 0; b_D = 0; while ((delta->size[0] > 0) && (b_D <= delta->size[0])) { ar = 0; i0 = br + delta->size[1]; for (ib = br + 1; ib <= i0; ib++) { ia = ar; i1 = b_D + delta->size[0]; for (ic = b_D; ic + 1 <= i1; ic++) { ia++; b_C->data[ic] += delta->data[ia - 1]; } ar += delta->size[0]; } br += delta->size[1]; b_D += delta->size[0]; } } } } if (guard1 == TRUE) { i0 = b_C->size[0] * b_C->size[1]; b_C->size[0] = delta->size[0]; b_C->size[1] = 2; emxEnsureCapacity((emxArray__common *)b_C, i0, (int32_T)sizeof(real_T)); br = delta->size[0]; for (i0 = 0; i0 < br; i0++) { for (i1 = 0; i1 < 2; i1++) { b_C->data[i0 + b_C->size[0] * i1] = 0.0; ar = delta->size[1]; for (b_D = 0; b_D < ar; b_D++) { b_C->data[i0 + b_C->size[0] * i1] += delta->data[i0 + delta->size[0] * b_D]; } } } } i0 = H->size[0] * H->size[1]; H->size[1] = 2; emxEnsureCapacity((emxArray__common *)H, i0, (int32_T)sizeof(real_T)); b_D = H->size[0]; br = H->size[1]; br *= b_D; for (i0 = 0; i0 < br; i0++) { H->data[i0] = ((H->data[i0] - deltaone->data[i0]) - 2.0 * y->data[i0] * C->data[i0]) + y2->data[i0] * b_C->data[i0]; } b_D = H->size[0] << 1; i0 = s->size[0]; s->size[0] = b_D; emxEnsureCapacity((emxArray__common *)s, i0, (int32_T)sizeof(real_T)); br = 0; do { exitg3 = 0; b_D = H->size[0] << 1; if (br <= b_D - 1) { s->data[br] = fabs(H->data[br]); br++; } else { exitg3 = 1; } } while (exitg3 == 0); i0 = s->size[0]; s->size[0] = g->size[0] << 1; emxEnsureCapacity((emxArray__common *)s, i0, (int32_T)sizeof(real_T)); br = g->size[0] << 1; for (i0 = 0; i0 < br; i0++) { s->data[i0] = -g->data[i0] / s->data[i0]; } i0 = deltaone->size[0] * deltaone->size[1]; deltaone->size[0] = y->size[0]; deltaone->size[1] = 2; emxEnsureCapacity((emxArray__common *)deltaone, i0, (int32_T)sizeof(real_T)); br = y->size[0] * y->size[1]; for (i0 = 0; i0 < br; i0++) { deltaone->data[i0] = y->data[i0]; } /* use step-halving procedure to ensure progress is made */ ib = 1; ia = 0; exitg2 = FALSE; while ((exitg2 == FALSE) && (ia < 20)) { ib = ia + 1; b_D = deltaone->size[0] << 1; i0 = b_deltaone->size[0]; b_deltaone->size[0] = b_D; emxEnsureCapacity((emxArray__common *)b_deltaone, i0, (int32_T)sizeof (real_T)); for (i0 = 0; i0 < b_D; i0++) { b_deltaone->data[i0] = deltaone->data[i0] + s->data[i0]; } for (i0 = 0; i0 < 2; i0++) { b_y[i0] = y->size[i0]; } i0 = y->size[0] * y->size[1]; y->size[0] = b_y[0]; y->size[1] = b_y[1]; emxEnsureCapacity((emxArray__common *)y, i0, (int32_T)sizeof(real_T)); br = b_y[1]; for (i0 = 0; i0 < br; i0++) { ar = b_y[0]; for (i1 = 0; i1 < ar; i1++) { y->data[i1 + y->size[0] * i0] = b_deltaone->data[i1 + b_y[0] * i0]; } } b_euclid(y, y, d); b_D = x->size[0]; i0 = delta->size[0] * delta->size[1]; delta->size[0] = b_D; emxEnsureCapacity((emxArray__common *)delta, i0, (int32_T)sizeof(real_T)); b_D = x->size[0]; i0 = delta->size[0] * delta->size[1]; delta->size[1] = b_D; emxEnsureCapacity((emxArray__common *)delta, i0, (int32_T)sizeof(real_T)); br = x->size[0] * x->size[0]; for (i0 = 0; i0 < br; i0++) { delta->data[i0] = 0.0; } E_new = x->size[0]; u1 = x->size[0]; if (E_new <= u1) { } else { E_new = u1; } b_D = (int32_T)E_new; if (b_D > 0) { for (br = 0; br + 1 <= b_D; br++) { delta->data[br + delta->size[0] * br] = 1.0; } } i0 = d->size[0] * d->size[1]; emxEnsureCapacity((emxArray__common *)d, i0, (int32_T)sizeof(real_T)); b_D = d->size[0]; br = d->size[1]; br *= b_D; for (i0 = 0; i0 < br; i0++) { d->data[i0] += delta->data[i0]; } rdivide(d, dinv); i0 = d_D->size[0] * d_D->size[1]; d_D->size[0] = D->size[0]; d_D->size[1] = D->size[1]; emxEnsureCapacity((emxArray__common *)d_D, i0, (int32_T)sizeof(real_T)); br = D->size[0] * D->size[1]; for (i0 = 0; i0 < br; i0++) { d_D->data[i0] = D->data[i0] - d->data[i0]; } power(d_D, delta); i0 = c_delta->size[0] * c_delta->size[1]; c_delta->size[0] = delta->size[0]; c_delta->size[1] = delta->size[1]; emxEnsureCapacity((emxArray__common *)c_delta, i0, (int32_T)sizeof(real_T)); br = delta->size[0] * delta->size[1]; for (i0 = 0; i0 < br; i0++) { c_delta->data[i0] = delta->data[i0] * Dinv->data[i0]; } c_sum(c_delta, b_x); if (b_x->size[1] == 0) { E_new = 0.0; } else { E_new = b_x->data[0]; for (br = 2; br <= b_x->size[1]; br++) { E_new += b_x->data[br - 1]; } } if (E_new < E) { exitg2 = TRUE; } else { i0 = s->size[0]; emxEnsureCapacity((emxArray__common *)s, i0, (int32_T)sizeof(real_T)); br = s->size[0]; for (i0 = 0; i0 < br; i0++) { s->data[i0] *= 0.5; } ia++; } } /* bomb out if too many halving steps are required */ if ((ib == 20) || (fabs((E - E_new) / E) < 1.0E-9)) { exitg1 = TRUE; } else { /* evaluate termination criterion */ /* report progress */ E = E_new; i++; } } emxFree_real_T(&b_deltaone); emxFree_real_T(&d_D); emxFree_real_T(&c_delta); emxFree_real_T(&b_x); emxFree_real_T(&b_C); emxFree_real_T(&C); emxFree_real_T(&s); emxFree_real_T(&H); emxFree_real_T(&y2); emxFree_real_T(&g); emxFree_real_T(&deltaone); emxFree_real_T(&delta); emxFree_real_T(&dinv); emxFree_real_T(&d); emxFree_real_T(&Dinv); emxFree_real_T(&D); /* fiddle stress to match the original Sammon paper */ }