1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591 |
-
-
- #include "qr_solve.h"
-
- #if ENABLED(AUTO_BED_LEVELING_GRID)
-
- #include <stdlib.h>
- #include <math.h>
-
-
-
- int i4_min(int i1, int i2)
-
-
-
- {
- return (i1 < i2) ? i1 : i2;
- }
-
- double r8_epsilon(void)
-
-
-
- {
- const double value = 2.220446049250313E-016;
- return value;
- }
-
- double r8_max(double x, double y)
-
-
-
- {
- return (y < x) ? x : y;
- }
-
- double r8_abs(double x)
-
-
-
- {
- return (x < 0.0) ? -x : x;
- }
-
- double r8_sign(double x)
-
-
-
- {
- return (x < 0.0) ? -1.0 : 1.0;
- }
-
- double r8mat_amax(int m, int n, double a[])
-
-
-
- {
- double value = r8_abs(a[0 + 0 * m]);
- for (int j = 0; j < n; j++) {
- for (int i = 0; i < m; i++) {
- NOLESS(value, r8_abs(a[i + j * m]));
- }
- }
- return value;
- }
-
- void r8mat_copy(double a2[], int m, int n, double a1[])
-
-
-
- {
- for (int j = 0; j < n; j++) {
- for (int i = 0; i < m; i++)
- a2[i + j * m] = a1[i + j * m];
- }
- }
-
-
-
- void daxpy(int n, double da, double dx[], int incx, double dy[], int incy)
-
-
-
- {
- if (n <= 0 || da == 0.0) return;
-
- int i, ix, iy, m;
-
-
- if (incx != 1 || incy != 1) {
- if (0 <= incx)
- ix = 0;
- else
- ix = (- n + 1) * incx;
- if (0 <= incy)
- iy = 0;
- else
- iy = (- n + 1) * incy;
- for (i = 0; i < n; i++) {
- dy[iy] = dy[iy] + da * dx[ix];
- ix = ix + incx;
- iy = iy + incy;
- }
- }
-
-
- else {
- m = n % 4;
- for (i = 0; i < m; i++)
- dy[i] = dy[i] + da * dx[i];
- for (i = m; i < n; i = i + 4) {
- dy[i ] = dy[i ] + da * dx[i ];
- dy[i + 1] = dy[i + 1] + da * dx[i + 1];
- dy[i + 2] = dy[i + 2] + da * dx[i + 2];
- dy[i + 3] = dy[i + 3] + da * dx[i + 3];
- }
- }
- }
-
-
- double ddot(int n, double dx[], int incx, double dy[], int incy)
-
-
-
- {
-
- if (n <= 0) return 0.0;
-
- int i, m;
- double dtemp = 0.0;
-
-
-
- if (incx != 1 || incy != 1) {
- int ix = (incx >= 0) ? 0 : (-n + 1) * incx,
- iy = (incy >= 0) ? 0 : (-n + 1) * incy;
- for (i = 0; i < n; i++) {
- dtemp += dx[ix] * dy[iy];
- ix = ix + incx;
- iy = iy + incy;
- }
- }
-
-
- else {
- m = n % 5;
- for (i = 0; i < m; i++)
- dtemp += dx[i] * dy[i];
- for (i = m; i < n; i = i + 5) {
- dtemp += dx[i] * dy[i]
- + dx[i + 1] * dy[i + 1]
- + dx[i + 2] * dy[i + 2]
- + dx[i + 3] * dy[i + 3]
- + dx[i + 4] * dy[i + 4];
- }
- }
- return dtemp;
- }
-
-
- double dnrm2(int n, double x[], int incx)
-
-
-
- {
- double norm;
- if (n < 1 || incx < 1)
- norm = 0.0;
- else if (n == 1)
- norm = r8_abs(x[0]);
- else {
- double scale = 0.0, ssq = 1.0;
- int ix = 0;
- for (int i = 0; i < n; i++) {
- if (x[ix] != 0.0) {
- double absxi = r8_abs(x[ix]);
- if (scale < absxi) {
- ssq = 1.0 + ssq * (scale / absxi) * (scale / absxi);
- scale = absxi;
- }
- else
- ssq = ssq + (absxi / scale) * (absxi / scale);
- }
- ix += incx;
- }
- norm = scale * sqrt(ssq);
- }
- return norm;
- }
-
-
- void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
- int jpvt[], double qraux[])
-
-
-
- {
- double work[n];
-
- for (int i = 0; i < n; i++)
- jpvt[i] = 0;
-
- int job = 1;
-
- dqrdc(a, lda, m, n, qraux, jpvt, work, job);
-
- *kr = 0;
- int k = i4_min(m, n);
- for (int j = 0; j < k; j++) {
- if (r8_abs(a[j + j * lda]) <= tol * r8_abs(a[0 + 0 * lda]))
- return;
- *kr = j + 1;
- }
- }
-
-
- void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
- double work[], int job)
-
-
-
- {
- int jp;
- int j;
- int lup;
- int maxj;
- double maxnrm, nrmxl, t, tt;
-
- int pl = 1, pu = 0;
-
-
- if (job != 0) {
- for (j = 1; j <= p; j++) {
- int swapj = (0 < jpvt[j - 1]);
- jpvt[j - 1] = (jpvt[j - 1] < 0) ? -j : j;
- if (swapj) {
- if (j != pl)
- dswap(n, a + 0 + (pl - 1)*lda, 1, a + 0 + (j - 1), 1);
- jpvt[j - 1] = jpvt[pl - 1];
- jpvt[pl - 1] = j;
- pl++;
- }
- }
- pu = p;
- for (j = p; 1 <= j; j--) {
- if (jpvt[j - 1] < 0) {
- jpvt[j - 1] = -jpvt[j - 1];
- if (j != pu) {
- dswap(n, a + 0 + (pu - 1)*lda, 1, a + 0 + (j - 1)*lda, 1);
- jp = jpvt[pu - 1];
- jpvt[pu - 1] = jpvt[j - 1];
- jpvt[j - 1] = jp;
- }
- pu = pu - 1;
- }
- }
- }
-
-
- for (j = pl; j <= pu; j++)
- qraux[j - 1] = dnrm2(n, a + 0 + (j - 1) * lda, 1);
- for (j = pl; j <= pu; j++)
- work[j - 1] = qraux[j - 1];
-
-
- lup = i4_min(n, p);
- for (int l = 1; l <= lup; l++) {
-
-
- if (pl <= l && l < pu) {
- maxnrm = 0.0;
- maxj = l;
- for (j = l; j <= pu; j++) {
- if (maxnrm < qraux[j - 1]) {
- maxnrm = qraux[j - 1];
- maxj = j;
- }
- }
- if (maxj != l) {
- dswap(n, a + 0 + (l - 1)*lda, 1, a + 0 + (maxj - 1)*lda, 1);
- qraux[maxj - 1] = qraux[l - 1];
- work[maxj - 1] = work[l - 1];
- jp = jpvt[maxj - 1];
- jpvt[maxj - 1] = jpvt[l - 1];
- jpvt[l - 1] = jp;
- }
- }
-
-
- qraux[l - 1] = 0.0;
- if (l != n) {
- nrmxl = dnrm2(n - l + 1, a + l - 1 + (l - 1) * lda, 1);
- if (nrmxl != 0.0) {
- if (a[l - 1 + (l - 1)*lda] != 0.0)
- nrmxl = nrmxl * r8_sign(a[l - 1 + (l - 1) * lda]);
- dscal(n - l + 1, 1.0 / nrmxl, a + l - 1 + (l - 1)*lda, 1);
- a[l - 1 + (l - 1)*lda] = 1.0 + a[l - 1 + (l - 1) * lda];
-
-
- for (j = l + 1; j <= p; j++) {
- t = -ddot(n - l + 1, a + l - 1 + (l - 1) * lda, 1, a + l - 1 + (j - 1) * lda, 1)
- / a[l - 1 + (l - 1) * lda];
- daxpy(n - l + 1, t, a + l - 1 + (l - 1)*lda, 1, a + l - 1 + (j - 1)*lda, 1);
- if (pl <= j && j <= pu) {
- if (qraux[j - 1] != 0.0) {
- tt = 1.0 - pow(r8_abs(a[l - 1 + (j - 1) * lda]) / qraux[j - 1], 2);
- tt = r8_max(tt, 0.0);
- t = tt;
- tt = 1.0 + 0.05 * tt * pow(qraux[j - 1] / work[j - 1], 2);
- if (tt != 1.0)
- qraux[j - 1] = qraux[j - 1] * sqrt(t);
- else {
- qraux[j - 1] = dnrm2(n - l, a + l + (j - 1) * lda, 1);
- work[j - 1] = qraux[j - 1];
- }
- }
- }
- }
-
-
- qraux[l - 1] = a[l - 1 + (l - 1) * lda];
- a[l - 1 + (l - 1)*lda] = -nrmxl;
- }
- }
- }
- }
-
-
- int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
- double x[], double rsd[], int jpvt[], double qraux[], int itask)
-
-
-
- {
- int ind;
- if (lda < m) {
-
-
- ind = -1;
- return ind;
- }
-
- if (n <= 0) {
-
-
- ind = -2;
- return ind;
- }
-
- if (itask < 1) {
-
-
- ind = -3;
- return ind;
- }
-
- ind = 0;
-
-
- if (itask == 1)
- dqrank(a, lda, m, n, tol, kr, jpvt, qraux);
-
-
- dqrlss(a, lda, m, n, *kr, b, x, rsd, jpvt, qraux);
- return ind;
- }
-
-
- void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
- double rsd[], int jpvt[], double qraux[])
-
-
-
- {
- int i;
- int info;
- int j;
- int job;
- int k;
- double t;
-
- if (kr != 0) {
- job = 110;
- info = dqrsl(a, lda, m, kr, qraux, b, rsd, rsd, x, rsd, rsd, job); UNUSED(info);
- }
-
- for (i = 0; i < n; i++)
- jpvt[i] = - jpvt[i];
-
- for (i = kr; i < n; i++)
- x[i] = 0.0;
-
- for (j = 1; j <= n; j++) {
- if (jpvt[j - 1] <= 0) {
- k = - jpvt[j - 1];
- jpvt[j - 1] = k;
-
- while (k != j) {
- t = x[j - 1];
- x[j - 1] = x[k - 1];
- x[k - 1] = t;
- jpvt[k - 1] = -jpvt[k - 1];
- k = jpvt[k - 1];
- }
- }
- }
- }
-
-
- int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
- double qy[], double qty[], double b[], double rsd[], double ab[], int job)
-
-
-
- {
- int cab;
- int cb;
- int cqty;
- int cqy;
- int cr;
- int i;
- int info;
- int j;
- int jj;
- int ju;
- double t;
- double temp;
-
-
- info = 0;
-
-
-
- cqy = ( job / 10000 != 0);
- cqty = ((job % 10000) != 0);
- cb = ((job % 1000) / 100 != 0);
- cr = ((job % 100) / 10 != 0);
- cab = ((job % 10) != 0);
- ju = i4_min(k, n - 1);
-
-
-
- if (ju == 0) {
- if (cqy)
- qy[0] = y[0];
- if (cqty)
- qty[0] = y[0];
- if (cab)
- ab[0] = y[0];
- if (cb) {
- if (a[0 + 0 * lda] == 0.0)
- info = 1;
- else
- b[0] = y[0] / a[0 + 0 * lda];
- }
- if (cr)
- rsd[0] = 0.0;
- return info;
- }
-
-
- if (cqy) {
- for (i = 1; i <= n; i++)
- qy[i - 1] = y[i - 1];
- }
- if (cqty) {
- for (i = 1; i <= n; i++)
- qty[i - 1] = y[i - 1];
- }
-
-
- if (cqy) {
- for (jj = 1; jj <= ju; jj++) {
- j = ju - jj + 1;
- if (qraux[j - 1] != 0.0) {
- temp = a[j - 1 + (j - 1) * lda];
- a[j - 1 + (j - 1)*lda] = qraux[j - 1];
- t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, qy + j - 1, 1) / a[j - 1 + (j - 1) * lda];
- daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, qy + j - 1, 1);
- a[j - 1 + (j - 1)*lda] = temp;
- }
- }
- }
-
-
- if (cqty) {
- for (j = 1; j <= ju; j++) {
- if (qraux[j - 1] != 0.0) {
- temp = a[j - 1 + (j - 1) * lda];
- a[j - 1 + (j - 1)*lda] = qraux[j - 1];
- t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, qty + j - 1, 1) / a[j - 1 + (j - 1) * lda];
- daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, qty + j - 1, 1);
- a[j - 1 + (j - 1)*lda] = temp;
- }
- }
- }
-
-
- if (cb) {
- for (i = 1; i <= k; i++)
- b[i - 1] = qty[i - 1];
- }
- if (cab) {
- for (i = 1; i <= k; i++)
- ab[i - 1] = qty[i - 1];
- }
- if (cr && k < n) {
- for (i = k + 1; i <= n; i++)
- rsd[i - 1] = qty[i - 1];
- }
- if (cab && k + 1 <= n) {
- for (i = k + 1; i <= n; i++)
- ab[i - 1] = 0.0;
- }
- if (cr) {
- for (i = 1; i <= k; i++)
- rsd[i - 1] = 0.0;
- }
-
-
- if (cb) {
- for (jj = 1; jj <= k; jj++) {
- j = k - jj + 1;
- if (a[j - 1 + (j - 1)*lda] == 0.0) {
- info = j;
- break;
- }
- b[j - 1] = b[j - 1] / a[j - 1 + (j - 1) * lda];
- if (j != 1) {
- t = -b[j - 1];
- daxpy(j - 1, t, a + 0 + (j - 1)*lda, 1, b, 1);
- }
- }
- }
-
-
- if (cr || cab) {
- for (jj = 1; jj <= ju; jj++) {
- j = ju - jj + 1;
- if (qraux[j - 1] != 0.0) {
- temp = a[j - 1 + (j - 1) * lda];
- a[j - 1 + (j - 1)*lda] = qraux[j - 1];
- if (cr) {
- t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, rsd + j - 1, 1)
- / a[j - 1 + (j - 1) * lda];
- daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, rsd + j - 1, 1);
- }
- if (cab) {
- t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, ab + j - 1, 1)
- / a[j - 1 + (j - 1) * lda];
- daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, ab + j - 1, 1);
- }
- a[j - 1 + (j - 1)*lda] = temp;
- }
- }
- }
- return info;
- }
-
-
-
-
- void dscal(int n, double sa, double x[], int incx)
-
-
-
- {
- int i;
- int ix;
- int m;
-
- if (n <= 0) return;
-
- if (incx == 1) {
- m = n % 5;
- for (i = 0; i < m; i++)
- x[i] = sa * x[i];
- for (i = m; i < n; i = i + 5) {
- x[i] = sa * x[i];
- x[i + 1] = sa * x[i + 1];
- x[i + 2] = sa * x[i + 2];
- x[i + 3] = sa * x[i + 3];
- x[i + 4] = sa * x[i + 4];
- }
- }
- else {
- if (0 <= incx)
- ix = 0;
- else
- ix = (- n + 1) * incx;
- for (i = 0; i < n; i++) {
- x[ix] = sa * x[ix];
- ix = ix + incx;
- }
- }
- }
-
-
-
- void dswap(int n, double x[], int incx, double y[], int incy)
-
-
-
- {
- if (n <= 0) return;
-
- int i, ix, iy, m;
- double temp;
-
- if (incx == 1 && incy == 1) {
- m = n % 3;
- for (i = 0; i < m; i++) {
- temp = x[i];
- x[i] = y[i];
- y[i] = temp;
- }
- for (i = m; i < n; i = i + 3) {
- temp = x[i];
- x[i] = y[i];
- y[i] = temp;
- temp = x[i + 1];
- x[i + 1] = y[i + 1];
- y[i + 1] = temp;
- temp = x[i + 2];
- x[i + 2] = y[i + 2];
- y[i + 2] = temp;
- }
- }
- else {
- ix = (incx >= 0) ? 0 : (-n + 1) * incx;
- iy = (incy >= 0) ? 0 : (-n + 1) * incy;
- for (i = 0; i < n; i++) {
- temp = x[ix];
- x[ix] = y[iy];
- y[iy] = temp;
- ix = ix + incx;
- iy = iy + incy;
- }
- }
- }
-
-
-
-
- void qr_solve(double x[], int m, int n, double a[], double b[])
-
-
-
- {
- double a_qr[n * m], qraux[n], r[m], tol;
- int ind, itask, jpvt[n], kr, lda;
-
- r8mat_copy(a_qr, m, n, a);
- lda = m;
- tol = r8_epsilon() / r8mat_amax(m, n, a_qr);
- itask = 1;
-
- ind = dqrls(a_qr, lda, m, n, tol, &kr, b, x, r, jpvt, qraux, itask); UNUSED(ind);
- }
-
-
- #endif
|