void AdaptiveManifoldFilterN::computeDTVer(vector<Mat>& srcCn, Mat& dst, float sigma_s, float sigma_r)
{
    int cnNum = (int)srcCn.size();
    int h = srcCn[0].rows;
    int w = srcCn[0].cols;

    dst.create(h-1, w, CV_32F);

    float sigmaRatioSqr = (float) SQR(sigma_s / sigma_r);
    float lnAlpha       = (float) (-sqrt(2.0) / sigma_s);

    for (int i = 0; i < h-1; i++)
    {
        float *dstRow = dst.ptr<float>(i);

        for (int cn = 0; cn < cnNum; cn++)
        {
            float *srcRow1 = srcCn[cn].ptr<float>(i);
            float *srcRow2 = srcCn[cn].ptr<float>(i+1);

            if (cn == 0)
                sqr_dif(dstRow, srcRow1, srcRow2, w);
            else
                add_sqr_dif(dstRow, srcRow1, srcRow2, w);
        }

        mad(dstRow, dstRow, sigmaRatioSqr, 1.0f, w);
        sqrt_(dstRow, dstRow, w);

        mul(dstRow, dstRow, lnAlpha, w);
        //Exp_32f(dstRow, dstRow, w);
    }

    cv::exp(dst, dst);
}
void vg_tess_clerp_approx(float *x, float *y, float x0, float y0, float x1, float y1, float t)
{
   float c = (x0 * x1) + (y0 * y1), s;
   t = t * t; /* because cos looks like a quadratic around x = 0 (ie this approximation is better when the angle is smaller) */
   c = (1.0f - t) + (t * c);
   s = sqrt_(_maxf(1.0f - (c * c), 0.0f)); /* 1 - c^2 should be >= 0, but we might end up slightly less than 0 due to fp lameness... */
   if ((x0 * y1) < (y0 * x1)) { s = -s; }
   *x = (x0 * c) - (y0 * s);
   *y = (y0 * c) + (x0 * s);
}
Exemplo n.º 3
0
int main(){
	int n;
	printf("Enter the number :");
	scanf("%d",&n);
	printf("sqrt(%d) :%d\n",n,sqrt_(n));
}
Exemplo n.º 4
0
namespace
{
RCP<const Basic> sqrt_(const RCP<const Basic> &arg)
{
    return pow(arg, div(one, i2));
}
}

RCP<const Basic> i3 = integer(3);
RCP<const Basic> i5 = integer(5);
RCP<const Basic> im2 = integer(-2);
RCP<const Basic> im3 = integer(-3);
RCP<const Basic> im5 = integer(-5);

RCP<const Basic> sq3 = sqrt_(i3);
RCP<const Basic> sq2 = sqrt_(i2);
RCP<const Basic> sq5 = sqrt_(i5);

RCP<const Basic> C0 = div(sub(sq3, one), mul(i2, sq2));
RCP<const Basic> C1 = div(one, i2);
RCP<const Basic> C2 = div(sq2, i2);
RCP<const Basic> C3 = div(sq3, i2);
RCP<const Basic> C4 = div(add(sq3, one), mul(i2, sq2));
RCP<const Basic> C5 = div(sqrt_(sub(i5, sqrt_(i5))), integer(8));
RCP<const Basic> C6 = div(sub(sqrt_(i5), one), integer(4));

