/*
* NOTE: this implementation follows Standard SVG 1.1 implementation guidelines
* for elliptical arc curves. See Appendix F.6.
*/
void EllipticalArc::_updateCenterAndAngles(bool svg)
{
Point d = initialPoint() - finalPoint();
// TODO move this to SVGElipticalArc?
if (svg)
{
if ( initialPoint() == finalPoint() )
{
_rot_angle = _start_angle = _end_angle = 0;
_center = initialPoint();
_rays = Geom::Point(0,0);
_large_arc = _sweep = false;
return;
}
_rays[X] = std::fabs(_rays[X]);
_rays[Y] = std::fabs(_rays[Y]);
if ( are_near(ray(X), 0) || are_near(ray(Y), 0) )
{
_rays[X] = L2(d) / 2;
_rays[Y] = 0;
_rot_angle = std::atan2(d[Y], d[X]);
_start_angle = 0;
_end_angle = M_PI;
_center = middle_point(initialPoint(), finalPoint());
_large_arc = false;
_sweep = false;
return;
}
}
else
{
if ( are_near(initialPoint(), finalPoint()) )
{
if ( are_near(ray(X), 0) && are_near(ray(Y), 0) )
{
_start_angle = _end_angle = 0;
_center = initialPoint();
return;
}
else
{
THROW_RANGEERROR("initial and final point are the same");
}
}
if ( are_near(ray(X), 0) && are_near(ray(Y), 0) )
{ // but initialPoint != finalPoint
THROW_RANGEERROR(
"there is no ellipse that satisfies the given constraints: "
"ray(X) == 0 && ray(Y) == 0 but initialPoint != finalPoint"
);
}
if ( are_near(ray(Y), 0) )
{
Point v = initialPoint() - finalPoint();
if ( are_near(L2sq(v), 4*ray(X)*ray(X)) )
{
Angle angle(v);
if ( are_near( angle, _rot_angle ) )
{
_start_angle = 0;
_end_angle = M_PI;
_center = v/2 + finalPoint();
return;
}
angle -= M_PI;
if ( are_near( angle, _rot_angle ) )
{
_start_angle = M_PI;
_end_angle = 0;
_center = v/2 + finalPoint();
return;
}
THROW_RANGEERROR(
"there is no ellipse that satisfies the given constraints: "
"ray(Y) == 0 "
"and slope(initialPoint - finalPoint) != rotation_angle "
"and != rotation_angle + PI"
);
}
if ( L2sq(v) > 4*ray(X)*ray(X) )
{
THROW_RANGEERROR(
"there is no ellipse that satisfies the given constraints: "
"ray(Y) == 0 and distance(initialPoint, finalPoint) > 2*ray(X)"
);
}
else
{
THROW_RANGEERROR(
"there is infinite ellipses that satisfy the given constraints: "
"ray(Y) == 0 and distance(initialPoint, finalPoint) < 2*ray(X)"
);
}
}
if ( are_near(ray(X), 0) )
{
Point v = initialPoint() - finalPoint();
if ( are_near(L2sq(v), 4*ray(Y)*ray(Y)) )
{
double angle = std::atan2(v[Y], v[X]);
if (angle < 0) angle += 2*M_PI;
double rot_angle = _rot_angle.radians() + M_PI/2;
if ( !(rot_angle < 2*M_PI) ) rot_angle -= 2*M_PI;
if ( are_near( angle, rot_angle ) )
{
_start_angle = M_PI/2;
_end_angle = 3*M_PI/2;
_center = v/2 + finalPoint();
return;
}
angle -= M_PI;
if ( angle < 0 ) angle += 2*M_PI;
if ( are_near( angle, rot_angle ) )
{
_start_angle = 3*M_PI/2;
_end_angle = M_PI/2;
_center = v/2 + finalPoint();
return;
}
THROW_RANGEERROR(
"there is no ellipse that satisfies the given constraints: "
"ray(X) == 0 "
"and slope(initialPoint - finalPoint) != rotation_angle + PI/2 "
"and != rotation_angle + (3/2)*PI"
);
}
if ( L2sq(v) > 4*ray(Y)*ray(Y) )
{
THROW_RANGEERROR(
"there is no ellipse that satisfies the given constraints: "
"ray(X) == 0 and distance(initialPoint, finalPoint) > 2*ray(Y)"
);
}
else
{
THROW_RANGEERROR(
"there is infinite ellipses that satisfy the given constraints: "
"ray(X) == 0 and distance(initialPoint, finalPoint) < 2*ray(Y)"
);
}
}
}
Rotate rm(_rot_angle);
Affine m(rm);
m[1] = -m[1];
m[2] = -m[2];
Point p = (d / 2) * m;
double rx2 = _rays[X] * _rays[X];
double ry2 = _rays[Y] * _rays[Y];
double rxpy = _rays[X] * p[Y];
double rypx = _rays[Y] * p[X];
double rx2py2 = rxpy * rxpy;
double ry2px2 = rypx * rypx;
double num = rx2 * ry2;
double den = rx2py2 + ry2px2;
assert(den != 0);
double rad = num / den;
Point c(0,0);
if (rad > 1)
{
rad -= 1;
rad = std::sqrt(rad);
if (_large_arc == _sweep) rad = -rad;
c = rad * Point(rxpy / ray(Y), -rypx / ray(X));
_center = c * rm + middle_point(initialPoint(), finalPoint());
}
else if (rad == 1 || svg)
{
double lamda = std::sqrt(1 / rad);
_rays[X] *= lamda;
_rays[Y] *= lamda;
_center = middle_point(initialPoint(), finalPoint());
}
else
{
THROW_RANGEERROR(
"there is no ellipse that satisfies the given constraints"
);
}
Point sp((p[X] - c[X]) / ray(X), (p[Y] - c[Y]) / ray(Y));
Point ep((-p[X] - c[X]) / ray(X), (-p[Y] - c[Y]) / ray(Y));
Point v(1, 0);
_start_angle = angle_between(v, sp);
double sweep_angle = angle_between(sp, ep);
if (!_sweep && sweep_angle > 0) sweep_angle -= 2*M_PI;
if (_sweep && sweep_angle < 0) sweep_angle += 2*M_PI;
_end_angle = _start_angle;
_end_angle += sweep_angle;
}
[8] = { { 810000, HFPLL, 1, 0x1E }, 1100000, 1100000, 4 },
[9] = { { 864000, HFPLL, 1, 0x20 }, 1100000, 1100000, 4 },
[10] = { { 918000, HFPLL, 1, 0x22 }, 1100000, 1100000, 5 },
[11] = { { 972000, HFPLL, 1, 0x24 }, 1100000, 1100000, 5 },
[12] = { { 1026000, HFPLL, 1, 0x26 }, 1100000, 1100000, 5 },
[13] = { { 1080000, HFPLL, 1, 0x28 }, 1100000, 1100000, 5 },
[14] = { { 1134000, HFPLL, 1, 0x2A }, 1100000, 1100000, 5 },
[15] = { { 1188000, HFPLL, 1, 0x2C }, 1100000, 1100000, 5 },
[16] = { { 1242000, HFPLL, 1, 0x2E }, 1100000, 1100000, 5 },
[17] = { { 1296000, HFPLL, 1, 0x30 }, 1100000, 1100000, 5 },
[18] = { { 1350000, HFPLL, 1, 0x32 }, 1100000, 1100000, 5 },
{ }
};
static struct acpu_level tbl_slow[] __initdata = {
{ 1, { 384000, PLL_8, 0, 0x00 }, L2(0), 950000 },
{ 1, { 486000, HFPLL, 2, 0x24 }, L2(5), 975000 },
{ 1, { 594000, HFPLL, 1, 0x16 }, L2(5), 1000000 },
{ 1, { 702000, HFPLL, 1, 0x1A }, L2(5), 1025000 },
{ 1, { 810000, HFPLL, 1, 0x1E }, L2(5), 1075000 },
{ 1, { 918000, HFPLL, 1, 0x22 }, L2(5), 1100000 },
{ 1, { 1026000, HFPLL, 1, 0x26 }, L2(5), 1125000 },
{ 1, { 1134000, HFPLL, 1, 0x2A }, L2(16), 1175000 },
{ 1, { 1242000, HFPLL, 1, 0x2E }, L2(16), 1200000 },
{ 1, { 1350000, HFPLL, 1, 0x32 }, L2(16), 1225000 },
{ 1, { 1458000, HFPLL, 1, 0x36 }, L2(16), 1237500 },
{ 1, { 1512000, HFPLL, 1, 0x38 }, L2(16), 1250000 },
{ 1, { 1620000, HFPLL, 1, 0x3C }, L2(16), 1275000 },
{ 1, { 1728000, HFPLL, 1, 0x40 }, L2(16), 1300000 },
// { 1, { 1836000, HFPLL, 1, 0x44 }, L2(16), 1312500 },
// { 1, { 1944000, HFPLL, 1, 0x48 }, L2(14), 1325000 },
extern "C" magma_int_t
magma_ctstrf_gpu( char storev, magma_int_t m, magma_int_t n, magma_int_t ib, magma_int_t nb,
magmaFloatComplex *hU, magma_int_t ldhu, magmaFloatComplex *dU, magma_int_t lddu,
magmaFloatComplex *hA, magma_int_t ldha, magmaFloatComplex *dA, magma_int_t ldda,
magmaFloatComplex *hL, magma_int_t ldhl, magmaFloatComplex *dL, magma_int_t lddl,
magma_int_t *ipiv,
magmaFloatComplex *hwork, magma_int_t ldhwork, magmaFloatComplex *dwork, magma_int_t lddwork,
magma_int_t *info)
{
/* -- MAGMA (version 1.4.0) --
Univ. of Tennessee, Knoxville
Univ. of California, Berkeley
Univ. of Colorado, Denver
August 2013
Purpose
=======
CSSSSM applies the LU factorization update from a complex
matrix formed by a lower triangular IB-by-K tile L1 on top of a
M2-by-K tile L2 to a second complex matrix formed by a M1-by-N1
tile A1 on top of a M2-by-N2 tile A2 (N1 == N2).
This is the right-looking Level 2.5 BLAS version of the algorithm.
Arguments
=========
M (input) INTEGER
The number of rows of the matrix A. M >= 0.
N (input) INTEGER
The number of columns of the matrix A. N >= 0.
IB (input) INTEGER
The inner-blocking size. IB >= 0.
NB (input) INTEGER
The blocking size. NB >= 0.
hU (input,output) COMPLEX array, dimension(LDHU, N), on cpu.
On entry, the NB-by-N upper triangular tile hU.
On exit, the content is incomplete. Shouldn't be used.
LDHU (input) INTEGER
The leading dimension of the array hU. LDHU >= max(1,NB).
dU (input,output) COMPLEX array, dimension(LDDU, N), on gpu.
On entry, the NB-by-N upper triangular tile dU identical to hU.
On exit, the new factor U from the factorization.
LDDU (input) INTEGER
The leading dimension of the array dU. LDDU >= max(1,NB).
hA (input,output) COMPLEX array, dimension(LDHA, N), on cpu.
On entry, only the M-by-IB first panel needs to be identical to dA(1..M, 1..IB).
On exit, the content is incomplete. Shouldn't be used.
LDHA (input) INTEGER
The leading dimension of the array hA. LDHA >= max(1,M).
dA (input,output) COMPLEX array, dimension(LDDA, N) , on gpu.
On entry, the M-by-N tile to be factored.
On exit, the factor L from the factorization
LDDA (input) INTEGER
The leading dimension of the array dA. LDDA >= max(1,M).
hL (output) COMPLEX array, dimension(LDHL, K), on vpu.
On exit, contains in the upper part the IB-by-K lower triangular tile,
and in the lower part IB-by-K the inverse of the top part.
LDHL (input) INTEGER
The leading dimension of the array hL. LDHL >= max(1,2*IB).
dL (output) COMPLEX array, dimension(LDDL, K), on gpu.
On exit, contains in the upper part the IB-by-K lower triangular tile,
and in the lower part IB-by-K the inverse of the top part.
LDDL (input) INTEGER
The leading dimension of the array dL. LDDL >= max(1,2*IB).
hWORK (output) COMPLEX array, dimension(LDHWORK, 2*IB), on cpu.
Workspace.
LDHWORK (input) INTEGER
The leading dimension of the array hWORK. LDHWORK >= max(NB, 1).
dWORK (output) COMPLEX array, dimension(LDDWORK, 2*IB), on gpu.
Workspace.
LDDWORK (input) INTEGER
The leading dimension of the array dWORK. LDDWORK >= max(NB, 1).
IPIV (output) INTEGER array on the cpu.
The pivot indices array of size K as returned by CTSTRF
INFO (output) INTEGER
- PLASMA_SUCCESS successful exit
- < 0 if INFO = -k, the k-th argument had an illegal value
- > 0 if INFO = k, U(k,k) is exactly zero. The factorization
has been completed, but the factor U is exactly
singular, and division by zero will occur if it is used
to solve a system of equations.
===================================================================== */
#define UT(i,j) (dUT + (i)*ib*lddu + (j)*ib )
#define AT(i,j) (dAT + (i)*ib*ldda + (j)*ib )
#define L(i) (dL + (i)*ib*lddl )
#define L2(i) (dL2 + (i)*ib*lddl )
#define hU(i,j) (hU + (j)*ib*ldhu + (i)*ib )
#define hA(i,j) (hA + (j)*ib*ldha + (i)*ib )
#define hL(i) (hL + (i)*ib*ldhl )
#define hL2(i) (hL2 + (i)*ib*ldhl )
magmaFloatComplex c_one = MAGMA_C_ONE;
magmaFloatComplex c_neg_one = MAGMA_C_NEG_ONE;
int iinfo = 0;
int maxm, mindim;
int i, j, im, s, ip, ii, sb, p = 1;
magmaFloatComplex *dAT, *dUT;
magmaFloatComplex *dAp, *dUp;
#ifndef WITHOUTTRTRI
magmaFloatComplex *dL2 = dL + ib;
magmaFloatComplex *hL2 = hL + ib;
p = 2;
#endif
/* Check input arguments */
*info = 0;
if (m < 0) {
*info = -1;
}
else if (n < 0) {
*info = -2;
}
else if (ib < 0) {
*info = -3;
}
else if ((lddu < max(1,m)) && (m > 0)) {
*info = -6;
}
else if ((ldda < max(1,m)) && (m > 0)) {
*info = -8;
}
else if ((lddl < max(1,ib)) && (ib > 0)) {
*info = -10;
}
if (*info != 0) {
magma_xerbla( __func__, -(*info) );
return *info;
}
/* quick return */
if ((m == 0) || (n == 0) || (ib == 0))
return *info;
ip = 0;
/* Function Body */
mindim = min(m, n);
s = mindim / ib;
if ( ib >= mindim ) {
/* Use CPU code. */
CORE_ctstrf(m, n, ib, nb,
(PLASMA_Complex32_t*)hU, ldhu,
(PLASMA_Complex32_t*)hA, ldha,
(PLASMA_Complex32_t*)hL, ldhl,
ipiv,
(PLASMA_Complex32_t*)hwork, ldhwork,
info);
#ifndef WITHOUTTRTRI
CORE_clacpy( PlasmaUpperLower, mindim, mindim,
(PLASMA_Complex32_t*)hL, ldhl,
(PLASMA_Complex32_t*)hL2, ldhl );
CORE_ctrtri( PlasmaLower, PlasmaUnit, mindim,
(PLASMA_Complex32_t*)hL2, ldhl, info );
if (*info != 0 ) {
fprintf(stderr, "ERROR, trtri returned with info = %d\n", *info);
}
#endif
if ( (storev == 'R') || (storev == 'r') ) {
magma_csetmatrix( m, n, hU, ldhu, dwork, lddwork );
magmablas_ctranspose( dU, lddu, dwork, lddwork, m, n );
magma_csetmatrix( m, n, hA, ldha, dwork, lddwork );
magmablas_ctranspose( dA, ldda, dwork, lddwork, m, n );
} else {
magma_csetmatrix( m, n, hU, ldhu, dU, lddu );
magma_csetmatrix( m, n, hA, ldha, dA, ldda );
}
magma_csetmatrix( p*ib, n, hL, ldhl, dL, lddl );
}
else {
/* Use hybrid blocked code. */
maxm = ((m + 31)/32)*32;
if ( (storev == 'C') || (storev == 'c') ) {
magmablas_cgetmo_in( dU, dUT, lddu, m, n );
magmablas_cgetmo_in( dA, dAT, ldda, m, n );
} else {
dUT = dU; dAT = dA;
}
dAp = dwork;
dUp = dAp + ib*lddwork;
ip = 0;
for( i=0; i<s; i++ )
{
ii = i * ib;
sb = min(mindim-ii, ib);
if ( i>0 ){
// download i-th panel
magmablas_ctranspose( dUp, lddu, UT(0, i), lddu, sb, ii );
magmablas_ctranspose( dAp, ldda, AT(0, i), ldda, sb, m );
magma_cgetmatrix( ii, sb, dUp, lddu, hU(0, i), ldhu );
magma_cgetmatrix( m, sb, dAp, ldda, hA(0, i), ldha );
// make sure that gpu queue is empty
//magma_device_sync();
#ifndef WITHOUTTRTRI
magma_ctrmm( MagmaRight, MagmaLower, MagmaTrans, MagmaUnit,
n-(ii+sb), ib,
c_one, L2(i-1), lddl,
UT(i-1, i+1), lddu);
#else
magma_ctrsm( MagmaRight, MagmaLower, MagmaTrans, MagmaUnit,
n-(ii+sb), ib,
c_one, L(i-1), lddl,
UT(i-1, i+1), lddu);
#endif
magma_cgemm( MagmaNoTrans, MagmaNoTrans,
n-(ii+sb), m, ib,
c_neg_one, UT(i-1, i+1), lddu,
AT(0, i-1), ldda,
c_one, AT(0, i+1), ldda );
}
// do the cpu part
CORE_ctstrf(m, sb, ib, nb,
(PLASMA_Complex32_t*)hU(i, i), ldhu,
(PLASMA_Complex32_t*)hA(0, i), ldha,
(PLASMA_Complex32_t*)hL(i), ldhl,
ipiv+ii,
(PLASMA_Complex32_t*)hwork, ldhwork,
info);
if ( (*info == 0) && (iinfo > 0) )
*info = iinfo + ii;
// Need to swap betw U and A
#ifndef NOSWAPBLK
magmablas_cswapblk( 'R', n-(ii+sb),
UT(i, i+1), lddu,
AT(0, i+1), ldda,
1, sb, ipiv+ii, 1, nb );
for(j=0; j<ib; j++) {
im = ipiv[ip]-1;
if ( im == j ) {
ipiv[ip] += ii;
}
ip++;
}
#else
for(j=0; j<ib; j++) {
im = ipiv[ip]-1;
if ( im != (j) ) {
im = im - nb;
assert( (im>=0) && (im<m) );
magmablas_cswap( n-(ii+sb), UT(i, i+1)+j*lddu, 1, AT(0, i+1)+im*ldda, 1 );
} else {
ipiv[ip] += ii;
}
ip++;
}
#endif
#ifndef WITHOUTTRTRI
CORE_clacpy( PlasmaUpperLower, sb, sb,
(PLASMA_Complex32_t*)hL(i), ldhl,
(PLASMA_Complex32_t*)hL2(i), ldhl );
CORE_ctrtri( PlasmaLower, PlasmaUnit, sb,
(PLASMA_Complex32_t*)hL2(i), ldhl, info );
if (*info != 0 ) {
fprintf(stderr, "ERROR, trtri returned with info = %d\n", *info);
}
#endif
// upload i-th panel
magma_csetmatrix( sb, sb, hU(i, i), ldhu, dUp, lddu );
magma_csetmatrix( m, sb, hA(0, i), ldha, dAp, ldda );
magma_csetmatrix( p*ib, sb, hL(i), ldhl, L(i), lddl );
magmablas_ctranspose( UT(i, i), lddu, dUp, lddu, sb, sb);
magmablas_ctranspose( AT(0, i), ldda, dAp, ldda, m, sb);
// make sure that gpu queue is empty
//magma_device_sync();
// do the small non-parallel computations
if ( s > (i+1) ) {
#ifndef WITHOUTTRTRI
magma_ctrmm( MagmaRight, MagmaLower, MagmaTrans, MagmaUnit,
sb, sb,
c_one, L2(i), lddl,
UT(i, i+1), lddu);
#else
magma_ctrsm( MagmaRight, MagmaLower, MagmaTrans, MagmaUnit,
sb, sb,
c_one, L(i), lddl,
UT(i, i+1), lddu);
#endif
magma_cgemm( MagmaNoTrans, MagmaNoTrans,
sb, m, sb,
c_neg_one, UT(i, i+1), lddu,
AT(0, i ), ldda,
c_one, AT(0, i+1), ldda );
}
else {
#ifndef WITHOUTTRTRI
magma_ctrmm( MagmaRight, MagmaLower, MagmaTrans, MagmaUnit,
n-mindim, sb,
c_one, L2(i), lddl,
UT(i, i+1), lddu);
#else
magma_ctrsm( MagmaRight, MagmaLower, MagmaTrans, MagmaUnit,
n-mindim, sb,
c_one, L(i), lddl,
UT(i, i+1), lddu);
#endif
magma_cgemm( MagmaNoTrans, MagmaNoTrans,
n-mindim, m, sb,
c_neg_one, UT(i, i+1), lddu,
AT(0, i ), ldda,
c_one, AT(0, i+1), ldda );
}
}
if ( (storev == 'C') || (storev == 'c') ) {
magmablas_cgetmo_out( dU, dUT, lddu, m, n );
magmablas_cgetmo_out( dA, dAT, ldda, m, n );
}
}
return *info;
}
[4] = { { 594000, HFPLL, 1, 0x16 }, 1050000, 1050000, 2 },
[5] = { { 648000, HFPLL, 1, 0x18 }, 1050000, 1050000, 4 },
[6] = { { 702000, HFPLL, 1, 0x1A }, 1150000, 1150000, 4 },
[7] = { { 756000, HFPLL, 1, 0x1C }, 1150000, 1150000, 4 },
[8] = { { 810000, HFPLL, 1, 0x1E }, 1150000, 1150000, 4 },
[9] = { { 864000, HFPLL, 1, 0x20 }, 1150000, 1150000, 4 },
[10] = { { 918000, HFPLL, 1, 0x22 }, 1150000, 1150000, 5 },
[11] = { { 972000, HFPLL, 1, 0x24 }, 1150000, 1150000, 5 },
[12] = { { 1026000, HFPLL, 1, 0x26 }, 1150000, 1150000, 5 },
[13] = { { 1080000, HFPLL, 1, 0x28 }, 1150000, 1150000, 5 },
[14] = { { 1134000, HFPLL, 1, 0x2A }, 1150000, 1150000, 5 },
{ }
};
static struct acpu_level tbl_slow[] __initdata = {
{ 1, { 384000, PLL_8, 0, 0x00 }, L2(0), 950000 },
{ 0, { 432000, HFPLL, 2, 0x20 }, L2(5), 975000 },
{ 1, { 486000, HFPLL, 2, 0x24 }, L2(5), 975000 },
{ 0, { 540000, HFPLL, 2, 0x28 }, L2(5), 1000000 },
{ 1, { 594000, HFPLL, 1, 0x16 }, L2(5), 1000000 },
{ 0, { 648000, HFPLL, 1, 0x18 }, L2(5), 1025000 },
{ 1, { 702000, HFPLL, 1, 0x1A }, L2(5), 1025000 },
{ 0, { 756000, HFPLL, 1, 0x1C }, L2(5), 1075000 },
{ 1, { 810000, HFPLL, 1, 0x1E }, L2(5), 1075000 },
{ 0, { 864000, HFPLL, 1, 0x20 }, L2(5), 1100000 },
{ 1, { 918000, HFPLL, 1, 0x22 }, L2(5), 1100000 },
{ 0, { 972000, HFPLL, 1, 0x24 }, L2(5), 1125000 },
{ 1, { 1026000, HFPLL, 1, 0x26 }, L2(5), 1125000 },
{ 0, { 1080000, HFPLL, 1, 0x28 }, L2(14), 1175000 },
{ 1, { 1134000, HFPLL, 1, 0x2A }, L2(14), 1175000 },
{ 0, { 1188000, HFPLL, 1, 0x2C }, L2(14), 1200000 },
int main(int argc, char **argv)
{
ros::init(argc, argv, "ParseVito");
ros::NodeHandle node("~");
// VitoSkelethon vito_skelethon;
LineCollisions LineCollisionsLocal;
double spin_rate = 1000;
ros::param::get("~spin_rate",spin_rate);
ROS_DEBUG( "Spin Rate %lf", spin_rate);
ros::Rate rate(spin_rate);
std::string base("world");
double threshold = .15;
std::vector<std::string> list_brach_names;
list_brach_names.push_back("left_arm/");
list_brach_names.push_back("right_arm/");
node.param<std::string>("base", base, "world");
node.param<double>("threshold", threshold, .15);
ROS_INFO_STREAM("Using Base: " << base.c_str());
ROS_INFO_STREAM("Threshold: " << threshold);
tf::TransformListener tf_listener;
std::vector<std::vector<std::string>> links_all_branch;
ros::Publisher list_pub = node.advertise<visualization_msgs::Marker>("skelethon_lines", 10);
ros::Publisher point_pub = node.advertise<visualization_msgs::Marker>("skelethon_point", 10);
ros::Publisher collisions_lines_pub = node.advertise<visualization_msgs::Marker>("skelethon_collision_lines", 10);
ros::Publisher collisions_points_pub = node.advertise<visualization_msgs::Marker>("skelethon_collision_point", 10);
for (unsigned int i = 0; i< list_brach_names.size(); i++)
{
std::vector<std::string> links_in_brach = getSkelethonPoints(node, list_brach_names[i]);
links_all_branch.push_back(links_in_brach);
}
while (node.ok())
{
tf::StampedTransform trans, trans2;
std::vector<std::vector<LineSkelethon>> List_lines_in_all_chains, lines_and_collisions_multiple;
std::vector<LineSkelethon> lines_and_collisions;
for (unsigned int i = 0; i < links_all_branch.size(); ++i)
{
std::vector<LineSkelethon> list_lines_one_chain;
for (unsigned int j = 0; j < links_all_branch[i].size() -1 ; ++j)
{
try
{
tf_listener.waitForTransform(base, links_all_branch[i][j], ros::Time(0), ros::Duration(2.0));
tf_listener.lookupTransform( base , links_all_branch[i][j], ros::Time(0), trans);
tf_listener.waitForTransform(base, links_all_branch[i][j+1], ros::Time(0), ros::Duration(2.0));
tf_listener.lookupTransform( base , links_all_branch[i][j+1], ros::Time(0), trans2);
tf::Vector3 trasnX = trans.getOrigin();
tf::Vector3 trasnX2 = trans2.getOrigin();
PointSkelethon Point1( LineCollisions::Point(trasnX.getX(), trasnX.getY(), trasnX.getZ()), links_all_branch[i][j] );
PointSkelethon Point2( LineCollisions::Point(trasnX2.getX(), trasnX2.getY(), trasnX2.getZ()), links_all_branch[i][j] );
list_lines_one_chain.push_back( LineSkelethon( Point1, Point2) );
}
catch(tf::TransformException& ex)
{
ROS_ERROR("%s", ex.what());
}
}
List_lines_in_all_chains.push_back(list_lines_one_chain);
}
for (unsigned int i = 0; i < List_lines_in_all_chains.size() - 1; ++i)
{
for (unsigned int j = i+1; j < List_lines_in_all_chains.size() ; ++j)
{
for (unsigned int k = 0; k < List_lines_in_all_chains[i].size() ; ++k)
{
for (unsigned int l = 0; l < List_lines_in_all_chains[j].size(); ++l)
{
LineCollisions::Line L1(List_lines_in_all_chains[i][k].P1.P, List_lines_in_all_chains[i][k].P2.P);
LineCollisions::Line L2(List_lines_in_all_chains[j][l].P1.P, List_lines_in_all_chains[j][l].P2.P);
LineCollisions::Line collision_line = LineCollisionsLocal.getClosestPoints( L1, L2 );
if (collision_line.norm <= .15)
{
lines_and_collisions.push_back( LineSkelethon( PointSkelethon( collision_line.P1, List_lines_in_all_chains[i][k].P1.link_name ),
PointSkelethon( collision_line.P2, List_lines_in_all_chains[j][l].P1.link_name ) )) ;
ROS_INFO_STREAM("Collision in Point:" << lines_and_collisions.back().P1.P.transpose() << " of link " << lines_and_collisions.back().P1.link_name);
ROS_INFO_STREAM("Collision in Point:" << lines_and_collisions.back().P2.P.transpose() << " of link " << lines_and_collisions.back().P2.link_name);
}
}
}
}
}
// list_pub.publish(plot_lines(List_lines_in_all_chains, base, 1.0, 0.0, 0.0, 0, visualization_msgs::Marker::LINE_LIST));
point_pub.publish(plot_lines(List_lines_in_all_chains, base, 0.0, 1.0, 0.0, 1, visualization_msgs::Marker::POINTS));
std::vector<std::vector<LineSkelethon>> lis;
lis.push_back(lines_and_collisions);
collisions_lines_pub.publish(plot_lines(lis, base, 0.0, 0.0, 1.0, 2, visualization_msgs::Marker::LINE_LIST));
collisions_points_pub.publish(plot_lines(lis, base, 1.0, 0.0, 1.0, 3, visualization_msgs::Marker::POINTS));
ros::spinOnce();
rate.sleep();
}
return 0;
}
[9] = { { 810000, HFPLL, 1, 0x1E }, 1150000, 1150000, 4 },
[10] = { { 864000, HFPLL, 1, 0x20 }, 1150000, 1150000, 4 },
[11] = { { 918000, HFPLL, 1, 0x22 }, 1150000, 1150000, 5 },
[12] = { { 972000, HFPLL, 1, 0x24 }, 1150000, 1150000, 5 },
[13] = { { 1026000, HFPLL, 1, 0x26 }, 1150000, 1150000, 5 },
[14] = { { 1080000, HFPLL, 1, 0x28 }, 1150000, 1150000, 5 },
[15] = { { 1134000, HFPLL, 1, 0x2A }, 1150000, 1150000, 5 },
[16] = { { 1188000, HFPLL, 1, 0x2C }, 1150000, 1150000, 5 },
[17] = { { 1242000, HFPLL, 1, 0x2E }, 1175000, 1175000, 5 },
[18] = { { 1296000, HFPLL, 1, 0x30 }, 1175000, 1175000, 5 },
[19] = { { 1350000, HFPLL, 1, 0x32 }, 1175000, 1175000, 5 },
{ }
};
static struct acpu_level tbl_slow[] __initdata = {
{ 0, { 162000, HFPLL, 2, 0x0C }, L2(0), 925000 },
{ 1, { 175500, HFPLL, 2, 0x0D }, L2(0), 950000 },
{ 0, { 189000, HFPLL, 2, 0x0E }, L2(1), 950000 },
{ 1, { 216000, HFPLL, 2, 0x10 }, L2(1), 950000 },
{ 0, { 270000, HFPLL, 2, 0x14 }, L2(2), 950000 },
{ 1, { 324000, HFPLL, 2, 0x18 }, L2(2), 950000 },
{ 0, { 378000, HFPLL, 2, 0x1C }, L2(4), 950000 },
{ 1, { 384000, PLL_8, 0, 0x00 }, L2(7), 950000 },
{ 0, { 432000, HFPLL, 2, 0x10 }, L2(7), 975000 },
{ 1, { 486000, HFPLL, 2, 0x12 }, L2(7), 975000 },
{ 0, { 540000, HFPLL, 2, 0x14 }, L2(7), 1000000 },
{ 1, { 594000, HFPLL, 1, 0x16 }, L2(7), 1000000 },
{ 0, { 648000, HFPLL, 1, 0x18 }, L2(7), 1025000 },
{ 1, { 702000, HFPLL, 1, 0x1A }, L2(7), 1025000 },
{ 0, { 756000, HFPLL, 1, 0x1C }, L2(7), 1075000 },
{ 1, { 810000, HFPLL, 1, 0x1E }, L2(7), 1075000 },
MatrixXd der_logarithm_map(Matrix4d T)
{
MatrixXd dlogT_dT = MatrixXd::Zero(6,12);
// Approximate derivative of the logarithm_map wrt the transformation matrix
Matrix3d L1 = Matrix3d::Zero();
Matrix3d L2 = Matrix3d::Zero();
Matrix3d L3 = Matrix3d::Zero();
Matrix3d Vinv = Matrix3d::Identity();
Vector6d x = logmap_se3(T);
// estimates the cosine, sine, and theta
double b;
double cos_ = 0.5 * (T.block(0,0,3,3).trace() - 1.0 );
if(cos_ > 1.f)
cos_ = 1.f;
else if (cos_ < -1.f)
cos_ = -1.f;
double theta = acos(cos_);
double theta2 = theta*theta;
double sin_ = sin(theta);
double cot_ = 1.0 / tan( 0.5*theta );
double csc2_ = pow( 1.0/sin(0.5*theta) ,2);
// if the angle is small...
if( cos_ > 0.9999 )
{
b = 0.5;
L1(1,2) = -b;
L1(2,1) = b;
L2(0,2) = b;
L2(2,0) = -b;
L3(0,1) = -b;
L3(1,0) = b;
// form the full derivative
dlogT_dT.block(3,0,3,3) = L1;
dlogT_dT.block(3,3,3,3) = L2;
dlogT_dT.block(3,6,3,3) = L3;
dlogT_dT.block(0,9,3,3) = Vinv;
}
// if not...
else
{
// rotation part
double k;
Vector3d a;
a(0) = T(2,1) - T(1,2);
a(1) = T(0,2) - T(2,0);
a(2) = T(1,0) - T(0,1);
k = ( theta * cos_ - sin_ ) / ( 4 * pow(sin_,3) );
a = k * a;
L1.block(0,0,3,1) = a;
L2.block(0,1,3,1) = a;
L3.block(0,2,3,1) = a;
// translation part
Matrix3d w_skew = skew( x.tail(3) );
Vinv += w_skew * (1.f-cos_) / theta2 + w_skew * w_skew * (theta - sin_) / pow(theta,3);
Vinv = Vinv.inverse().eval();
// dVinv_dR
Vector3d t;
Matrix3d B, skew_t;
MatrixXd dVinv_dR(3,9);
t = T.block(0,3,3,1);
skew_t = skew( t );
// - form a
a = (theta*cos_-sin_)/(8.0*pow(sin_,3)) * w_skew * t
+ ( (theta*sin_-theta2*cos_)*(0.5*theta*cot_-1.0) - theta*sin_*(0.25*theta*cot_+0.125*theta2*csc2_-1.0))/(4.0*theta2*pow(sin_,4)) * w_skew * w_skew * t;
// - form B
Vector3d w;
Matrix3d dw_dR;
w = x.tail(3);
dw_dR.row(0) << -w(1)*t(1)-w(2)*t(2), 2.0*w(1)*t(0)-w(0)*t(1), 2.0*w(2)*t(0)-w(0)*t(2);
dw_dR.row(1) << -w(1)*t(0)+2.0*w(0)*t(1), -w(0)*t(0)-w(2)*t(2), 2.0*w(2)*t(1)-w(1)*t(2);
dw_dR.row(2) << -w(2)*t(0)+2.0*w(0)*t(2), -w(2)*t(1)+2.0*w(1)*t(2), -w(0)*t(0)-w(1)*t(1);
B = -0.5*theta*skew_t/sin_ - (theta*cot_-2.0)*dw_dR/(8.0*pow(sin_,2));
// - form dVinv_dR
dVinv_dR.col(0) = a;
dVinv_dR.col(1) = -B.col(2);
dVinv_dR.col(2) = B.col(1);
dVinv_dR.col(3) = B.col(2);
dVinv_dR.col(4) = a;
dVinv_dR.col(5) = -B.col(0);
dVinv_dR.col(6) = -B.col(1);
dVinv_dR.col(7) = B.col(0);
dVinv_dR.col(8) = a;
// form the full derivative
dlogT_dT.block(3,0,3,3) = L1;
dlogT_dT.block(3,3,3,3) = L2;
dlogT_dT.block(3,6,3,3) = L3;
dlogT_dT.block(0,9,3,3) = Vinv;
dlogT_dT.block(0,0,3,9) = dVinv_dR;
}
return dlogT_dT;
}
[8] = { { 810000, HFPLL, 1, 0, 0x1E }, 1150000, 1150000, 4 },
[9] = { { 864000, HFPLL, 1, 0, 0x20 }, 1150000, 1150000, 4 },
[10] = { { 918000, HFPLL, 1, 0, 0x22 }, 1150000, 1150000, 6 },
[11] = { { 972000, HFPLL, 1, 0, 0x24 }, 1150000, 1150000, 6 },
[12] = { { 1026000, HFPLL, 1, 0, 0x26 }, 1150000, 1150000, 6 },
[13] = { { 1080000, HFPLL, 1, 0, 0x28 }, 1150000, 1150000, 6 },
[14] = { { 1134000, HFPLL, 1, 0, 0x2A }, 1150000, 1150000, 6 },
[15] = { { 1188000, HFPLL, 1, 0, 0x2C }, 1150000, 1150000, 6 },
[16] = { { 1242000, HFPLL, 1, 0, 0x2E }, 1150000, 1150000, 6 },
[17] = { { 1296000, HFPLL, 1, 0, 0x30 }, 1150000, 1150000, 6 },
[18] = { { 1350000, HFPLL, 1, 0, 0x32 }, 1150000, 1150000, 6 },
};
static struct acpu_level acpu_freq_tbl_slow[] __initdata = {
#ifdef CONFIG_LOW_CPUCLOCKS
{ 1, { 162000, HFPLL, 2, 0, 0x0C }, L2(0), 900000 },
{ 1, { 216000, HFPLL, 2, 0, 0x10 }, L2(0), 900000 },
{ 1, { 270000, HFPLL, 2, 0, 0x12 }, L2(0), 900000 },
{ 1, { 324000, HFPLL, 2, 0, 0x14 }, L2(0), 925000 },
{ 1, { 378000, HFPLL, 2, 0, 0x1B }, L2(0), 925000 },
#endif
{ 1, { 384000, PLL_8, 0, 2, 0x00 }, L2(0), 950000 },
{ 0, { 432000, HFPLL, 2, 0, 0x20 }, L2(6), 975000 },
{ 1, { 486000, HFPLL, 2, 0, 0x24 }, L2(6), 975000 },
{ 0, { 540000, HFPLL, 2, 0, 0x28 }, L2(6), 1000000 },
{ 1, { 594000, HFPLL, 1, 0, 0x16 }, L2(6), 1000000 },
{ 0, { 648000, HFPLL, 1, 0, 0x18 }, L2(6), 1025000 },
{ 1, { 702000, HFPLL, 1, 0, 0x1A }, L2(6), 1025000 },
{ 0, { 756000, HFPLL, 1, 0, 0x1C }, L2(6), 1075000 },
{ 1, { 810000, HFPLL, 1, 0, 0x1E }, L2(6), 1075000 },
{ 0, { 864000, HFPLL, 1, 0, 0x20 }, L2(6), 1100000 },
int main()
{
/** DO NOT MODIFY UNDER THIS LINE
* ***************************************************************************************/
// create a new list
LinkedList L;
// add elements to list
L.addFirst(15);
L.addFirst(6);
L.addFirst(1);
L.addLast(15);
L.addLast(6);
L.addLast(1);
L.add(3, 20);
// Expected output: 1 6 15 20 15 6 1
L.print();
// Expected output: 7
std::cout << L.size() << std::endl;
// Expected output: true
std::cout << L.contains(20) << std::endl;
//Expected output: 3
std::cout << L.indexOf(20) << std::endl;
// mess up the list
L.removeFirst();
L.removeLast();
L.remove(1);
L.removeFirstOccurence(static_cast<myType>(6));
// Expected output: 20 15 6
myType* array = L.toArray();
for (unsigned i = 0; i < L.size(); i++)
std::cout << static_cast<int>(array[i]) << "\t";
std::cout << "\n";
// Expected output: 20
std::cout << L.getFirst() << std::endl;
// Expected output: 6
std::cout << L.getLast() << std::endl;
// Expected output: 15
std::cout << L.get(1) << std::endl;
myType collection[3] = {1, 6, 15};
LinkedList L2(collection, 3);
std::cout << "L2\n";
// Expected output: 1 6 15
L2.print();
std::cout << "L\n";
L.addAll(0, collection, 3);
// Expected output: 1 6 15 20 15 6
L.print();
L.removeLastOccurence(15);
// Expected output: 1 6 15 20 6
L.print();
// Expected output: 1
std::cout << L.set(0, 0) << std::endl;
// Expected output: 0
std::cout << L.getFirst() << std::endl;
// Expected output: 5
std::cout << L.size() << std::endl;
// empty the list
L.clear();
// Expected output: "List is Empty!" <-- or something like that
L.print();
// Hopefully you had a great spring break. This assignment is easy
// to let you recover from any hangover
return 0;
}
/**
Purpose
-------
ZSSSSM applies the LU factorization update from a complex
matrix formed by a lower triangular IB-by-K tile L1 on top of a
M2-by-K tile L2 to a second complex matrix formed by a M1-by-N1
tile A1 on top of a M2-by-N2 tile A2 (N1 == N2).
This is the right-looking Level 2.5 BLAS version of the algorithm.
Arguments
---------
@param[in]
m INTEGER
The number of rows of the matrix A. M >= 0.
@param[in]
n INTEGER
The number of columns of the matrix A. N >= 0.
@param[in]
ib INTEGER
The inner-blocking size. IB >= 0.
@param[in]
NB INTEGER
The blocking size. NB >= 0.
@param[in,out]
hU COMPLEX_16 array, dimension(LDHU, N), on cpu.
On entry, the NB-by-N upper triangular tile hU.
On exit, the content is incomplete. Shouldn't be used.
@param[in]
ldhu INTEGER
The leading dimension of the array hU. LDHU >= max(1,NB).
@param[in,out]
dU COMPLEX_16 array, dimension(LDDU, N), on gpu.
On entry, the NB-by-N upper triangular tile dU identical to hU.
On exit, the new factor U from the factorization.
@param[in]
lddu INTEGER
The leading dimension of the array dU. LDDU >= max(1,NB).
@param[in,out]
hA COMPLEX_16 array, dimension(LDHA, N), on cpu.
On entry, only the M-by-IB first panel needs to be identical to dA(1..M, 1..IB).
On exit, the content is incomplete. Shouldn't be used.
@param[in]
ldha INTEGER
The leading dimension of the array hA. LDHA >= max(1,M).
@param[in,out]
dA COMPLEX_16 array, dimension(LDDA, N), on gpu.
On entry, the M-by-N tile to be factored.
On exit, the factor L from the factorization
@param[in]
ldda INTEGER
The leading dimension of the array dA. LDDA >= max(1,M).
@param[out]
hL COMPLEX_16 array, dimension(LDHL, K), on vpu.
On exit, contains in the upper part the IB-by-K lower triangular tile,
and in the lower part IB-by-K the inverse of the top part.
@param[in]
ldhl INTEGER
The leading dimension of the array hL. LDHL >= max(1,2*IB).
@param[out]
dL COMPLEX_16 array, dimension(LDDL, K), on gpu.
On exit, contains in the upper part the IB-by-K lower triangular tile,
and in the lower part IB-by-K the inverse of the top part.
@param[in]
lddl INTEGER
The leading dimension of the array dL. LDDL >= max(1,2*IB).
@param[out]
hWORK COMPLEX_16 array, dimension(LDHWORK, 2*IB), on cpu.
Workspace.
@param[in]
ldhwork INTEGER
The leading dimension of the array hWORK. LDHWORK >= max(NB, 1).
@param[out]
dWORK COMPLEX_16 array, dimension(LDDWORK, 2*IB), on gpu.
Workspace.
@param[in]
lddwork INTEGER
The leading dimension of the array dWORK. LDDWORK >= max(NB, 1).
@param[out]
ipiv INTEGER array on the cpu.
The pivot indices array of size K as returned by ZTSTRF
@param[out]
info INTEGER
- PLASMA_SUCCESS successful exit
- < 0 if INFO = -k, the k-th argument had an illegal value
- > 0 if INFO = k, U(k,k) is exactly zero. The factorization
has been completed, but the factor U is exactly
singular, and division by zero will occur if it is used
to solve a system of equations.
@ingroup magma_zgesv_tile
********************************************************************/
extern "C" magma_int_t
magma_ztstrf_gpu(
magma_order_t order, magma_int_t m, magma_int_t n,
magma_int_t ib, magma_int_t nb,
magmaDoubleComplex *hU, magma_int_t ldhu,
magmaDoubleComplex_ptr dU, magma_int_t lddu,
magmaDoubleComplex *hA, magma_int_t ldha,
magmaDoubleComplex_ptr dA, magma_int_t ldda,
magmaDoubleComplex *hL, magma_int_t ldhl,
magmaDoubleComplex_ptr dL, magma_int_t lddl,
magma_int_t *ipiv,
magmaDoubleComplex *hwork, magma_int_t ldhwork,
magmaDoubleComplex_ptr dwork, magma_int_t lddwork,
magma_int_t *info)
{
#define UT(i,j) (dUT + (i)*ib*lddu + (j)*ib )
#define AT(i,j) (dAT + (i)*ib*ldda + (j)*ib )
#define L(i) (dL + (i)*ib*lddl )
#define L2(i) (dL2 + (i)*ib*lddl )
#define hU(i,j) (hU + (j)*ib*ldhu + (i)*ib )
#define hA(i,j) (hA + (j)*ib*ldha + (i)*ib )
#define hL(i) (hL + (i)*ib*ldhl )
#define hL2(i) (hL2 + (i)*ib*ldhl )
magmaDoubleComplex c_one = MAGMA_Z_ONE;
magmaDoubleComplex c_neg_one = MAGMA_Z_NEG_ONE;
int iinfo = 0;
int maxm, mindim;
int i, j, im, s, ip, ii, sb, p = 1;
magmaDoubleComplex_ptr dAT, dUT;
magmaDoubleComplex_ptr dAp, dUp;
#ifndef WITHOUTTRTRI
magmaDoubleComplex_ptr dL2 = dL + ib;
magmaDoubleComplex *hL2 = hL + ib;
p = 2;
#endif
/* Check input arguments */
*info = 0;
if (m < 0) {
*info = -1;
}
else if (n < 0) {
*info = -2;
}
else if (ib < 0) {
*info = -3;
}
else if ((lddu < max(1,m)) && (m > 0)) {
*info = -6;
}
else if ((ldda < max(1,m)) && (m > 0)) {
*info = -8;
}
else if ((lddl < max(1,ib)) && (ib > 0)) {
*info = -10;
}
if (*info != 0) {
magma_xerbla( __func__, -(*info) );
return *info;
}
/* quick return */
if ((m == 0) || (n == 0) || (ib == 0))
return *info;
ip = 0;
/* Function Body */
mindim = min(m, n);
s = mindim / ib;
if ( ib >= mindim ) {
/* Use CPU code. */
CORE_ztstrf(m, n, ib, nb,
(PLASMA_Complex64_t*)hU, ldhu,
(PLASMA_Complex64_t*)hA, ldha,
(PLASMA_Complex64_t*)hL, ldhl,
ipiv,
(PLASMA_Complex64_t*)hwork, ldhwork,
info);
#ifndef WITHOUTTRTRI
CORE_zlacpy( PlasmaUpperLower, mindim, mindim,
(PLASMA_Complex64_t*)hL, ldhl,
(PLASMA_Complex64_t*)hL2, ldhl );
CORE_ztrtri( PlasmaLower, PlasmaUnit, mindim,
(PLASMA_Complex64_t*)hL2, ldhl, info );
if (*info != 0 ) {
fprintf(stderr, "ERROR, trtri returned with info = %d\n", *info);
}
#endif
if ( order == MagmaRowMajor ) {
magma_zsetmatrix( m, n, hU, ldhu, dwork, lddwork );
magmablas_ztranspose( m, n, dwork, lddwork, dU, lddu );
magma_zsetmatrix( m, n, hA, ldha, dwork, lddwork );
magmablas_ztranspose( m, n, dwork, lddwork, dA, ldda );
} else {
magma_zsetmatrix( m, n, hU, ldhu, dU, lddu );
magma_zsetmatrix( m, n, hA, ldha, dA, ldda );
}
magma_zsetmatrix( p*ib, n, hL, ldhl, dL, lddl );
}
else {
/* Use hybrid blocked code. */
maxm = magma_roundup( m, 32 );
if ( order == MagmaColMajor ) {
magmablas_zgetmo_in( dU, dUT, lddu, m, n );
magmablas_zgetmo_in( dA, dAT, ldda, m, n );
} else {
dUT = dU; dAT = dA;
}
dAp = dwork;
dUp = dAp + ib*lddwork;
ip = 0;
for( i=0; i < s; i++ ) {
ii = i * ib;
sb = min(mindim-ii, ib);
if ( i > 0 ) {
// download i-th panel
magmablas_ztranspose( sb, ii, UT(0,i), lddu, dUp, lddu );
magmablas_ztranspose( sb, m, AT(0,i), ldda, dAp, ldda );
magma_zgetmatrix( ii, sb, dUp, lddu, hU(0, i), ldhu );
magma_zgetmatrix( m, sb, dAp, ldda, hA(0, i), ldha );
// make sure that gpu queue is empty
//magma_device_sync();
#ifndef WITHOUTTRTRI
magma_ztrmm( MagmaRight, MagmaLower, MagmaTrans, MagmaUnit,
n-(ii+sb), ib,
c_one, L2(i-1), lddl,
UT(i-1, i+1), lddu);
#else
magma_ztrsm( MagmaRight, MagmaLower, MagmaTrans, MagmaUnit,
n-(ii+sb), ib,
c_one, L(i-1), lddl,
UT(i-1, i+1), lddu);
#endif
magma_zgemm( MagmaNoTrans, MagmaNoTrans,
n-(ii+sb), m, ib,
c_neg_one, UT(i-1, i+1), lddu,
AT(0, i-1), ldda,
c_one, AT(0, i+1), ldda );
}
// do the cpu part
CORE_ztstrf(m, sb, ib, nb,
(PLASMA_Complex64_t*)hU(i, i), ldhu,
(PLASMA_Complex64_t*)hA(0, i), ldha,
(PLASMA_Complex64_t*)hL(i), ldhl,
ipiv+ii,
(PLASMA_Complex64_t*)hwork, ldhwork,
info);
if ( (*info == 0) && (iinfo > 0) )
*info = iinfo + ii;
// Need to swap betw U and A
#ifndef NOSWAPBLK
magmablas_zswapblk( MagmaRowMajor, n-(ii+sb),
UT(i, i+1), lddu,
AT(0, i+1), ldda,
1, sb, ipiv+ii, 1, nb );
for (j=0; j < ib; j++) {
im = ipiv[ip]-1;
if ( im == j ) {
ipiv[ip] += ii;
}
ip++;
}
#else
for (j=0; j < ib; j++) {
im = ipiv[ip]-1;
if ( im != (j) ) {
im = im - nb;
assert( (im >= 0) && (im < m) );
magmablas_zswap( n-(ii+sb), UT(i, i+1)+j*lddu, 1, AT(0, i+1)+im*ldda, 1 );
} else {
ipiv[ip] += ii;
}
ip++;
}
#endif
#ifndef WITHOUTTRTRI
CORE_zlacpy( PlasmaUpperLower, sb, sb,
(PLASMA_Complex64_t*)hL(i), ldhl,
(PLASMA_Complex64_t*)hL2(i), ldhl );
CORE_ztrtri( PlasmaLower, PlasmaUnit, sb,
(PLASMA_Complex64_t*)hL2(i), ldhl, info );
if (*info != 0 ) {
fprintf(stderr, "ERROR, trtri returned with info = %d\n", *info);
}
#endif
// upload i-th panel
magma_zsetmatrix( sb, sb, hU(i, i), ldhu, dUp, lddu );
magma_zsetmatrix( m, sb, hA(0, i), ldha, dAp, ldda );
magma_zsetmatrix( p*ib, sb, hL(i), ldhl, L(i), lddl );
magmablas_ztranspose( sb, sb, dUp, lddu, UT(i,i), lddu );
magmablas_ztranspose( m, sb, dAp, ldda, AT(0,i), ldda );
// make sure that gpu queue is empty
//magma_device_sync();
// do the small non-parallel computations
if ( s > (i+1) ) {
#ifndef WITHOUTTRTRI
magma_ztrmm( MagmaRight, MagmaLower, MagmaTrans, MagmaUnit,
sb, sb,
c_one, L2(i), lddl,
UT(i, i+1), lddu);
#else
magma_ztrsm( MagmaRight, MagmaLower, MagmaTrans, MagmaUnit,
sb, sb,
c_one, L(i), lddl,
UT(i, i+1), lddu);
#endif
magma_zgemm( MagmaNoTrans, MagmaNoTrans,
sb, m, sb,
c_neg_one, UT(i, i+1), lddu,
AT(0, i ), ldda,
c_one, AT(0, i+1), ldda );
}
else {
#ifndef WITHOUTTRTRI
magma_ztrmm( MagmaRight, MagmaLower, MagmaTrans, MagmaUnit,
n-mindim, sb,
c_one, L2(i), lddl,
UT(i, i+1), lddu);
#else
magma_ztrsm( MagmaRight, MagmaLower, MagmaTrans, MagmaUnit,
n-mindim, sb,
c_one, L(i), lddl,
UT(i, i+1), lddu);
#endif
magma_zgemm( MagmaNoTrans, MagmaNoTrans,
n-mindim, m, sb,
c_neg_one, UT(i, i+1), lddu,
AT(0, i ), ldda,
c_one, AT(0, i+1), ldda );
}
}
if ( order == MagmaColMajor ) {
magmablas_zgetmo_out( dU, dUT, lddu, m, n );
magmablas_zgetmo_out( dA, dAT, ldda, m, n );
}
}
return *info;
}
double
S2Phi( RECURSE_PARAMS *pR )
{
return( -pR->dSumC * exp( pR->dParams[P_PHI] ) *
L2( pR ));
}
double
dL2_dPhi_dPhi( RECURSE_PARAMS *pR )
{
double dTmp = pR->dSumC * exp( pR->dParams[P_PHI] );
return( dTmp * ( dTmp - 1.0 ) * L2( pR ));
}
QRectF transition::boundingRect() const{
/*----------------------
* Exactement le meme calcule que pour getP1 et getP2
* Mais le const ne permet pas l'utilisation de
* Fonctions internes à la classe
-----------------------*/
QPointF T1(n1->getX()+(n1->getWidth()/2),n1->getY()); //centre du T du noeud1
QPointF R1(n1->getX()+n1->getWidth(),n1->getY()+(n1->getHeight()/2));
QPointF B1(n1->getX()+(n1->getWidth()/2),n1->getY()+n1->getHeight());
QPointF L1(n1->getX(),n1->getY()+(n1->getHeight()/2));
QPointF T2(n2->getX()+(n2->getWidth()/2),n2->getY()); //centre du T du noeud1
QPointF R2(n2->getX()+n2->getWidth(),n2->getY()+(n2->getHeight()/2));
QPointF B2(n2->getX()+(n2->getWidth()/2),n2->getY()+n2->getHeight());
QPointF L2(n2->getX(),n2->getY()+(n2->getHeight()/2));
QPointF middleN1(n1->getX()+n1->getWidth()/2,n1->getY()+n1->getHeight()/2); //centre du noeud1
QPointF middleN2(n2->getX()+n2->getWidth()/2,n2->getY()+n2->getHeight()/2); //centre du noeud2
QPointF p1;
QPointF p2;
//si n1 est en dessou de n2
if( middleN1.y() > middleN2.y() ){
//si n1 est plus a gauche
if( middleN1.x() < n2->getX()){
p1 = L1;
}
//si n1 est plus a droite
else if(middleN1.x() > n2->getX()+n2->getWidth()){
p1 = R1;
}
//si non il est a peut pres au milieu
else{
p1 = B1;
}
}
//si n1 est au dessus de n2
else if( middleN1.y() < middleN2.y() ){
//si n1 est plus a gauche
if( middleN1.x() < n2->getX()){
p1 = L1;
}
//si n1 est plus a droite
else if(middleN1.x() > n2->getX()+n2->getWidth()){
p1 = R1;
}
//si non il est a peut pres au milieu
else{
p1 = T1;
}
}
//si les noeuds sont alignés horizontalement
else if(middleN1.y() == middleN2.y()){
//si n1 est plus a gauche
if( middleN1.x() < n2->getX()){
p1 = L1;
}
//si n1 est plus a droite
else if(middleN1.x() > n2->getX()+n2->getWidth()){
p1 = R1;
}
}
//si les noeud sont alignés verticalement
else if(middleN1.x() == middleN2.x()){
//si n1 est au dessus de n2
if(middleN1.y() < n2->getY()){
p1 = T1;
}
//si n1 est en dessous de n2
else if(middleN1.y() > n2->getY()+n2->getHeight()){
p1 = B1;
}
}
//si n1 est en dessou de n2
if( middleN2.y() > middleN1.y() ){
//si n1 est plus a gauche
if( middleN2.x() < n1->getX()){
p2 = L2;
}
//si n1 est plus a droite
else if(middleN2.x() > n1->getX()+n1->getWidth()){
p2 = R2;
}
//si non il est a peut pres au milieu
else{
p2 = B2;
}
}
//si n1 est au dessus de n2
else if( middleN2.y() < middleN1.y() ){
//si n1 est plus a gauche
if( middleN2.x() < n1->getX()){
p2 = L2;
}
//si n1 est plus a droite
else if(middleN2.x() > n1->getX()+n1->getWidth()){
p2 = R2;
}
//si non il est a peut pres au milieu
else{
p2 = T2;
}
}
//si les noeuds sont alignés horizontalement
else if(middleN2.y() == middleN1.y()){
//si n1 est plus a gauche
if( middleN2.x() < n1->getX()){
p2 = L2;
}
//si n1 est plus a droite
else if(middleN2.x() > n1->getX()+n1->getWidth()){
p2 = R2;
}
}
//si les noeud sont alignés verticalement
else if(middleN2.x() == middleN1.x()){
//si n1 est au dessus de n2
if(middleN2.y() < n1->getY()){
p2 = T2;
}
//si n1 est en dessous de n2
else if(middleN2.y() > n1->getY()+n1->getHeight()){
p2 = B2;
}
}
/*----------------------------
* Pour des noeud circulaires
*--------------------------*/
/* int xCenterNode1 = n1->getX()+n1->getWidth()/2;
int yCenterNode1 = n1->getY()+n1->getHeight()/2;
int xCenterNode2 = n2->getX()+n2->getWidth()/2;
int yCenterNode2 = n2->getY()+n2->getHeight()/2;
//determine les coordonés sur le cercle du 2e cercle
int longueur_vecteur = sqrt(pow(xCenterNode2-xCenterNode1,2)+pow(yCenterNode2-yCenterNode1,2));
int longueur_rayon = n2->getWidth()/2;
int ratio = longueur_vecteur/longueur_rayon;
int x1_2 = xCenterNode1-xCenterNode2; //corrdonée du grand vecteur p1->p2
int y1_2 = yCenterNode1-yCenterNode2;
//divise enssuite les coordonée du vecteur par le ratio
int nx1_2 = x1_2/ratio;
int ny1_2 = y1_2/ratio;
int px2 = xCenterNode2+nx1_2;
int py2 = yCenterNode2+ny1_2;
//determine les coodronés sur le cercle du 1er crecle
int px1 = nx1_2-xCenterNode1;
int py1 = ny1_2-yCenterNode1;
px1 = (-1)*px1;
py1 = (-1)*py1;
QPointF tmp_p1(px1,py1);
QPointF tmp_p2(px2,py2);
*/
QRectF rect(p1,p2);
return rect;
}
[5] = { { 648000, HFPLL, 1, 0x18 }, LVL_NOM, 1050000, 4 },
[6] = { { 702000, HFPLL, 1, 0x1A }, LVL_NOM, 1050000, 4 },
[7] = { { 756000, HFPLL, 1, 0x1C }, LVL_HIGH, 1150000, 4 },
[8] = { { 810000, HFPLL, 1, 0x1E }, LVL_HIGH, 1150000, 4 },
[9] = { { 864000, HFPLL, 1, 0x20 }, LVL_HIGH, 1150000, 4 },
[10] = { { 918000, HFPLL, 1, 0x22 }, LVL_HIGH, 1150000, 7 },
[11] = { { 972000, HFPLL, 1, 0x24 }, LVL_HIGH, 1150000, 7 },
[12] = { { 1026000, HFPLL, 1, 0x26 }, LVL_HIGH, 1150000, 7 },
[13] = { { 1080000, HFPLL, 1, 0x28 }, LVL_HIGH, 1150000, 7 },
[14] = { { 1134000, HFPLL, 1, 0x2A }, LVL_HIGH, 1150000, 7 },
[15] = { { 1188000, HFPLL, 1, 0x2C }, LVL_HIGH, 1150000, 7 },
{ }
};
static struct acpu_level acpu_freq_tbl_slow[] __initdata = {
{ 1, { 384000, PLL_8, 0, 0x00 }, L2(0), 950000 },
{ 1, { 432000, HFPLL, 2, 0x20 }, L2(5), 975000 },
{ 1, { 486000, HFPLL, 2, 0x24 }, L2(5), 975000 },
{ 1, { 540000, HFPLL, 2, 0x28 }, L2(5), 1000000 },
{ 1, { 594000, HFPLL, 1, 0x16 }, L2(5), 1000000 },
{ 1, { 648000, HFPLL, 1, 0x18 }, L2(5), 1025000 },
{ 1, { 702000, HFPLL, 1, 0x1A }, L2(5), 1025000 },
{ 1, { 756000, HFPLL, 1, 0x1C }, L2(10), 1075000 },
{ 1, { 810000, HFPLL, 1, 0x1E }, L2(10), 1075000 },
{ 1, { 864000, HFPLL, 1, 0x20 }, L2(10), 1100000 },
{ 1, { 918000, HFPLL, 1, 0x22 }, L2(10), 1100000 },
{ 1, { 972000, HFPLL, 1, 0x24 }, L2(10), 1125000 },
{ 1, { 1026000, HFPLL, 1, 0x26 }, L2(10), 1125000 },
{ 1, { 1080000, HFPLL, 1, 0x28 }, L2(15), 1175000 },
{ 1, { 1134000, HFPLL, 1, 0x2A }, L2(15), 1175000 },
{ 1, { 1188000, HFPLL, 1, 0x2C }, L2(15), 1200000 },
[6] = { { 702000, HFPLL, 1, 0x1A }, 1150000, 1150000, 4 },
[7] = { { 756000, HFPLL, 1, 0x1C }, 1150000, 1150000, 4 },
[8] = { { 810000, HFPLL, 1, 0x1E }, 1150000, 1150000, 4 },
[9] = { { 864000, HFPLL, 1, 0x20 }, 1150000, 1150000, 4 },
[10] = { { 918000, HFPLL, 1, 0x22 }, 1150000, 1150000, 5 },
[11] = { { 972000, HFPLL, 1, 0x24 }, 1150000, 1150000, 5 },
[12] = { { 1026000, HFPLL, 1, 0x26 }, 1150000, 1150000, 5 },
[13] = { { 1080000, HFPLL, 1, 0x28 }, 1150000, 1150000, 5 },
[14] = { { 1134000, HFPLL, 1, 0x2A }, 1150000, 1150000, 5 },
[15] = { { 1188000, HFPLL, 1, 0x2C }, 1150000, 1150000, 5 },
#endif
{ }
};
static struct acpu_level tbl_slow[] __initdata = {
{ 1, { 384000, PLL_8, 0, 0x00 }, L2(0), 950000 },
{ 0, { 432000, HFPLL, 2, 0x20 }, L2(5), 975000 },
{ 1, { 486000, HFPLL, 2, 0x24 }, L2(5), 975000 },
{ 0, { 540000, HFPLL, 2, 0x28 }, L2(5), 1000000 },
{ 1, { 594000, HFPLL, 1, 0x16 }, L2(5), 1000000 },
{ 0, { 648000, HFPLL, 1, 0x18 }, L2(5), 1025000 },
{ 1, { 702000, HFPLL, 1, 0x1A }, L2(5), 1025000 },
{ 0, { 756000, HFPLL, 1, 0x1C }, L2(5), 1075000 },
{ 1, { 810000, HFPLL, 1, 0x1E }, L2(5), 1075000 },
{ 0, { 864000, HFPLL, 1, 0x20 }, L2(5), 1100000 },
{ 1, { 918000, HFPLL, 1, 0x22 }, L2(5), 1100000 },
{ 0, { 972000, HFPLL, 1, 0x24 }, L2(5), 1125000 },
{ 1, { 1026000, HFPLL, 1, 0x26 }, L2(5), 1125000 },
{ 0, { 1080000, HFPLL, 1, 0x28 }, L2(15), 1175000 },
{ 1, { 1134000, HFPLL, 1, 0x2A }, L2(15), 1175000 },
{ 0, { 1188000, HFPLL, 1, 0x2C }, L2(15), 1200000 },
[1] = { { 486000, HFPLL, 2, 0x24 }, 1050000, 1050000, 2 },
[2] = { { 594000, HFPLL, 1, 0x16 }, 1050000, 1050000, 2 },
[3] = { { 702000, HFPLL, 1, 0x1A }, 1050000, 1050000, 4 },
[4] = { { 810000, HFPLL, 1, 0x1E }, 1050000, 1050000, 4 },
[5] = { { 918000, HFPLL, 1, 0x22 }, 1150000, 1150000, 5 },
[6] = { { 1026000, HFPLL, 1, 0x26 }, 1150000, 1150000, 5 },
[7] = { { 1134000, HFPLL, 1, 0x2A }, 1150000, 1150000, 5 },
[8] = { { 1242000, HFPLL, 1, 0x2E }, 1150000, 1150000, 5 },
[9] = { { 1350000, HFPLL, 1, 0x32 }, 1150000, 1150000, 5 },
{ }
};
#define AVS(x) .avsdscr_setting = (x)
static struct acpu_level freq_tbl_PVS0[] __initdata = {
{ 1, { 384000, PLL_8, 0, 0x00 }, L2(0), 950000, AVS(0x70001F) },
{ 1, { 486000, HFPLL, 2, 0x24 }, L2(4), 950000, AVS(0x0) },
{ 1, { 594000, HFPLL, 1, 0x16 }, L2(4), 975000, AVS(0x0) },
{ 1, { 702000, HFPLL, 1, 0x1A }, L2(4), 1000000, AVS(0x0) },
{ 1, { 810000, HFPLL, 1, 0x1E }, L2(4), 1025000, AVS(0x0) },
{ 1, { 918000, HFPLL, 1, 0x22 }, L2(4), 1050000, AVS(0x0) },
{ 1, { 1026000, HFPLL, 1, 0x26 }, L2(4), 1075000, AVS(0x0) },
{ 1, { 1134000, HFPLL, 1, 0x2A }, L2(9), 1100000, AVS(0x70000D) },
{ 1, { 1242000, HFPLL, 1, 0x2E }, L2(9), 1125000, AVS(0x0) },
{ 1, { 1350000, HFPLL, 1, 0x32 }, L2(9), 1150000, AVS(0x0) },
{ 1, { 1458000, HFPLL, 1, 0x36 }, L2(9), 1175000, AVS(0x0) },
{ 1, { 1566000, HFPLL, 1, 0x3A }, L2(9), 1200000, AVS(0x0) },
{ 1, { 1674000, HFPLL, 1, 0x3E }, L2(9), 1225000, AVS(0x0) },
{ 1, { 1728000, HFPLL, 1, 0x40 }, L2(9), 1250000, AVS(0x70000B) },
{ 0, { 0 } }
};
[6] = { { 729600, HFPLL, 1, 38 }, LVL_NOM, 950000, 3 },
[7] = { { 806400, HFPLL, 1, 42 }, LVL_HIGH, 1050000, 4 },
[8] = { { 883200, HFPLL, 1, 46 }, LVL_HIGH, 1050000, 4 },
[9] = { { 960000, HFPLL, 1, 50 }, LVL_HIGH, 1050000, 4 },
[10] = { { 1036800, HFPLL, 1, 54 }, LVL_HIGH, 1050000, 5 },
[11] = { { 1113600, HFPLL, 1, 58 }, LVL_HIGH, 1050000, 5 },
[12] = { { 1190400, HFPLL, 1, 62 }, LVL_HIGH, 1050000, 6 },
[13] = { { 1267200, HFPLL, 1, 66 }, LVL_HIGH, 1050000, 6 },
[14] = { { 1344000, HFPLL, 1, 70 }, LVL_HIGH, 1050000, 7 },
[15] = { { 1420800, HFPLL, 1, 74 }, LVL_HIGH, 1050000, 7 },
[16] = { { 1497600, HFPLL, 1, 78 }, LVL_HIGH, 1050000, 7 },
{ }
};
static struct acpu_level acpu_freq_tbl_v1_pvs0[] __initdata = {
{ 1, { 300000, PLL_0, 0, 0 }, L2(0), 825000, 73 },
{ 0, { 345600, HFPLL, 2, 36 }, L2(3), 825000, 85 },
{ 1, { 422400, HFPLL, 2, 44 }, L2(3), 825000, 104 },
{ 0, { 499200, HFPLL, 2, 52 }, L2(6), 825000, 124 },
{ 1, { 576000, HFPLL, 1, 30 }, L2(6), 825000, 144 },
{ 1, { 652800, HFPLL, 1, 34 }, L2(7), 825000, 165 },
{ 1, { 729600, HFPLL, 1, 38 }, L2(7), 825000, 186 },
{ 0, { 806400, HFPLL, 1, 42 }, L2(10), 835000, 208 },
{ 1, { 883200, HFPLL, 1, 46 }, L2(10), 845000, 229 },
{ 0, { 960000, HFPLL, 1, 50 }, L2(10), 860000, 252 },
{ 1, { 1036800, HFPLL, 1, 54 }, L2(10), 880000, 275 },
{ 0, { 1113600, HFPLL, 1, 58 }, L2(12), 905000, 298 },
{ 0, { 1190400, HFPLL, 1, 62 }, L2(12), 920000, 321 },
{ 0, { 1267200, HFPLL, 1, 66 }, L2(12), 940000, 346 },
{ 1, { 1344000, HFPLL, 1, 70 }, L2(12), 960000, 371 },
{ 0, { 1420800, HFPLL, 1, 74 }, L2(16), 980000, 397 },
