#include "rbfcore.h"
#include "mymesh/utility.h"
#include "Solver.h"
#include <armadillo>
#include <fstream>
#include <limits>
#include <unordered_map>
#include <ctime>
#include <chrono>
#include <iomanip>
#include<algorithm>
#include "mymesh/readers.h"
typedef std::chrono::high_resolution_clock Clock;
bool RBF_Core::Set_Hermite_DesignedCurve(){
if(curMethod!=Hermite_Tangent_UnitNormal)return false;
cout<<"Hermite_designcurve_weight: "<<Hermite_designcurve_weight<<endl;
double w = Hermite_designcurve_weight;
if(w==0)return false;
//saveK_finalH = K11;
int ne = edges.size()/2;
auto p_tangent = tangents.data();
if(1)for(int i=0;i<ne;++i){
int v1 = edges[i*2], v2 = edges[i*2+1];
arma::sp_mat b1(3,npt*3),b2(3,npt*3);
b1(0, v1+1*npt) = -p_tangent[v1*3+2];
b1(0, v1+2*npt) = p_tangent[v1*3+1];
b1(1, v1+0*npt) = p_tangent[v1*3+2];
b1(1, v1+2*npt) = -p_tangent[v1*3+0];
b1(2, v1+0*npt) = -p_tangent[v1*3+1];
b1(2, v1+1*npt) = p_tangent[v1*3+0];
b2(0, v2+1*npt) = -p_tangent[v2*3+2];
b2(0, v2+2*npt) = p_tangent[v2*3+1];
b2(1, v2+0*npt) = p_tangent[v2*3+2];
b2(1, v2+2*npt) = -p_tangent[v2*3+0];
b2(2, v2+0*npt) = -p_tangent[v2*3+1];
b2(2, v2+1*npt) = p_tangent[v2*3+0];
arma::sp_mat b3 = b1-b2, b4 = w* ( b3.t() * b3);
K11 += b4;
// K += b4;
// finalH += b4;
// if(isuse_sparse){
// sp_K += b4; sp_H += b4;
// }
}
else for(int i=0;i<ne;++i){
int v1 = edges[i*2], v2 = edges[i*2+1];
arma::sp_mat b(1,npt*3);
b(0,v1+1*npt) = -p_tangent[v1*3+2];
b(0,v1+2*npt) = p_tangent[v1*3+1];
b(0,v2+1*npt) = p_tangent[v2*3+2];
b(0,v2+2*npt) = -p_tangent[v2*3+1];
K += w* ( b.t() * b);
b.zeros();
b(0,v1+0*npt) = p_tangent[v1*3+2];
b(0,v1+2*npt) = -p_tangent[v1*3+0];
b(0,v2+0*npt) = -p_tangent[v2*3+2];
b(0,v2+2*npt) = p_tangent[v2*3+0];
K += w* ( b.t() * b);
b.zeros();
b(0,v1+0*npt) = -p_tangent[v1*3+1];
b(0,v1+1*npt) = p_tangent[v1*3+0];
b(0,v2+0*npt) = p_tangent[v2*3+1];
b(0,v2+1*npt) = -p_tangent[v2*3+0];
K += w* ( b.t() * b);
}
//saveK = K;
return true;
}
void RBF_Core::Set_Hermite_PredictNormal_TangentConstraint(vector<double>&pts, vector<double>&tgt, vector<uint> &ede){
Set_Hermite_PredictNormal(pts);
//Set_Hermite_DesignedCurve();
}
int RBF_Core::Solve_Hermite_PredictNormal_TangentConstraint_UnitNorm(){
arma::vec eigval, ny;
arma::mat eigvec;
arma::sp_mat c2(npt,npt*3);
arma::sp_mat eye;
auto p_tangent = tangents.data();
for(int i=0;i<npt;++i){
c2(i,i) = p_tangent[i*3];
c2(i,i+1*npt) = p_tangent[i*3+1];
c2(i,i+2*npt) = p_tangent[i*3+2];
}
eye.eye(npt*3,npt*3);
arma::mat cct(c2*c2.t());
arma::mat p = eye - c2.t()*arma::inv(cct)*c2;
arma::mat AA = p*K*p;
AA = (AA.t()+AA)*0.5;
ny = eig_sym( eigval, eigvec, AA);
//cout<<eigval.t()<<endl;
//vector<pair<double,int>>projection;
vector<bool>isNullVec(npt*3,false);
for(int i=0;i<npt*3;++i){
double val = arma::norm(c2*eigvec.col(i));
//projection.emplace_back(val,i);
if(val<1e-5)isNullVec[i]=true;
}
//sort(projection.begin(),projection.end());
//for(int i=0;i<npt*3;++i)cout<<projection[i].first<<' ';cout<<endl<<endl;
//for(int i=0;i<npt*3;++i)cout<<projection[i].second<<' ';cout<<endl<<endl;
int smalleig = 0;
for(int i=0;i<npt*3;++i)if(isNullVec[i]){smalleig = i;break;}
//smalleig = 0;
//ny = eig_sym( eigval, eigvec, K);
//int smalleig = 0;
//vector<double>dottangenttest(npt);
initnormals.resize(npt*3);
arma::vec y(npt*4);
for(int i=0;i<npt;++i)y(i) = 0;
for(int i=0;i<npt*3;++i)y(i+npt) = eigvec(i,smalleig);
for(int i=0;i<npt;++i){
initnormals[i*3] = y(npt+i);
initnormals[i*3+1] = y(npt+i+npt);
initnormals[i*3+2] = y(npt+i+npt*2);
//MyUtility::normalize(normals.data()+i*3);
//dottangenttest[i] = MyUtility::dot(p_tangent+i*3, normals.data()+i*3);
}
//for(auto a:dottangenttest)cout<<a<<' ';cout<<endl;
//NormalRecification(1);
//b = bprey * y;
//a = Minv * (y - N*b);
//sol.energy = arma::dot(a,M*a);
SetInitnormal_Uninorm();
cout<<"Solve_Hermite_PredictNormal_Tangent_UnitNorm"<<endl;
return 1;
}
/*************************************************/
//double optfunc_Hermite(const vector<double>&x, vector<double>&grad, void *fdata);
static int countopt = 0;
double optfunc_Hermite_Tangents(const vector<double>&x, vector<double>&grad, void *fdata){
RBF_Core *drbf = reinterpret_cast<RBF_Core*>(fdata);
int n = drbf->npt;
arma::vec arma_x(n*3);
vector<double>&coff_cos = drbf->coff_cos;
vector<double>&coff_sin = drbf->coff_sin;
//( sin(a)cos(b), sin(a)sin(b), cos(a) ) a =>[0, pi], b => [-pi, pi];
vector<double>cosa(n);
vector<double>sina(n);
for(int i=0;i<n;++i){
cosa[i] = cos(x[i]);
sina[i] = sin(x[i]);
}
for(int i=0;i<n;++i){
for(int j=0;j<3;++j){
arma_x(i+n*j) = coff_cos[i*3+j] * cosa[i] + coff_sin[i*3+j] * sina[i];
}
}
arma::vec a2 = drbf->finalH * arma_x;
if (!grad.empty()) {
grad.resize(n);
for(int i=0;i<n;++i){
grad[i] = 0;
for(int j=0;j<3;++j)grad[i] += a2(i+n*j) * (-coff_cos[i*3+j] * sina[i] + coff_sin[i*3+j] * cosa[i]);
}
}
//for(auto a:grad)cout<<a<<' ';cout<<endl;
double re = arma::dot( arma_x, a2 );
countopt++;
//cout<<countopt++<<' '<<re<<endl;
return re;
}
int RBF_Core::Solve_Hermite_PredictNormal_TangentConstraint_UnitNormal(){
if(1){
Solve_Hermite_PredictNormal_TangentConstraint_UnitNorm();
//initnormals = newnormals;
}else{
LocalEigenInit_PredictNormal_TangentConstraint();
//newnormals = initnormals;
}
//initnormals = newnormals = normals;
SetInitnormal_Uninorm();
BuildLFrame(initnormals_uninorm);
//cout<<"coanima"<<endl;
// return -1;
//LocalIterativeSolver(sol, newnormals, 1000, 1e-7);
//LocalIterativeSolver_Tconstraint(sol, newnormals, tangents, 1000, 1e-7);
sol.solveval.resize(npt);
for(int i=0;i<npt;++i){
sol.solveval[i] = 0.0;
}
//cout<<"smallvec: "<<smallvec<<endl;
if(1){
vector<double>upper(npt);
vector<double>lower(npt);
for(int i=0;i<npt;++i){
upper[i] = 2 * my_PI;
lower[i] = -2 * my_PI;
}
countopt = 0;
//LocalIterativeSolver(sol,kk==0?normals:newnormals,300,1e-7);
Solver::nloptwrapper(lower,upper,optfunc_Hermite_Tangents,this,1e-7,3000,sol);
cout<<"number of call: "<<countopt<<endl;
//for(int i=0;i<npt;++i)cout<< sol.solveval[i]<<' ';cout<<endl;
}
newnormals.resize(npt*3);
arma::vec y(npt*4);
for(int i=0;i<npt;++i)y(i) = 0;
if(1){
for(int i=0;i<npt;++i){
double cosa = cos(sol.solveval[i]), sina = sin(sol.solveval[i]);
for(int j=0;j<3;++j){
newnormals[i*3+j] = y(npt*(j+1)+i) = coff_cos[i*3+j]*cosa+coff_sin[i*3+j]*sina;
}
MyUtility::normalize(newnormals.data()+i*3);
}
}else{
}
Set_RBFCoef(y);
//sol.energy = arma::dot(a,M*a);
cout<<"Solve_Hermite_PredictNormal_TangentConstraint_UnitNormal"<<endl;
return 1;
}
int RBF_Core::Opt_Hermite_PredictNormal_TangentConstraint_UnitNormal(){
SetInitnormal_Uninorm();
BuildLFrame(initnormals_uninorm);
sol.solveval.resize(npt);
for(int i=0;i<npt;++i){
sol.solveval[i] = 0.0;
}
//cout<<"smallvec: "<<smallvec<<endl;
if(1){
vector<double>upper(npt);
vector<double>lower(npt);
for(int i=0;i<npt;++i){
upper[i] = 2 * my_PI;
lower[i] = -2 * my_PI;
}
countopt = 0;
Solver::nloptwrapper(lower,upper,optfunc_Hermite_Tangents,this,1e-7,3000,sol);
cout<<"number of call: "<<countopt<<endl;
//for(int i=0;i<npt;++i)cout<< sol.solveval[i]<<' ';cout<<endl;
}
newnormals.resize(npt*3);
arma::vec y(npt*4);
for(int i=0;i<npt;++i)y(i) = 0;
if(1){
for(int i=0;i<npt;++i){
double cosa = cos(sol.solveval[i]), sina = sin(sol.solveval[i]);
for(int j=0;j<3;++j){
newnormals[i*3+j] = y(npt*(j+1)+i) = coff_cos[i*3+j]*cosa+coff_sin[i*3+j]*sina;
}
MyUtility::normalize(newnormals.data()+i*3);
}
}else{
}
Set_RBFCoef(y);
//sol.energy = arma::dot(a,M*a);
cout<<"Opt_Hermite_PredictNormal_TangentConstraint_UnitNormal"<<endl;
return 1;
}
void RBF_Core::SparseMatrixTest(){
int nnpt = 500;
int n = nnpt*4;
int k = 5;
auto t1 = Clock::now();
arma::sp_mat A = arma::sprandu<arma::sp_mat>(n*n, n*k, 1e-4);//double(k)/nnpt
arma::vec bb;
cout << "init time: " << (setup_time = std::chrono::nanoseconds(Clock::now() - t1).count()/1e9) << endl;
t1 = Clock::now();
arma::sp_mat AA(A.t()*A);
bb.ones(n*n);
bb=A.t()*bb;
cout << "AAbb time: " << (setup_time = std::chrono::nanoseconds(Clock::now() - t1).count()/1e9) << endl;
cout<<"SparseMatrixTest "<<AA.n_nonzero<<endl;
t1 = Clock::now();
arma::vec X = arma::spsolve( AA,bb );
cout << "Setup time: " << (setup_time = std::chrono::nanoseconds(Clock::now() - t1).count()/1e9) << endl;
exit(12321);
}
void RBF_Core::LocalEigenInit_PredictNormal_TangentConstraint(){
cout<<"LocalEigenInit_PredictNormal"<<endl;
arma::mat eigenmatrix;
arma::vec eigenval;
initnormals.resize(npt*3);
vector<double>eva(3);
for(int i=0;i<npt;++i){
arma::mat aa(3,3);
for(int j=0;j<3;++j)for(int k=0;k<3;++k)aa(j,k) = K(i+j*npt,i+k*npt);
arma::mat c2(1,3);
arma::mat eye;
auto p_tangent = tangents.data();
{
c2(0,0) = p_tangent[i*3];
c2(0,1) = p_tangent[i*3+1];
c2(0,2) = p_tangent[i*3+2];
}
eye.eye(3,3);
arma::mat cct(c2*c2.t());
arma::mat p = eye - c2.t()*arma::inv(cct)*c2;
arma::mat AA = p*aa*p;
AA = (AA.t()+AA)*0.5;
eig_sym( eigenval, eigenmatrix, AA);
vector<bool>isNullVec(3,false);
for(int i=0;i<3;++i){
double val = arma::norm(c2*eigenmatrix.col(i));
if(val<1e-5)isNullVec[i]=true;
}
int smalleig = 0;
for(int i=0;i<npt*3;++i)if(isNullVec[i]){smalleig = i;break;}
for(int j=0;j<3;++j)eva[j] = eigenval(j);
double stdv = eigenval(2) - eigenval(1);
//cout<<stdv<<' ';
for(int j=0;j<3;++j)initnormals[i*3+j] = stdv*eigenmatrix(j,smalleig);
}
cout<<endl;
//newnormals = initnormals;
NormalReOrientation(initnormals,false);
}
void RBF_Core::BuildLFrame(vector<double>&nors){
double css[3];
auto p_nor = nors.data();
coff_cos.resize(npt*3);
coff_sin.resize(npt*3);
auto p_cos = coff_cos.data();
auto p_sin = coff_sin.data();
for(int i=0;i<npt;++i){
MyUtility::cross(tangents.data()+i*3,p_nor+i*3,css);
MyUtility::normalize(css);
MyUtility::copyVec(p_nor+i*3,p_cos+i*3);
MyUtility::copyVec(css,p_sin+i*3);
}
}
int RBF_Core::Solve_Hermite_PredictNormal_TangentConstraint_UnitNorm_Iterative_MST(){
return 1;
}