RCP<const Basic> mC0 = mul(minus_one, C0);
RCP<const Basic> mC1 = mul(minus_one, C1);
RCP<const Basic> mC2 = mul(minus_one, C2);
RCP<const Basic> mC3 = mul(minus_one, C3);
Exemplo n.º 5
0
int TRL::dstqrb_(integer_ * n, doublereal_ * d__, doublereal_ * e,
			doublereal_ * z__, doublereal_ * work, integer_ * info)
{
	/*

	Purpose
	=======
	DSTQRB computes all eigenvalues and the last component of the eigenvectors of a
	symmetric tridiagonal matrix using the implicit QL or QR method.
	This is mainly a modification of the CLAPACK subroutine dsteqr.c

	Arguments
	=========

	N       (input) INTEGER
	The order of the matrix.  N >= 0.

	D       (input/output) DOUBLE PRECISION array, dimension (N)
	On entry, the diagonal elements of the tridiagonal matrix.
	On exit, if INFO = 0, the eigenvalues in ascending order.

	E       (input/output) DOUBLE PRECISION array, dimension (N-1)
	On entry, the (n-1) subdiagonal elements of the tridiagonal
	matrix.
	On exit, E has been destroyed.

	Z       (input/output) DOUBLE PRECISION array, dimension (LDZ, N)
	On entry, if  COMPZ = 'V', then Z contains the orthogonal
	matrix used in the reduction to tridiagonal form.
	On exit, if INFO = 0, then if  COMPZ = 'V', Z contains the
	orthonormal eigenvectors of the original symmetric matrix,
	and if COMPZ = 'I', Z contains the orthonormal eigenvectors
	of the symmetric tridiagonal matrix.
	If COMPZ = 'N', then Z is not referenced.

	WORK    (workspace) DOUBLE PRECISION array, dimension (max(1,2*N-2))
	If COMPZ = 'N', then WORK is not referenced.

	INFO    (output) INTEGER
	= 0:  successful exit
	< 0:  if INFO = -i, the i-th argument had an illegal value
	> 0:  the algorithm has failed to find all the eigenvalues in
	a total of 30*N iterations; if INFO = i, then i
	elements of E have not converged to zero; on exit, D
	and E contain the elements of a symmetric tridiagonal
	matrix which is orthogonally similar to the original
	matrix.

	=====================================================================
	*/
	/* Table of constant values */
	doublereal_ c_b10 = 1.;
	integer_ c__0 = 0;
	integer_ c__1 = 1;

	/* System generated locals */
	integer_ i__1, i__2;
	doublereal_ d__1, d__2;
	/* Builtin functions */
	//double sqrt_(doublereal_), d_sign(doublereal_ *, doublereal_ *);
	//extern /* Subroutine */ int dlae2_(doublereal_ *, doublereal_ *, doublereal_
	//	*, doublereal_ *, doublereal_ *);
	doublereal_ b, c__, f, g;
	integer_ i__, j, k, l, m;
	doublereal_ p, r__, s;
	//extern logical_ lsame_(char *, char *);
	//extern /* Subroutine */ int dlasr_(char *, char *, char *, integer_ *,
	//	integer_ *, doublereal_ *,
	//	doublereal_ *, doublereal_ *,
	//	integer_ *);
	doublereal_ anorm;
	//extern /* Subroutine */ int dswap_(integer_ *, doublereal_ *, integer_ *,
	//	doublereal_ *, integer_ *);
	integer_ l1;
	//extern /* Subroutine */ int dlaev2_(doublereal_ *, doublereal_ *,
	//	doublereal_ *, doublereal_ *,
	//	doublereal_ *, doublereal_ *,
	//	doublereal_ *);
	integer_ lendm1, lendp1;
	//extern doublereal_ dlapy2_(doublereal_ *, doublereal_ *);
	integer_ ii;
	//extern doublereal_ dlamch_(char *);
	integer_ mm, iscale;
	//extern /* Subroutine */ int dlascl_(char *, integer_ *, integer_ *,
	//	doublereal_ *, doublereal_ *,
	//	integer_ *, integer_ *, doublereal_ *,
	//	integer_ *, integer_ *),
	//	dlaset_(char *, integer_ *, integer_ *, doublereal_ *, doublereal_ *,
	//	doublereal_ *, integer_ *);
	doublereal_ safmin;
	//extern /* Subroutine */ int dlartg_(doublereal_ *, doublereal_ *,
	//	doublereal_ *, doublereal_ *,
	//	doublereal_ *);
	doublereal_ safmax;
	//extern /* Subroutine */ int xerbla_(char *, integer_ *);
	//extern doublereal_ dlanst_(char *, integer_ *, doublereal_ *,
	//	doublereal_ *);
	//extern /* Subroutine */ int dlasrt_(char *, integer_ *, doublereal_ *,
	//	integer_ *);
	/* Local variables */
	integer_ lend, jtot;
	integer_ lendsv;
	doublereal_ ssfmin;
	integer_ nmaxit;
	doublereal_ ssfmax;
	integer_ lm1, mm1, nm1;
	doublereal_ rt1, rt2, eps;
	integer_ lsv;
	doublereal_ tst, eps2;

	--d__;
	--e;
	--z__;
	/* z_dim1 = *ldz;             */
	/* z_offset = 1 + z_dim1 * 1; */
	/* z__ -= z_offset;           */
	--work;

	/* Function Body */
	*info = 0;
	/* Taken out for TRLan
	if (lsame_(compz, "N")) {
	icompz = 0;
	} else if (lsame_(compz, "V")) {
	icompz = 1;
	} else if (lsame_(compz, "I")) {
	icompz = 2;
	} else {
	icompz = -1;
	}
	if (icompz < 0) {
	*info = -1;
	} else if (*n < 0) {
	*info = -2;
	} else if (*ldz < 1 || icompz > 0 && *ldz < max(1,*n)) {
	*info = -6;
	}
	if (*info != 0) {
	i__1 = -(*info);
	xerbla_("DSTEQR", &i__1);
	return 0;
	}
	*/
	/*  icompz = 2; */

	/*	Quick return if possible */

	if (*n == 0) {
		return 0;
	}

	if (*n == 1) {
		z__[1] = 1;
		return 0;
	}

	/*	Determine the unit roundoff and over/underflow thresholds. */

	eps = dlamch_("E");
	/*	Computing 2nd power */
	d__1 = eps;
	eps2 = d__1 * d__1;
	safmin = dlamch_("S");
	safmax = 1. / safmin;
	ssfmax = sqrt_(safmax) / 3.;
	ssfmin = sqrt_(safmin) / eps2;

	/*	Compute the eigenvalues and eigenvectors of the tridiagonal
	matrix. */
	/* Taken out for TRLan
	if (icompz == 2) {
	dlaset_("Full", n, n, &c_b9, &c_b10, &z__[z_offset], ldz);
	}
	*/
	for (j = 1; j < *n; j++) {
		z__[j] = 0.0;
	}
	z__[*n] = 1.0;
	nmaxit = *n * 30;
	jtot = 0;

	/*	Determine where the matrix splits and choose QL or QR iteration
	for each block, according to whether top or bottom diagonal
	element is smaller. */

	l1 = 1;
	nm1 = *n - 1;

L10:
	if (l1 > *n) {
		goto L160;
	}
	if (l1 > 1) {
		e[l1 - 1] = 0.;
	}
	if (l1 <= nm1) {
		i__1 = nm1;
		for (m = l1; m <= i__1; ++m) {
			tst = (d__1 = e[m], fabs(d__1));
			if (tst == 0.) {
				goto L30;
			}
			if (tst <=
				sqrt_((d__1 = d__[m], fabs(d__1))) * sqrt_((d__2 =
				d__[m + 1],
				fabs(d__2))) *
				eps) {
					e[m] = 0.;
					goto L30;
			}
			/* L20: */
		}
	}
	m = *n;

L30:
	l = l1;
	lsv = l;
	lend = m;
	lendsv = lend;
	l1 = m + 1;
	if (lend == l) {
		goto L10;
	}

	/*	Scale submatrix in rows and columns L to LEND */

	i__1 = lend - l + 1;
	anorm = dlanst_("I", &i__1, &d__[l], &e[l]);
	iscale = 0;
	if (anorm == 0.) {
		goto L10;
	}
	if (anorm > ssfmax) {
		iscale = 1;
		i__1 = lend - l + 1;
		dlascl_("G", &c__0, &c__0, &anorm, &ssfmax, &i__1, &c__1, &d__[l],
			n, info);
		i__1 = lend - l;
		dlascl_("G", &c__0, &c__0, &anorm, &ssfmax, &i__1, &c__1, &e[l], n,
			info);
	} else if (anorm < ssfmin) {
		iscale = 2;
		i__1 = lend - l + 1;
		dlascl_("G", &c__0, &c__0, &anorm, &ssfmin, &i__1, &c__1, &d__[l],
			n, info);
		i__1 = lend - l;
		dlascl_("G", &c__0, &c__0, &anorm, &ssfmin, &i__1, &c__1, &e[l], n,
			info);
	}

	/*	Choose between QL and QR iteration */

	if ((d__1 = d__[lend], fabs(d__1)) < (d__2 = d__[l], fabs(d__2))) {
		lend = lsv;
		l = lendsv;
	}

	if (lend > l) {

		/*	QL Iteration

		Look for small subdiagonal element. */

L40:

		if (l != lend) {
			lendm1 = lend - 1;
			i__1 = lendm1;
			for (m = l; m <= i__1; ++m) {
				/*		Computing 2nd power */
				d__2 = (d__1 = e[m], fabs(d__1));
				tst = d__2 * d__2;
				if (tst <=
					eps2 * (d__1 = d__[m], fabs(d__1)) * (d__2 =
					d__[m + 1],
					fabs(d__2)) +
					safmin) {
						goto L60;
				}
				/* L50: */
			}
		}

		m = lend;

L60:
		if (m < lend) {
			e[m] = 0.;
		}
		p = d__[l];
		if (m == l) {
			goto L80;
		}

		/*	If remaining matrix is 2-by-2, use DLAE2 or SLAEV2
		to compute its eigensystem. */

		if (m == l + 1) {
			dlaev2_(&d__[l], &e[l], &d__[l + 1], &rt1, &rt2, &c__, &s);
			work[l] = c__;
			work[*n - 1 + l] = s;
			/* Taken out for TRLan
			dlasr_("R", "V", "B", n, &c__2, &work[l], &work[*n - 1 + l], &
			z___ref(1, l), ldz);
			*/
			tst = z__[l + 1];
			z__[l + 1] = c__ * tst - s * z__[l];
			z__[l] = s * tst + c__ * z__[l];
			d__[l] = rt1;
			d__[l + 1] = rt2;
			e[l] = 0.;
			l += 2;
			if (l <= lend) {
				goto L40;
			}
			goto L140;
		}

		if (jtot == nmaxit) {
			goto L140;
		}
		++jtot;

		/*	Form shift. */

		g = (d__[l + 1] - p) / (e[l] * 2.);
		r__ = dlapy2_(&g, &c_b10);
		g = d__[m] - p + e[l] / (g + d_sign(&r__, &g));

		s = 1.;
		c__ = 1.;
		p = 0.;

		/*	Inner loop */

		mm1 = m - 1;
		i__1 = l;
		for (i__ = mm1; i__ >= i__1; --i__) {
			f = s * e[i__];
			b = c__ * e[i__];
			dlartg_(&g, &f, &c__, &s, &r__);
			if (i__ != m - 1) {
				e[i__ + 1] = r__;
			}
			g = d__[i__ + 1] - p;
			r__ = (d__[i__] - g) * s + c__ * 2. * b;
			p = s * r__;
			d__[i__ + 1] = g + p;
			g = c__ * r__ - b;

			/*		If eigenvectors are desired, then save rotations. */

			work[i__] = c__;
			work[*n - 1 + i__] = -s;

			/* L70: */
		}

		/*	If eigenvectors are desired, then apply saved rotations. */

		mm = m - l + 1;
		/* Taken out for TRLan
		dlasr_("R", "V", "B", n, &mm, &work[l], &work[*n - 1 + l], &
		z___ref(1, l), ldz);
		*/
		dlasr_("R", "V", "B", &c__1, &mm, &work[l], &work[*n - 1 + l],
			&z__[l], &c__1);

		d__[l] -= p;
		e[l] = g;
		goto L40;

		/* 	Eigenvalue found. */

L80:
		d__[l] = p;

		++l;
		if (l <= lend) {
			goto L40;
		}
		goto L140;

	} else {

		/*	QR Iteration

		Look for small superdiagonal element. */

L90:
		if (l != lend) {
			lendp1 = lend + 1;
			i__1 = lendp1;
			for (m = l; m >= i__1; --m) {
				/*			Computing 2nd power */
				d__2 = (d__1 = e[m - 1], fabs(d__1));
				tst = d__2 * d__2;
				if (tst <=
					eps2 * (d__1 = d__[m], fabs(d__1)) * (d__2 =
					d__[m - 1],
					fabs(d__2)) +
					safmin) {
						goto L110;
				}
				/* L100: */
			}
		}

		m = lend;

L110:
		if (m > lend) {
			e[m - 1] = 0.;
		}
		p = d__[l];
		if (m == l) {
			goto L130;
		}

		/*	If remaining matrix is 2-by-2, use DLAE2 or SLAEV2
		to compute its eigensystem. */

		if (m == l - 1) {
			dlaev2_(&d__[l - 1], &e[l - 1], &d__[l], &rt1, &rt2, &c__, &s);
			/* Taken out for TRLan
			work[m] = c__;
			work[*n - 1 + m] = s;
			dlasr_("R", "V", "F", n, &c__2, &work[m], &work[*n - 1 + m], &
			z___ref(1, l - 1), ldz);
			*/
			tst = z__[l];
			z__[l] = c__ * tst - s * z__[l - 1];
			z__[l - 1] = s * tst + c__ * z__[l - 1];

			d__[l - 1] = rt1;
			d__[l] = rt2;
			e[l - 1] = 0.;
			l += -2;
			if (l >= lend) {
				goto L90;
			}
			goto L140;
		}

		if (jtot == nmaxit) {
			goto L140;
		}
		++jtot;

		/*	Form shift. */

		g = (d__[l - 1] - p) / (e[l - 1] * 2.);
		r__ = dlapy2_(&g, &c_b10);
		g = d__[m] - p + e[l - 1] / (g + d_sign(&r__, &g));

		s = 1.;
		c__ = 1.;
		p = 0.;

		/*	Inner loop */

		lm1 = l - 1;
		i__1 = lm1;
		for (i__ = m; i__ <= i__1; ++i__) {
			f = s * e[i__];
			b = c__ * e[i__];
			dlartg_(&g, &f, &c__, &s, &r__);
			if (i__ != m) {
				e[i__ - 1] = r__;
			}
			g = d__[i__] - p;
			r__ = (d__[i__ + 1] - g) * s + c__ * 2. * b;
			p = s * r__;
			d__[i__] = g + p;
			g = c__ * r__ - b;

			/*		If eigenvectors are desired, then save rotations. */

			work[i__] = c__;
			work[*n - 1 + i__] = s;

			/* L120: */
		}

		/*	If eigenvectors are desired, then apply saved rotations. */

		mm = l - m + 1;
		/*
		dlasr_("R", "V", "F", n, &mm, &work[m], &work[*n - 1 + m], &
		z___ref(1, m), ldz);
		*/
		dlasr_("R", "V", "F", &c__1, &mm, &work[m], &work[*n - 1 + m],
			&z__[m], &c__1);

		d__[l] -= p;
		e[lm1] = g;
		goto L90;

		/*        Eigenvalue found. */

L130:
		d__[l] = p;

		--l;
		if (l >= lend) {
			goto L90;
		}
		goto L140;

	}

	/*     Undo scaling if necessary */

L140:
	if (iscale == 1) {
		i__1 = lendsv - lsv + 1;
		dlascl_("G", &c__0, &c__0, &ssfmax, &anorm, &i__1, &c__1,
			&d__[lsv], n, info);
		i__1 = lendsv - lsv;
		dlascl_("G", &c__0, &c__0, &ssfmax, &anorm, &i__1, &c__1, &e[lsv],
			n, info);
	} else if (iscale == 2) {
		i__1 = lendsv - lsv + 1;
		dlascl_("G", &c__0, &c__0, &ssfmin, &anorm, &i__1, &c__1,
			&d__[lsv], n, info);
		i__1 = lendsv - lsv;
		dlascl_("G", &c__0, &c__0, &ssfmin, &anorm, &i__1, &c__1, &e[lsv],
			n, info);
	}

	/*     Check for no convergence to an eigenvalue after a total
	of N*MAXIT iterations. */

	if (jtot < nmaxit) {
		goto L10;
	}
	i__1 = *n - 1;
	for (i__ = 1; i__ <= i__1; ++i__) {
		if (e[i__] != 0.) {
			++(*info);
		}
		/* L150: */
	}
	goto L190;

	/*     Order eigenvalues and eigenvectors. */

L160:

	/*        Use Selection Sort to minimize swaps of eigenvectors */

	i__1 = *n;
	for (ii = 2; ii <= i__1; ++ii) {
		i__ = ii - 1;
		k = i__;
		p = d__[i__];
		i__2 = *n;
		for (j = ii; j <= i__2; ++j) {
			if (d__[j] < p) {
				k = j;
				p = d__[j];
			}
			/* L170: */
		}
		if (k != i__) {
			d__[k] = d__[i__];
			d__[i__] = p;
			/* Taken out for TRLan
			dswap_(n, &z___ref(1, i__), &c__1, &z___ref(1, k), &c__1);
			*/
			p = z__[k];
			z__[k] = z__[i__];
			z__[i__] = p;
		}
		/* L180: */
	}

L190:
	return 0;
}	
uint32_t vg_tess_arc_to_cubics(
   float *p,
   float p0_x, float p0_y, float p1_x, float p1_y, float p1_m_p0_x, float p1_m_p0_y,
   float r_x, float r_y, float angle,
   bool large, bool cw)
{
   float u1_x, u1_y;
   float c_x, c_y; /* center */
   float d_sq, s_sq;
   float u0_angle, u1_angle;
   uint32_t count;
   /* coverity [uninit] : Coverity doesn't follow initialisation in vg_mat3x3_set_identity() */
   VG_MAT3X3_T a;
   uint32_t i;

   if ((r_x < EPS) || (r_y < EPS)) {
      /* degenerate ellipse, approximate as line */
      line_to_cubic(p, p0_x, p0_y, p1_x, p1_y);
      return 1;
   }

   /*
      transform p0/p1 - p0 on ellipse - p0 to u0/u1 on unit circle with center c
   */

   /* u0 = (0, 0) */
   ellipse_to_unit_circle(&u1_x, &u1_y, p1_m_p0_x, p1_m_p0_y, r_x, r_y, angle);

   /*
      d = chord length = length(u1 - u0)
      l = distance from middle of chord to c = sqrt(1 - (d/2)^2)
      s = l/d = sqrt(1/d^2 - 1/4)
   */

   d_sq = (u1_x * u1_x) + (u1_y * u1_y);
   if (d_sq < EPS) {
      /* points almost coincident, approximate as line */
      line_to_cubic(p, p0_x, p0_y, p1_x, p1_y);
      return 1;
   }
   s_sq = recip_(d_sq) - 0.25f;
   if (s_sq < 0.0f) {
      /*
         chord length >= 2, points are too far apart for both to be able to lie on ellipse
         spec says should uniformly scale ellipse so that it is just big enough
         this will result in a chord of length 2, with c = (u0 + u1)/2
         scaling ellipse by s will cause chord length to scale by 1/s
         want chord length = 2, so want to scale chord length by 2/d, so want to scale ellipse by d/2
      */

      float scale = 0.5f * sqrt_(d_sq);
      r_x *= scale;
      r_y *= scale;
      ellipse_to_unit_circle(&u1_x, &u1_y, p1_m_p0_x, p1_m_p0_y, r_x, r_y, angle);
      c_x = u1_x * 0.5f;
      c_y = u1_y * 0.5f;
   } else {
      float s;

      /*
         moving a distance l perpendicular to chord from middle of chord m will get us to the two possible centers
         which center to use depends on large/cw
      */

      c_x = u1_x * 0.5f;
      c_y = u1_y * 0.5f;
      s = sqrt_(s_sq);
      if (large ^ cw) {
         c_x += s * u1_y;
         c_y -= s * u1_x;
      } else {
         c_x -= s * u1_y;
         c_y += s * u1_x;
      }
   }

   /*
      approximate arc along unit circle
   */

   u0_angle = atan2_(-c_y, -c_x);
   u1_angle = atan2_(u1_y - c_y, u1_x - c_x);
   count = unit_arc_to_cubics(p, u1_angle - u0_angle, cw);
   vcos_assert((count != 0) && (count <= 4));

   /*
      transform cubics to ellipse

      rotate ccw by u0_angle
      translate by c
      scale by r
      rotate ccw by angle
      translate by p0
   */

   vg_mat3x3_set_identity(&a);
   vg_mat3x3_premul_rotate(&a, u0_angle);
   vg_mat3x3_premul_translate(&a, c_x, c_y);
   vg_mat3x3_premul_scale(&a, r_x, r_y);
   vg_mat3x3_premul_rotate(&a, angle);
   vg_mat3x3_premul_translate(&a, p0_x, p0_y);
#ifndef NDEBUG
   vcos_verify(vg_mat3x3_is_affine(&a)); /* why vcos_verify? see vg_mat3x3_is_affine... */
#endif

   for (i = 0; i != (count * 8); i += 2) {
      vg_mat3x3_affine_transform(&a, p + i, p + i + 1);
   }

   /*
      force first and last points to coincide exactly with p0/p1
   */

   p[0] = p0_x;
   p[1] = p0_y;
   p[(count * 8) - 2] = p1_x;
   p[(count * 8) - 1] = p1_y;

   return count;
}
void vg_mat3x3_rsq(const VG_MAT3X3_T *a, /* only top-left 2x2 matrix used */
   float *r, float *s0, float *s1) /* don't need q for anything atm, so not calculated */
{
   /*
      a = r * s * q (svd, r is rotation, s is scale, q is rotation/flip)
      a^T = q^T * s * r^T
      a * a^T = r * s * q * q^T * s * r^T = r * s^2 * r^T

      eigenvalues of a * a^T will give s^2
      eigenvectors of a * a^T will give r
   */

   /*
      ( b c ) = a * a^T
      ( d e )
   */

   float b = (a->m[0][0] * a->m[0][0]) + (a->m[0][1] * a->m[0][1]);
   float c = (a->m[0][0] * a->m[1][0]) + (a->m[0][1] * a->m[1][1]);
   /* d = c */
   float e = (a->m[1][0] * a->m[1][0]) + (a->m[1][1] * a->m[1][1]);

   float bpe = b + e;
   float bme = b - e;

   /*
      solve:

      bx + cy = sx
      dx + ey = sy

      cy * dx = (s - b)x * (s - e)y
      c^2 = (s - b) * (s - e)
      s^2 - (b + e)s + (be - c^2) = 0
      s = (b + e +/- sqrt((b + e)^2 - 4(be - c^2))) / 2
      s = (b + e +/- sqrt(b^2 + e^2 - 2be + 4c^2)) / 2
      s = (b + e +/- sqrt((b - e)^2 + 4c^2)) / 2
   */

   float t = sqrt_((bme * bme) + (4.0f * c * c));
   float v = (bpe + t) * 0.5f; /* first eigenvalue */
   if (s0) {
      *s0 = sqrt_(v);
   }
   if (s1) {
      *s1 = sqrt_(
         /* second eigenvalue */
         _maxf(bpe - t, 0.0f) * 0.5f);
   }

   /*
      angle of eigenvector corresponds to r
   */

   if (r) {
      /* first eigenvector is (c, v - b) / (v - e, c) */
      float x = (v - e) + c;
      float y = (v - b) + c;
      *r = ((absf_(x) < EPS) && (absf_(y) < EPS)) ? 0.0f : atan2_(y, x);
   }
}
Exemplo n.º 8
0
int64_t sqrt__(int64_t val){
	return (int64_t)sqrt_((float)val);
}