void SymbolicQr::init() {
// Call the base class initializer
LinearSolverInternal::init();
// Read options
bool codegen = getOption("codegen");
string compiler = getOption("compiler");
// Make sure that command processor is available
if (codegen) {
#ifdef WITH_DL
int flag = system(static_cast<const char*>(0));
casadi_assert_message(flag!=0, "No command procesor available");
#else // WITH_DL
casadi_error("Codegen requires CasADi to be compiled with option \"WITH_DL\" enabled");
#endif // WITH_DL
}
// Symbolic expression for A
SX A = SX::sym("A", input(0).sparsity());
// Get the inverted column permutation
std::vector<int> inv_colperm(colperm_.size());
for (int k=0; k<colperm_.size(); ++k)
inv_colperm[colperm_[k]] = k;
// Get the inverted row permutation
std::vector<int> inv_rowperm(rowperm_.size());
for (int k=0; k<rowperm_.size(); ++k)
inv_rowperm[rowperm_[k]] = k;
// Permute the linear system
SX Aperm = A(rowperm_, colperm_);
// Generate the QR factorization function
vector<SX> QR(2);
qr(Aperm, QR[0], QR[1]);
SXFunction fact_fcn(A, QR);
// Optionally generate c code and load as DLL
if (codegen) {
stringstream ss;
ss << "symbolic_qr_fact_fcn_" << this;
fact_fcn_ = dynamicCompilation(fact_fcn, ss.str(),
"Symbolic QR factorization function", compiler);
} else {
fact_fcn_ = fact_fcn;
}
// Initialize factorization function
fact_fcn_.setOption("name", "QR_fact");
fact_fcn_.init();
// Symbolic expressions for solve function
SX Q = SX::sym("Q", QR[0].sparsity());
SX R = SX::sym("R", QR[1].sparsity());
SX b = SX::sym("b", input(1).size1(), 1);
// Solve non-transposed
// We have Pb' * Q * R * Px * x = b <=> x = Px' * inv(R) * Q' * Pb * b
// Permute the right hand sides
SX bperm = b(rowperm_, ALL);
// Solve the factorized system
SX xperm = casadi::solve(R, mul(Q.T(), bperm));
// Permute back the solution
SX x = xperm(inv_colperm, ALL);
// Generate the QR solve function
vector<SX> solv_in(3);
solv_in[0] = Q;
solv_in[1] = R;
solv_in[2] = b;
SXFunction solv_fcn(solv_in, x);
// Optionally generate c code and load as DLL
if (codegen) {
stringstream ss;
ss << "symbolic_qr_solv_fcn_N_" << this;
solv_fcn_N_ = dynamicCompilation(solv_fcn, ss.str(), "QR_solv_N", compiler);
} else {
solv_fcn_N_ = solv_fcn;
}
// Initialize solve function
solv_fcn_N_.setOption("name", "QR_solv");
solv_fcn_N_.init();
// Solve transposed
// We have (Pb' * Q * R * Px)' * x = b
// <=> Px' * R' * Q' * Pb * x = b
// <=> x = Pb' * Q * inv(R') * Px * b
// Permute the right hand side
bperm = b(colperm_, ALL);
// Solve the factorized system
xperm = mul(Q, casadi::solve(R.T(), bperm));
// Permute back the solution
x = xperm(inv_rowperm, ALL);
// Mofify the QR solve function
solv_fcn = SXFunction(solv_in, x);
// Optionally generate c code and load as DLL
if (codegen) {
stringstream ss;
ss << "symbolic_qr_solv_fcn_T_" << this;
solv_fcn_T_ = dynamicCompilation(solv_fcn, ss.str(), "QR_solv_T", compiler);
} else {
solv_fcn_T_ = solv_fcn;
}
// Initialize solve function
solv_fcn_T_.setOption("name", "QR_solv_T");
solv_fcn_T_.init();
// Allocate storage for QR factorization
Q_ = DMatrix::zeros(Q.sparsity());
R_ = DMatrix::zeros(R.sparsity());
}
void check_QR(
orthotope<T> const& A
, orthotope<T> const& Q
, orthotope<T> const& R
)
{ // {{{
BOOST_ASSERT(2 == A.order());
BOOST_ASSERT(2 == Q.order());
BOOST_ASSERT(2 == R.order());
BOOST_ASSERT(A.hypercube());
BOOST_ASSERT(Q.hypercube());
BOOST_ASSERT(R.hypercube());
std::size_t const n = A.extent(0);
BOOST_ASSERT(n == Q.extent(0));
BOOST_ASSERT(n == R.extent(0));
///////////////////////////////////////////////////////////////////////////
/// Make sure Q * R equals A.
orthotope<T> QR = matrix_multiply(Q, R);
for (std::size_t l = 0; l < n; ++l)
{
for (std::size_t i = 0; i < n; ++i)
{
if (!compare_floating(A(l, i), QR(l, i), 1e-6))
std::cout << "WARNING: QR[" << l << "][" << i << "] (value "
<< QR(l, i) << ") is not equal to A[" << l << "]["
<< i << "] (value " << A(l, i) << ")\n";
}
}
///////////////////////////////////////////////////////////////////////////
/// Make sure R is an upper triangular matrix.
for (std::size_t l = 0; l < (n - 1); ++l)
{
for (std::size_t i = l + 1; i < n; ++i)
{
if (!compare_floating(0.0, R(i, l), 1e-6))
std::cout << "WARNING: R[" << i << "][" << l << "] is not 0 "
"(value is " << R(i, l) << "), R is not an upper "
"triangular matrix\n";
}
}
///////////////////////////////////////////////////////////////////////////
/// Make sure Q is orthogonal. A matrix is orthogonal if its transpose is
/// equal to its inverse:
///
/// Q^T = Q^-1
///
/// This implies that:
///
/// Q^T * Q = Q * Q^T = I
///
/// We use the above formula to verify Q's orthogonality.
orthotope<T> QT = Q.copy();
// Transpose QT.
for (std::size_t l = 0; l < (n - 1); ++l)
for (std::size_t i = l + 1; i < n; ++i)
std::swap(QT(l, i), QT(i, l));
// Compute Q^T * Q and store the result in QT.
QT = matrix_multiply(Q, QT);
for (std::size_t l = 0; l < n; ++l)
{
for (std::size_t i = 0; i < n; ++i)
{
// Diagonals should be 1.
if (l == i)
{
if (!compare_floating(1.0, QT(l, i), 1e-6))
std::cout << "WARNING: (Q^T * Q)[" << l << "][" << i << "] "
"is not 1 (value is " << QT(l, i) << "), Q is "
"not an orthogonal matrix\n";
}
// All other entries should be 0.
else
{
if (!compare_floating(0.0, QT(l, i), 1e-6))
std::cout << "WARNING: (Q^T * Q)[" << l << "][" << i << "] "
"is not 0 (value is " << QT(l, i) << "), Q is "
"not an orthogonal matrix\n";
}
}
}
} // }}}
// LLSQ Approx. Trilateration
void locateLLSQ(int numantennas, double* antennas, double* distances, double* x)
{
// function saves triangulated position in vector x
// input *antennas holds
// x1, y1, z1,
// x2, y2, z2,
// x3, y3, z3,
// x4, y4, z4;
// numantennas holds integer number of antennas (min 5)
// double *distances holds pointer to array holding distances from each antenna
// Theory used:
// http://inside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf
// define some locals
int m = (numantennas - 1); // Number of rows in A
int n = 3; // Number of columns in A (always three)
if (m >= 4)n = 4; // Number of columns in A: three coordinates plus K
double A[m * n];
double b[m * 1];
double r;
double Atranspose[n * m];
double AtAinA[n * m];
double AtA[n * n];
//Calculating b vector
for (int i = 0; i < m; i++)
{
r = dist(antennas[0], antennas[1], antennas[2], antennas[(i + 1) * 3], antennas[(i + 1) * 3 + 1], antennas[(i + 1) * 3 + 2]);
b[i] = 0.5*(distances[0] * distances[0] - distances[i + 1] * distances[i + 1] + r * r); // If given exact distances
if (n == 4)b[i] = 0.5 * r*r; // For the case when we calculate K, overwrite b
}
#ifdef VERBOSE
matrixPrint(b, m, 1, "b");
#endif // DEBUG
//Calculating A matrix
for (int i = 0; i < m; i++)
{
A[i * n] = antennas[(i + 1) * 3] - antennas[0]; //xi-x1
A[i * n + 1] = antennas[(i + 1) * 3 + 1] - antennas[1]; //yi-y1
A[i * n + 2] = antennas[(i + 1) * 3 + 2] - antennas[2]; //zi-z1
if (n == 4) A[i * n + 3] = -0.5 * (distances[0] * distances[0] - distances[i + 1] * distances[i + 1]); // for K
}
#ifdef VERBOSE
matrixPrint(A, m, n, "A");
#endif // DEBUG
matrixTranspose(A, m, n, Atranspose);
matrixMult(Atranspose, A, n, m, n, AtA);
if (matrixInvert(AtA, n) == 1) //well behaved
{
matrixMult(AtA, Atranspose, n, n, m, AtAinA);
matrixMult(AtAinA, b, n, m, 1, x);
}
else // Ill-behaved A
{
double Q[m * m], Qtranspose[m * m], Qtransposeb[m * 1];
double R[m * m];
QR(A, m, n, Q, R);
matrixTranspose(Q, m, m, Qtranspose);
matrixMult(Qtranspose, b, m, m, 1, Qtransposeb);
matrixInvert(R, m);
matrixMult(R, Qtransposeb, m, m, 1, x);
}
// Adding back the reference point
x[0] = x[0] + antennas[0];
x[1] = x[1] + antennas[1];
x[2] = x[2] + antennas[2];
if (n == 4)x[3] = 1 / sqrt(x[3]); // Calculate K
}
void CreateRoutine(Func f)
{
routine_func* r = new routine_func(f);
QR(*r);
}
static void salsa20_core(uint32_t *x)
{
uint32_t x0 = x[0];
uint32_t x1 = x[1];
uint32_t x2 = x[2];
uint32_t x3 = x[3];
uint32_t x4 = x[4];
uint32_t x5 = x[5];
uint32_t x6 = x[6];
uint32_t x7 = x[7];
uint32_t x8 = x[8];
uint32_t x9 = x[9];
uint32_t x10 = x[10];
uint32_t x11 = x[11];
uint32_t x12 = x[12];
uint32_t x13 = x[13];
uint32_t x14 = x[14];
uint32_t x15 = x[15];
/* columnround 1 */
QR(x0, x4, x8, x12);
QR(x5, x9, x13, x1);
QR(x10, x14, x2, x6);
QR(x15, x3, x7, x11);
/* rowround 2 */
QR(x0, x1, x2, x3);
QR(x5, x6, x7, x4);
QR(x10, x11, x8, x9);
QR(x15, x12, x13, x14);
/* columnround 3 */
QR(x0, x4, x8, x12);
QR(x5, x9, x13, x1);
QR(x10, x14, x2, x6);
QR(x15, x3, x7, x11);
/* rowround 4 */
QR(x0, x1, x2, x3);
QR(x5, x6, x7, x4);
QR(x10, x11, x8, x9);
QR(x15, x12, x13, x14);
/* columnround 5 */
QR(x0, x4, x8, x12);
QR(x5, x9, x13, x1);
QR(x10, x14, x2, x6);
QR(x15, x3, x7, x11);
/* rowround 6 */
QR(x0, x1, x2, x3);
QR(x5, x6, x7, x4);
QR(x10, x11, x8, x9);
QR(x15, x12, x13, x14);
/* columnround 7 */
QR(x0, x4, x8, x12);
QR(x5, x9, x13, x1);
QR(x10, x14, x2, x6);
QR(x15, x3, x7, x11);
/* rowround 8 */
QR(x0, x1, x2, x3);
QR(x5, x6, x7, x4);
QR(x10, x11, x8, x9);
QR(x15, x12, x13, x14);
x[0] += x0;
x[1] += x1;
x[2] += x2;
x[3] += x3;
x[4] += x4;
x[5] += x5;
x[6] += x6;
x[7] += x7;
x[8] += x8;
x[9] += x9;
x[10] += x10;
x[11] += x11;
x[12] += x12;
x[13] += x13;
x[14] += x14;
x[15] += x15;
}
inline void
HouseholderSolve
( Orientation orientation,
Matrix<R>& A, const Matrix<R>& B,
Matrix<R>& X )
{
#ifndef RELEASE
PushCallStack("HouseholderSolve");
#endif
// TODO: Add scaling
const int m = A.Height();
const int n = A.Width();
if( orientation == NORMAL )
{
if( m != B.Height() )
throw std::logic_error("A and B do not conform");
if( m >= n )
{
// Overwrite A with its packed QR factorization
QR( A );
// Copy B into X
X = B;
// Apply Q' to X
ApplyPackedReflectors( LEFT, LOWER, VERTICAL, FORWARD, 0, A, X );
// Shrink X to its new height
X.ResizeTo( n, X.Width() );
// Solve against R (checking for singularities)
Matrix<R> AT;
LockedView( AT, A, 0, 0, n, n );
Trsm( LEFT, UPPER, NORMAL, NON_UNIT, R(1), AT, X, true );
}
else
{
// Overwrite A with its packed LQ factorization
LQ( A );
// Copy B into X
X.ResizeTo( n, B.Width() );
Matrix<R> XT,
XB;
PartitionDown( X, XT,
XB, m );
XT = B;
Zero( XB );
// Solve against L (checking for singularities)
Matrix<R> AL;
LockedView( AL, A, 0, 0, m, m );
Trsm( LEFT, LOWER, NORMAL, NON_UNIT, R(1), AL, XT, true );
// Apply Q' to X
ApplyPackedReflectors( LEFT, UPPER, HORIZONTAL, BACKWARD, 0, A, X );
}
}
else // orientation == ADJOINT
{
if( n != B.Height() )
throw std::logic_error("A and B do not conform");
if( m >= n )
{
// Overwrite A with its packed QR factorization
QR( A );
// Copy B into X
X.ResizeTo( m, B.Width() );
Matrix<R> XT,
XB;
PartitionDown( X, XT,
XB, n );
XT = B;
Zero( XB );
// Solve against R' (checking for singularities)
Matrix<R> AT;
LockedView( AT, A, 0, 0, n, n );
Trsm( LEFT, UPPER, ADJOINT, NON_UNIT, R(1), AT, XT, true );
// Apply Q to X
ApplyPackedReflectors( LEFT, LOWER, VERTICAL, BACKWARD, 0, A, X );
}
else
{
// Overwrite A with its packed LQ factorization
LQ( A );
// Copy B into X
X = B;
// Apply Q to X
ApplyPackedReflectors( LEFT, UPPER, HORIZONTAL, FORWARD, 0, A, X );
// Shrink X to its new size
X.ResizeTo( m, X.Width() );
// Solve against L' (check for singularities)
Matrix<R> AL;
LockedView( AL, A, 0, 0, m, m );
Trsm( LEFT, LOWER, ADJOINT, NON_UNIT, R(1), AL, X, true );
}
}
#ifndef RELEASE
PopCallStack();
#endif
}
inline void
HouseholderSolve
( Orientation orientation,
DistMatrix<Complex<R> >& A,
const DistMatrix<Complex<R> >& B,
DistMatrix<Complex<R> >& X )
{
#ifndef RELEASE
PushCallStack("HouseholderSolve");
if( A.Grid() != B.Grid() || A.Grid() != X.Grid() )
throw std::logic_error("Grids do not match");
if( orientation == TRANSPOSE )
throw std::logic_error("Invalid orientation");
#endif
typedef Complex<R> C;
const Grid& g = A.Grid();
// TODO: Add scaling
const int m = A.Height();
const int n = A.Width();
DistMatrix<C,MD,STAR> t( g );
if( orientation == NORMAL )
{
if( m != B.Height() )
throw std::logic_error("A and B do not conform");
if( m >= n )
{
// Overwrite A with its packed QR factorization (and store the
// corresponding Householder scalars in t)
QR( A, t );
// Copy B into X
X = B;
// Apply Q' to X
ApplyPackedReflectors
( LEFT, LOWER, VERTICAL, FORWARD, CONJUGATED, 0, A, t, X );
// Shrink X to its new height
X.ResizeTo( n, X.Width() );
// Solve against R (checking for singularities)
DistMatrix<C> AT( g );
LockedView( AT, A, 0, 0, n, n );
Trsm( LEFT, UPPER, NORMAL, NON_UNIT, C(1), AT, X, true );
}
else
{
// Overwrite A with its packed LQ factorization (and store the
// corresponding Householder scalars in it)
LQ( A, t );
// Copy B into X
X.ResizeTo( n, B.Width() );
DistMatrix<C> XT( g ),
XB( g );
PartitionDown( X, XT,
XB, m );
XT = B;
Zero( XB );
// Solve against L (checking for singularities)
DistMatrix<C> AL( g );
LockedView( AL, A, 0, 0, m, m );
Trsm( LEFT, LOWER, NORMAL, NON_UNIT, C(1), AL, XT, true );
// Apply Q' to X
ApplyPackedReflectors
( LEFT, UPPER, HORIZONTAL, BACKWARD, CONJUGATED, 0, A, t, X );
}
}
else // orientation == ADJOINT
{
if( n != B.Height() )
throw std::logic_error("A and B do not conform");
if( m >= n )
{
// Overwrite A with its packed QR factorization (and store the
// corresponding Householder scalars in t)
QR( A, t );
// Copy B into X
X.ResizeTo( m, B.Width() );
DistMatrix<C> XT( g ),
XB( g );
PartitionDown( X, XT,
XB, n );
XT = B;
Zero( XB );
// Solve against R' (checking for singularities)
DistMatrix<C> AT( g );
LockedView( AT, A, 0, 0, n, n );
Trsm( LEFT, UPPER, ADJOINT, NON_UNIT, C(1), AT, XT, true );
// Apply Q to X
ApplyPackedReflectors
( LEFT, LOWER, VERTICAL, BACKWARD, UNCONJUGATED, 0, A, t, X );
}
else
{
// Overwrite A with its packed LQ factorization (and store the
// corresponding Householder scalars in t)
LQ( A, t );
// Copy B into X
X = B;
// Apply Q to X
ApplyPackedReflectors
( LEFT, UPPER, HORIZONTAL, FORWARD, UNCONJUGATED, 0, A, t, X );
// Shrink X to its new height
X.ResizeTo( m, X.Width() );
// Solve against L' (check for singularities)
DistMatrix<C> AL( g );
LockedView( AL, A, 0, 0, m, m );
Trsm( LEFT, LOWER, ADJOINT, NON_UNIT, C(1), AL, X, true );
}
}
#ifndef RELEASE
PopCallStack();
#endif
}
Vector SchemeRoe(const Cell& Cell1,const Cell& Cell2,const Cell& Cell3,const Cell& Cell4, int AxisNo)
{
// Local variables
Vector V1, V2, V3, V4; // Velocities
real rho1, rho2, rho3, rho4; // Densities
real p1, p2, p3, p4; // Pressures
real rhoE1, rhoE2, rhoE3, rhoE4; // Energies
Vector Result(QuantityNb);
Vector F1(QuantityNb); // Fluxes
Vector F2(QuantityNb);
Vector F3(QuantityNb);
Vector F4(QuantityNb);
Vector Q1, Q2, Q3, Q4; // Conservative quantities
Vector FL(QuantityNb),FR(QuantityNb); // Left and right fluxex
Vector QL(QuantityNb),QR(QuantityNb); // Left and right conservative quantities
real rhoL, rhoR; // Left and right densities
real pL, pR; // Left and right pressures
real rhoEL, rhoER; // Left and right energies
Vector VL(Dimension), VR(Dimension); // Left and right velocities
Vector One(QuantityNb);
real rho; // central density with Roe's average
Vector V(Dimension); // central velocity with Roe's average
real H; // central enthalpy with Roe's average
real c; // central speed of sound with Roe's average
real Roe; // Coefficient for Roe's average
Matrix L, R; // left and right eigenmatrix
Matrix Lambda(QuantityNb); // diagonal matrix containing the eigenvalues
Matrix A; // absolute value of the jacobian matrix
Vector Lim; // limiter (Van Leer)
int i; // coutner
// vector one.
for(i=1; i<=QuantityNb; i++ )
One.setValue(i,1.);
// --- Get conservative quantities ---
Q1 = Cell1.average();
Q2 = Cell2.average();
Q3 = Cell3.average();
Q4 = Cell4.average();
// --- Get primitive variables ---
// density
rho1 = Cell1.density();
rho2 = Cell2.density();
rho3 = Cell3.density();
rho4 = Cell4.density();
// velocity
V1 = Cell1.velocity();
V2 = Cell2.velocity();
V3 = Cell3.velocity();
V4 = Cell4.velocity();
// energy
rhoE1 = Cell1.energy();
rhoE2 = Cell2.energy();
rhoE3 = Cell3.energy();
rhoE4 = Cell4.energy();
// pressure
p1 = Cell1.pressure();
p2 = Cell2.pressure();
p3 = Cell3.pressure();
p4 = Cell4.pressure();
// --- Compute Euler fluxes ---
F1.setValue(1,rho1*V1.value(AxisNo));
F2.setValue(1,rho2*V2.value(AxisNo));
F3.setValue(1,rho3*V3.value(AxisNo));
F4.setValue(1,rho4*V4.value(AxisNo));
for(i=1; i<=Dimension; i++)
{
F1.setValue(i+1, rho1*V1.value(AxisNo)*V1.value(i) + ((AxisNo == i)? p1 : 0.));
F2.setValue(i+1, rho2*V2.value(AxisNo)*V2.value(i) + ((AxisNo == i)? p2 : 0.));
F3.setValue(i+1, rho3*V3.value(AxisNo)*V3.value(i) + ((AxisNo == i)? p3 : 0.));
F4.setValue(i+1, rho4*V4.value(AxisNo)*V4.value(i) + ((AxisNo == i)? p4 : 0.));
}
F1.setValue(QuantityNb,(rhoE1+p1)*V1.value(AxisNo));
F2.setValue(QuantityNb,(rhoE2+p2)*V2.value(AxisNo));
F3.setValue(QuantityNb,(rhoE3+p3)*V3.value(AxisNo));
F4.setValue(QuantityNb,(rhoE4+p4)*V4.value(AxisNo));
// --- Van Leer limiter ---
// Left
Lim = Limiter(Q3-Q2, Q2-Q1);
FL = F2 + 0.5*(Lim|(F2-F1)) + 0.5*((One-Lim)|(F3-F2));
QL = Q2 + 0.5*(Lim|(Q2-Q1)) + 0.5*((One-Lim)|(Q3-Q2));
// Right
Lim = Limiter(Q3-Q2, Q4-Q3);
FR = F3 - 0.5*(Lim|(F4-F3)) - 0.5*((One-Lim)|(F3-F2));
QR = Q3 - 0.5*(Lim|(Q4-Q3)) - 0.5*((One-Lim)|(Q3-Q2));
/*
FL = F2;
FR = F3;
QL = Q2;
QR = Q3;
*/
// --- Extract left and right primitive variables ---
rhoL = QL.value(1);
rhoR = QR.value(1);
for (i=1; i<= Dimension; i++)
{
VL.setValue(i,QL.value(i+1)/rhoL);
VR.setValue(i,QR.value(i+1)/rhoR);
}
rhoEL=QL.value(QuantityNb);
rhoER=QR.value(QuantityNb);
pL = (Gamma-1)*(rhoEL - .5*rhoL*(VL*VL));
pR = (Gamma-1)*(rhoER - .5*rhoR*(VR*VR));
// --- Compute Roe's averages ---
Roe = sqrt(rhoR/rhoL);
rho = Roe*rhoL;
V = 1./(1.+Roe)*( Roe*VR + VL );
H = 1./(1.+Roe)*( Roe*(rhoER+pR)/rhoR + (rhoEL+pL)/rhoL );
c = sqrt ( (Gamma-1)*( H - 0.5*(V*V) ) );
// --- Compute diagonal matrix containing the absolute value of the eigenvalues ---
for (i=1;i<=Dimension;i++)
Lambda.setValue(i,i, fabs(V.value(AxisNo)));
Lambda.setValue(Dimension+1, Dimension+1, fabs(V.value(AxisNo)+c));
Lambda.setValue(Dimension+2, Dimension+2, fabs(V.value(AxisNo)-c));
// --- Set left and right eigenmatrices ---
L.setEigenMatrix(true, AxisNo, V, c);
R.setEigenMatrix(false, AxisNo, V, c, H);
// --- Compute absolute Jacobian matrix ---
A = R*Lambda*L;
// --- Compute Euler Flux ---
Result = 0.5*(FL+FR) - 0.5*(A*(QR-QL));
return Result;
}
static void chacha_core(int rounds, block *out, const cryptonite_chacha_state *in)
{
uint32_t x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, x10, x11, x12, x13, x14, x15;
int i;
x0 = in->d[0];
x1 = in->d[1];
x2 = in->d[2];
x3 = in->d[3];
x4 = in->d[4];
x5 = in->d[5];
x6 = in->d[6];
x7 = in->d[7];
x8 = in->d[8];
x9 = in->d[9];
x10 = in->d[10];
x11 = in->d[11];
x12 = in->d[12];
x13 = in->d[13];
x14 = in->d[14];
x15 = in->d[15];
for (i = rounds; i > 0; i -= 2) {
QR(x0, x4, x8, x12);
QR(x1, x5, x9, x13);
QR(x2, x6, x10, x14);
QR(x3, x7, x11, x15);
QR(x0, x5, x10, x15);
QR(x1, x6, x11, x12);
QR(x2, x7, x8, x13);
QR(x3, x4, x9, x14);
}
x0 += in->d[0];
x1 += in->d[1];
x2 += in->d[2];
x3 += in->d[3];
x4 += in->d[4];
x5 += in->d[5];
x6 += in->d[6];
x7 += in->d[7];
x8 += in->d[8];
x9 += in->d[9];
x10 += in->d[10];
x11 += in->d[11];
x12 += in->d[12];
x13 += in->d[13];
x14 += in->d[14];
x15 += in->d[15];
out->d[0] = cpu_to_le32(x0);
out->d[1] = cpu_to_le32(x1);
out->d[2] = cpu_to_le32(x2);
out->d[3] = cpu_to_le32(x3);
out->d[4] = cpu_to_le32(x4);
out->d[5] = cpu_to_le32(x5);
out->d[6] = cpu_to_le32(x6);
out->d[7] = cpu_to_le32(x7);
out->d[8] = cpu_to_le32(x8);
out->d[9] = cpu_to_le32(x9);
out->d[10] = cpu_to_le32(x10);
out->d[11] = cpu_to_le32(x11);
out->d[12] = cpu_to_le32(x12);
out->d[13] = cpu_to_le32(x13);
out->d[14] = cpu_to_le32(x14);
out->d[15] = cpu_to_le32(x15);
}
void QrRegSuf::clear(){
sumsqy = 0;
Qty = 0;
qr = QR(SpdMatrix(Qty.size(), 0.0));
}
/*
Hyperparameters - vector of length 7 (if of length 6, then default value of 1.0 is used for Hyperparameters(3))
Sets prior_X and prior_Y
*/
void SBVAR_symmetric::SimsZhaDummyObservations(const TDenseVector &Hyperparameters, double PeriodsPerYear, double VarianceScale)
{
// check hyperparameters
TDenseVector Mu(7);
if (Hyperparameters.dim == 6)
{
for (int i=0; i < 7; i++)
if (i < 3)
Mu(i)=Hyperparameters(i);
else if (i == 3)
Mu(i)=1.0;
else
Mu(i)=Hyperparameters(i-1);
}
else if (Hyperparameters.dim == 7)
Mu=Hyperparameters;
else
throw dw_exception("Sims-Zha prior requires seven hyperparameters");
if (PeriodsPerYear <= 0)
throw dw_exception("Periods per year must be positive");
if (VarianceScale <= 0)
throw dw_exception("variance scale must be positive");
for (int i=6; i >= 0; i--)
if (Mu(i) <= 0) throw dw_exception("Sims-Zha hyperparameters must be positive");
// data info
int n_obs=NumberObservations();
TDenseMatrix Y=Data();
TDenseMatrix X=PredeterminedData();
// compute mean of initial data contained in the first observaton of the predetermined data
TDenseVector m(n_vars,0.0);
if (n_lags > 0)
{
for (int i=n_lags-1; i >= 0; i--)
for (int j=n_vars-1; j >= 0; j--)
m(j)+=X(0,i*n_vars+j);
m=(1.0/(double)n_lags)*m;
}
// compute variance of univariate AR residuals
TDenseVector s(n_vars,0.0), y(n_obs), e(n_obs);
TDenseMatrix Q, R, M(n_obs,n_lags+1);
for (int i=n_vars-1; i >= 0; i--)
{
for (int j=n_lags-1; j >= 0; j--)
M.Insert(0,j,X,0,n_obs-1,j*n_vars+i,j*n_vars+i);
M.Insert(0,n_lags,X,0,n_obs-1,n_predetermined-1,n_predetermined-1);
QR(Q,R,M);
y.ColumnVector(Y,i);
e=y - Q*(Transpose(Q)*y);
s(i)=Norm(e)/sqrt((double)n_obs);
}
// dummy observations
prior_Y.Zeros(2+n_vars*(n_lags+2),n_vars);
prior_X.Zeros(2+n_vars*(n_lags+2),n_predetermined);
// prior on a(k,i): dummy observations of the form
// y=s(i)/mu(1) * e(i,n_vars)
// x=0
int row=0;
for (int i=0; i < n_vars; i++)
prior_Y(row+i,i)=s(i)/Mu(0);
row+=n_vars;
// prior on constant: dummy observation of the form
// y=0
// x=1.0/(mu(1)*mu(3)) * e(n_predermined,n_predetermined)
prior_X(row,n_predetermined-1)=1.0/(Mu(0)*Mu(2));
row+=1;
if (n_lags > 0)
{
// random walk prior: dummy observations of the form
// y=s(i)/(mu(2)*mu(1)) * e(i,n_vars)
// x=s(i)/(mu(2)*mu(1)) * e(i,n_predetermined)
for (int i=0; i < n_vars; i++)
{
prior_Y(row+i,i)=s(i)/(Mu(0)*Mu(1));
prior_X(row+i,i)=s(i)/(Mu(0)*Mu(1));
}
row+=n_vars;
// lag decay prior: dummy observations of the form
// y=0
// x=s(i)*(4*j/PeriodsPerYear+1)^mu(4)/(mu(1)*mu(2)) * e(j)*nvars+i,n_pred)
for (int j=1; j < n_lags; j++)
{
for (int i=0; i < n_vars; i++)
prior_X(row+i,j*n_vars+i)=s(i)*pow(4.0*Mu(3)*(double)j/PeriodsPerYear+1.0,Mu(4))/(Mu(0)*Mu(1));
row+=n_vars;
}
// sums-of-coefficients prior: dummy observations of the form
// y=mu(5)*m(i) * e(i,n_vars)
// x=sum_j { mu(5)*m(i) * e((j-1)*n_vars+i,n_pred) }
for (int i=0; i < n_vars; i++)
{
prior_Y(row+i,i)=Mu(5)*m(i);
for (int j=0; j < n_lags; j++)
prior_X(row+i,j*n_vars+i)=Mu(5)*m(i);
}
row+=n_vars;
// co-persistence prior: dummy observation of the form
// y=sum_i { mu(6)*m(i) * e(i,n_vars) }
// x=sum_i,j { mu(6)*m(i) * e((j-1)*n_vars+i,n_pred) } + mu(6) * e(n_pred,n_pred)
for (int i=0; i < n_vars; i++)
{
prior_Y(row,i)=Mu(6)*m(i);
for (int j=0; j < n_lags; j++)
prior_X(row,j*n_vars+i)=Mu(6)*m(i);
}
prior_X(row,n_predetermined-1)=Mu(6);
}
// variance scale
prior_Y=sqrt(1.0/VarianceScale)*prior_Y;
prior_X=sqrt(1.0/VarianceScale)*prior_X;
}
int Feature06::fitCircle(int n, double *x_vec, double *y_vec, double& xc, double& yc, double& r)
{
int i;
if (n > 3) {
Eigen::MatrixXd A(n, 3);
Eigen::MatrixXd X(3, 1);
Eigen::MatrixXd B(n, 3);
Eigen::MatrixXd P(3, n);
Eigen::MatrixXd T(3, 3);
// We want the overdetermined linear equation system Ax = b
// Fill in matrix A
for (i = 0; i < n; i++) {
A(i, 0) = -2 * x_vec[i];
A(i, 1) = -2 * y_vec[i];
A(i, 2) = 1.0;
}
// Fill in vector b
for (i = 0; i < n; i++) {
B(i, 0) = -(x_vec[i] * x_vec[i] + y_vec[i] * y_vec[i]);
}
// Now we have the equation system with n equations and 3 unknowns.
// The LSQ-solution is given by the pseudo-inverse:
P = A.transpose();
T = P * A;
Eigen::FullPivLU<Eigen::MatrixXd> lu(T);
if (lu.isInvertible()) {
if (log(lu.determinant()) > MATRIX_LN_EPS) {
X = lu.inverse() * P * B;
// Extract circle center and radius
xc = X(0, 0);
yc = X(1, 0);
r = sqrt(-X(2, 0) + xc * xc + yc * yc);
return 1;
}
else {
// Points badly conditioned (n > 3)
return -3;
}
}
else {
// try QR
Eigen::FullPivHouseholderQR<Eigen::MatrixXd> QR(A);
X = QR.solve(B);
// Extract circle center and radius
xc = X(0, 0);
yc = X(1, 0);
r = sqrt(-X(2, 0) + xc * xc + yc * yc);
return 1;
}
}
else if (n == 3) {
double a, b, c, d, e, f, g;
a = x_vec[1] - x_vec[0];
b = y_vec[1] - y_vec[0];
c = x_vec[2] - x_vec[0];
d = y_vec[2] - y_vec[0];
e = a * (x_vec[0] + x_vec[1]) + b * (y_vec[0] + y_vec[1]);
f = c * (x_vec[0] + x_vec[2]) + d * (y_vec[0] + y_vec[2]);
g = 2 * (a * (y_vec[2] - y_vec[1]) - b * (x_vec[2] - x_vec[1]));
if (g != 0.0) {
xc = (d * e - b * f) / g;
yc = (a * f - c * e) / g;
r = sqrt((x_vec[0] - xc) * (x_vec[0] - xc) + (y_vec[0] - yc) * (y_vec[0] - yc));
return 2;
}
else {
// Points badly conditioned (n == 3)
return -2;
}
}
else {
// Too few points
xc = 0.0;
yc = 0.0;
r = -1.0;
return -1;
}
}