int ray_trace(int h,int w,double x0, double y0, double k0, int p, double D)
{
int px,py,pp=(p*k0>0?1:-1);
double nx0,ny0,nx1,ny1,nx,ny,d0,d1;
printf("%d %d %.4lf %.4lf %.6lf %d %.4lf\n", h,w,x0,y0,k0,p,D);
if(D<=0) return 0;
if(hmap[h][w]=='X' && x0!=mx && y0!=my && equal(my,k0*(mx-x0)+y0)) {
d0=distance(x0,y0,mx,my);
printf("@@@%.4lf %.4lf\n",D,d0);
if(d0<D || equal(d0,D)) return 1;
else return 0;
}
// printf(">%c %.4lf s:%.7lf\n",hmap[h][w],d0,fabs(my-k0*(mx-x0)-y0));
if(p>0) nx0=floor(x0+1);
else nx0=ceil(x0-1);
if(pp>0) ny1=floor(y0+1);
else ny1=ceil(y0-1);
ny0=intersect_x(x0,y0,k0,nx0); nx1=intersect_y(x0,y0,k0,ny1);
d0=distance(x0,y0,nx0,ny0); d1=distance(x0,y0,nx1,ny1);
// printf("n0: %.4lf %.4lf %.4lf ", nx0, ny0, d0);
// printf("n1: %.4lf %.4lf %.4lf\n", nx1, ny1,d1);
if(equal(d0,d1)) {
if(hmap[h+p][w+pp]=='#') {
if(hmap[h+p][w]=='#' && hmap[h][w+pp]=='#')
return ray_trace(h,w,nx0,ny0,k0,-1*p,D-d0);
else if(hmap[h+p][w]=='#')
return ray_trace(h+p,w,nx0,ny0,ref_x(k0),p,D-d0);
else if(hmap[h][w+pp]=='#')
return ray_trace(h,w+pp,nx0,ny0,ref_y(k0),-1*p,D-d0);
else return 0;
} else return ray_trace(h+p,w+pp,nx0,ny0,k0,p,D-d0);
}
if(d0<d1) {
if(hmap[h+p][w]=='#')
return ray_trace(h,w,nx0,ny0,ref_x(k0),p,D-d0);
else return ray_trace(h+p,w,nx0,ny0,k0,p,D-d0);
} else {
if(hmap[h][w+pp]=='#')
return ray_trace(h,w,nx1,ny1,ref_y(k0),-1*p,D-d1);
else return ray_trace(h,w+pp,nx1,ny1,k0,p,D-d1);
}
}
template<typename Scalar> void sparse_ldlt(int rows, int cols)
{
static bool odd = true;
odd = !odd;
double density = (std::max)(8./(rows*cols), 0.01);
typedef Matrix<Scalar,Dynamic,Dynamic> DenseMatrix;
typedef Matrix<Scalar,Dynamic,1> DenseVector;
SparseMatrix<Scalar> m2(rows, cols);
DenseMatrix refMat2(rows, cols);
DenseVector b = DenseVector::Random(cols);
DenseVector refX(cols), x(cols);
initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeUpperTriangular, 0, 0);
SparseMatrix<Scalar> m3 = m2 * m2.adjoint(), m3_lo(rows,rows), m3_up(rows,rows);
DenseMatrix refMat3 = refMat2 * refMat2.adjoint();
refX = refMat3.template selfadjointView<Upper>().ldlt().solve(b);
typedef SparseMatrix<Scalar,Upper|SelfAdjoint> SparseSelfAdjointMatrix;
x = b;
SparseLDLT<SparseSelfAdjointMatrix> ldlt(m3);
if (ldlt.succeeded())
ldlt.solveInPlace(x);
else
std::cerr << "warning LDLT failed\n";
VERIFY_IS_APPROX(refMat3.template selfadjointView<Upper>() * x, b);
VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LDLT: default");
#ifdef EIGEN_CHOLMOD_SUPPORT
{
x = b;
SparseLDLT<SparseSelfAdjointMatrix, Cholmod> ldlt2(m3);
if (ldlt2.succeeded())
{
ldlt2.solveInPlace(x);
VERIFY_IS_APPROX(refMat3.template selfadjointView<Upper>() * x, b);
VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LDLT: cholmod solveInPlace");
x = ldlt2.solve(b);
VERIFY_IS_APPROX(refMat3.template selfadjointView<Upper>() * x, b);
VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LDLT: cholmod solve");
}
else
std::cerr << "warning LDLT failed\n";
}
#endif
// new Simplicial LLT
// new API
{
SparseMatrix<Scalar> m2(rows, cols);
DenseMatrix refMat2(rows, cols);
DenseVector b = DenseVector::Random(cols);
DenseVector ref_x(cols), x(cols);
DenseMatrix B = DenseMatrix::Random(rows,cols);
DenseMatrix ref_X(rows,cols), X(rows,cols);
initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeLowerTriangular, 0, 0);
for(int i=0; i<rows; ++i)
m2.coeffRef(i,i) = refMat2(i,i) = internal::abs(internal::real(refMat2(i,i)));
SparseMatrix<Scalar> m3 = m2 * m2.adjoint(), m3_lo(rows,rows), m3_up(rows,rows);
DenseMatrix refMat3 = refMat2 * refMat2.adjoint();
m3_lo.template selfadjointView<Lower>().rankUpdate(m2,0);
m3_up.template selfadjointView<Upper>().rankUpdate(m2,0);
// with a single vector as the rhs
ref_x = refMat3.template selfadjointView<Lower>().llt().solve(b);
x = SimplicialCholesky<SparseMatrix<Scalar>, Lower>().setMode(odd ? SimplicialCholeskyLLt : SimplicialCholeskyLDLt).compute(m3).solve(b);
VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "SimplicialCholesky: solve, full storage, lower, single dense rhs");
x = SimplicialCholesky<SparseMatrix<Scalar>, Upper>().setMode(odd ? SimplicialCholeskyLLt : SimplicialCholeskyLDLt).compute(m3).solve(b);
VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "SimplicialCholesky: solve, full storage, upper, single dense rhs");
x = SimplicialCholesky<SparseMatrix<Scalar>, Lower>(m3_lo).solve(b);
VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "SimplicialCholesky: solve, lower only, single dense rhs");
x = SimplicialCholesky<SparseMatrix<Scalar>, Upper>(m3_up).solve(b);
VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "SimplicialCholesky: solve, upper only, single dense rhs");
// with multiple rhs
ref_X = refMat3.template selfadjointView<Lower>().llt().solve(B);
X = SimplicialCholesky<SparseMatrix<Scalar>, Lower>().setMode(odd ? SimplicialCholeskyLLt : SimplicialCholeskyLDLt).compute(m3).solve(B);
VERIFY(ref_X.isApprox(X,test_precision<Scalar>()) && "SimplicialCholesky: solve, full storage, lower, multiple dense rhs");
X = SimplicialCholesky<SparseMatrix<Scalar>, Upper>().setMode(odd ? SimplicialCholeskyLLt : SimplicialCholeskyLDLt).compute(m3).solve(B);
VERIFY(ref_X.isApprox(X,test_precision<Scalar>()) && "SimplicialCholesky: solve, full storage, upper, multiple dense rhs");
// with a sparse rhs
// SparseMatrix<Scalar> spB(rows,cols), spX(rows,cols);
// B.diagonal().array() += 1;
// spB = B.sparseView(0.5,1);
//
// ref_X = refMat3.template selfadjointView<Lower>().llt().solve(DenseMatrix(spB));
//
// spX = SimplicialCholesky<SparseMatrix<Scalar>, Lower>(m3).solve(spB);
// VERIFY(ref_X.isApprox(spX.toDense(),test_precision<Scalar>()) && "LLT: cholmod solve, multiple sparse rhs");
//
// spX = SimplicialCholesky<SparseMatrix<Scalar>, Upper>(m3).solve(spB);
// VERIFY(ref_X.isApprox(spX.toDense(),test_precision<Scalar>()) && "LLT: cholmod solve, multiple sparse rhs");
}
// for(int i=0; i<rows; ++i)
// m2.coeffRef(i,i) = refMat2(i,i) = internal::abs(internal::real(refMat2(i,i)));
//
// refX = refMat2.template selfadjointView<Upper>().ldlt().solve(b);
// typedef SparseMatrix<Scalar,Upper|SelfAdjoint> SparseSelfAdjointMatrix;
// x = b;
// SparseLDLT<SparseSelfAdjointMatrix> ldlt(m2);
// if (ldlt.succeeded())
// ldlt.solveInPlace(x);
// else
// std::cerr << "warning LDLT failed\n";
//
// VERIFY_IS_APPROX(refMat2.template selfadjointView<Upper>() * x, b);
// VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LDLT: default");
}
template<typename Scalar> void sparse_llt(int rows, int cols)
{
double density = std::max(8./(rows*cols), 0.01);
typedef Matrix<Scalar,Dynamic,Dynamic> DenseMatrix;
typedef Matrix<Scalar,Dynamic,1> DenseVector;
// TODO fix the issue with complex (see SparseLLT::solveInPlace)
SparseMatrix<Scalar> m2(rows, cols);
DenseMatrix refMat2(rows, cols);
DenseVector b = DenseVector::Random(cols);
DenseVector ref_x(cols), x(cols);
DenseMatrix B = DenseMatrix::Random(rows,cols);
DenseMatrix ref_X(rows,cols), X(rows,cols);
initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeLowerTriangular, 0, 0);
for(int i=0; i<rows; ++i)
m2.coeffRef(i,i) = refMat2(i,i) = internal::abs(internal::real(refMat2(i,i)));
ref_x = refMat2.template selfadjointView<Lower>().llt().solve(b);
if (!NumTraits<Scalar>::IsComplex)
{
x = b;
SparseLLT<SparseMatrix<Scalar> > (m2).solveInPlace(x);
VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "LLT: default");
}
#ifdef EIGEN_CHOLMOD_SUPPORT
// legacy API
{
// Cholmod, as configured in CholmodSupport.h, only supports self-adjoint matrices
SparseMatrix<Scalar> m3 = m2.adjoint()*m2;
DenseMatrix refMat3 = refMat2.adjoint()*refMat2;
ref_x = refMat3.template selfadjointView<Lower>().llt().solve(b);
x = b;
SparseLLT<SparseMatrix<Scalar>, Cholmod>(m3).solveInPlace(x);
VERIFY((m3*x).isApprox(b,test_precision<Scalar>()) && "LLT legacy: cholmod solveInPlace");
x = SparseLLT<SparseMatrix<Scalar>, Cholmod>(m3).solve(b);
VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "LLT legacy: cholmod solve");
}
// new API
{
// Cholmod, as configured in CholmodSupport.h, only supports self-adjoint matrices
SparseMatrix<Scalar> m3 = m2 * m2.adjoint(), m3_lo(rows,rows), m3_up(rows,rows);
DenseMatrix refMat3 = refMat2 * refMat2.adjoint();
m3_lo.template selfadjointView<Lower>().rankUpdate(m2,0);
m3_up.template selfadjointView<Upper>().rankUpdate(m2,0);
// with a single vector as the rhs
ref_x = refMat3.template selfadjointView<Lower>().llt().solve(b);
x = CholmodDecomposition<SparseMatrix<Scalar>, Lower>(m3).solve(b);
VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "LLT: cholmod solve, single dense rhs");
x = CholmodDecomposition<SparseMatrix<Scalar>, Upper>(m3).solve(b);
VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "LLT: cholmod solve, single dense rhs");
x = CholmodDecomposition<SparseMatrix<Scalar>, Lower>(m3_lo).solve(b);
VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "LLT: cholmod solve, single dense rhs");
x = CholmodDecomposition<SparseMatrix<Scalar>, Upper>(m3_up).solve(b);
VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "LLT: cholmod solve, single dense rhs");
// with multiple rhs
ref_X = refMat3.template selfadjointView<Lower>().llt().solve(B);
#ifndef EIGEN_DEFAULT_TO_ROW_MAJOR
// TODO make sure the API is properly documented about this fact
X = CholmodDecomposition<SparseMatrix<Scalar>, Lower>(m3).solve(B);
VERIFY(ref_X.isApprox(X,test_precision<Scalar>()) && "LLT: cholmod solve, multiple dense rhs");
X = CholmodDecomposition<SparseMatrix<Scalar>, Upper>(m3).solve(B);
VERIFY(ref_X.isApprox(X,test_precision<Scalar>()) && "LLT: cholmod solve, multiple dense rhs");
#endif
// with a sparse rhs
SparseMatrix<Scalar> spB(rows,cols), spX(rows,cols);
B.diagonal().array() += 1;
spB = B.sparseView(0.5,1);
ref_X = refMat3.template selfadjointView<Lower>().llt().solve(DenseMatrix(spB));
spX = CholmodDecomposition<SparseMatrix<Scalar>, Lower>(m3).solve(spB);
VERIFY(ref_X.isApprox(spX.toDense(),test_precision<Scalar>()) && "LLT: cholmod solve, multiple sparse rhs");
spX = CholmodDecomposition<SparseMatrix<Scalar>, Upper>(m3).solve(spB);
VERIFY(ref_X.isApprox(spX.toDense(),test_precision<Scalar>()) && "LLT: cholmod solve, multiple sparse rhs");
}
#endif
}