/* Solve min||Ax-b|| for a matrix A whose rank is given. */
VectorXd solve( const VectorXd& b , bool inY=false)
{
if( rank==0 ) return VectorXd::Zero(NC);
/* Approximate solution using no basis transfo (result is meanigless
* appart from the computation time pov. */
/*
VectorXd sol = b.head(rank);
matrixL().solveInPlace( sol );
VectorXd res; res.setZero(NC);
res.head(rank)=sol; return res;
*/
/* With plain matrices. */
/*
VectorXd sol = matrixUr().transpose()*b;
matrixL().solveInPlace( sol );
return matrixVr()*sol;
*/
/* Using the HH representation of V. */
assert( m_computeThinU || m_computeFullU );
VectorXd sol;
if( inY ) sol.setZero(rank); else sol.setZero(NC);
sol.head(rank) = matrixUr().transpose()*b;
matrixL().solveInPlace( sol.head(rank) );
if( ! inY ) sol.applyOnTheLeft(qrv.householderQ());
return sol;
}
ConjugateGradientType(Functional f, const GridDescription &gdin, double kT, VectorXd *data, LineMinimizer lm,
double stepsize = 0.1)
: MinimizerInterface(f, gdin, kT, data), step(stepsize), orig_step(step), direction(*data), oldgrad(*data), linmin(lm) {
direction.setZero();
oldgrad.setZero();
oldgradsqr = 0;
}
void minimize(Functional newf, const GridDescription &gdnew, VectorXd *newx = 0) {
step = orig_step;
MinimizerInterface::minimize(newf, gdnew, newx);
if (newx) {
direction = *newx;
oldgrad = *newx;
}
direction.setZero();
oldgrad.setZero();
oldgradsqr = 0;
}
/**
* reconstruct the displacements u in Euler space with RS coordinates y
* provided.
*
* @param y the RS coordinates.
* @param u the constructed Euler coordinates represents the displacements.
*
* @return true if construction is success.
*/
bool RS2Euler::reconstruct(const VectorXd &y, VectorXd &u){
assert (tetmesh != NULL);
const int node_numx3 = tetmesh->nodes().size()*3;
bool succ = true;
// assemble the right_side
VectorXd b;
assemble_b(y,b);
assert_eq(VG_t.cols(),b.size());
assert_eq(VG_t.rows(),node_numx3);
VectorXd right_side(node_numx3 + numFixedDofs());
right_side.segment(0, node_numx3) = VG_t*b;
right_side.segment(node_numx3,numFixedDofs()).setZero();
right_side.segment(node_numx3,barycenter_uc.size()) = barycenter_uc;
// solve A*u = right_side
assert_eq (right_side.size(), A_solver.rows());
u = A_solver.solve(right_side);
// get the result
if(A_solver.info()!=Eigen::Success) {
succ = false;
u.resize(node_numx3);
u.setZero();
ERROR_LOG("failed to solve for A X = P.");
}else{
assert_gt(u.size(), node_numx3);
const VectorXd x = u.head(node_numx3);
u = x;
}
return succ;
}
void ImplicitEuler::ImplicitCalculateForces( MatrixXd& TVk, SparseMatrix<double>& forceGradient, VectorXd& x_k, VectorXd& f){
// //gravity
f.setZero();
for(unsigned int i=0; i<M.tets.size(); i++){
double vertex_mass = M.tets[i].undeformedVol/4;//assume const density 1
Vector4i indices = M.tets[i].verticesIndex;
f(3*indices(0)+1) += vertex_mass*gravity;
f(3*indices(1)+1) += vertex_mass*gravity;
f(3*indices(2)+1) += vertex_mass*gravity;
f(3*indices(3)+1) += vertex_mass*gravity;
}
//elastic
for(unsigned int i=0; i<M.tets.size(); i++){
Vector4i indices = M.tets[i].verticesIndex;
MatrixXd F_tet = M.tets[i].computeElasticForces(TVk, simTime%2);
f.segment<3>(3*indices(0)) += F_tet.col(0);
f.segment<3>(3*indices(1)) += F_tet.col(1);
f.segment<3>(3*indices(2)) += F_tet.col(2);
f.segment<3>(3*indices(3)) += F_tet.col(3);
}
// cout<<f<<endl<<endl;
//damping
f += rayleighCoeff*forceGradient*(x_k - x_old)/h;
// cout<<f<<endl<<endl;
return;
}
void BuildCausalVector(VectorXd& vec2Build , VectorXd& index){
vec2Build.setZero();
for(int i = 0; i <index.size(); i++){
if(index[i] >0){
vec2Build[index[i]-1] = 1;
}
}
}
VectorXd vec(MatrixXd& M){
VectorXd V;
V.setZero(M.rows()*M.cols());
for(int i=0;i<M.cols();i++){
V.segment(i*M.rows(), M.rows()) = M.col(i);
}
return V;
}
VectorXd Tetra10::Give_weights()
{
VectorXd w;
w.setZero(ngauss);
w(0) = 0.0416666666666667;
w(1) = 0.0416666666666667;
w(2) = 0.0416666666666667;
w(3) = 0.0416666666666667;
return w;
}
VectorXd vech(MatrixXd& M){
int n = M.rows();
VectorXd V;
V.setZero(n*(n-1)/2+n);
V.segment(0,n) = M.diagonal();
int k = n;
for(int i=0;i<n-1;i++){
V.segment(k,n-i-1) = M.block(i,i+1,1,n-i-1).transpose();
k += n-i-1;
}
return V;
}
void Simulation::calculateElasticForces(VectorXd &f, MatrixXd& TV){
f.setZero();
//elastic
for(unsigned int i=0; i< M.tets.size(); i++){
Vector4i indices = M.tets[i].verticesIndex;
MatrixXd F_tet = M.tets[i].computeElasticForces(TV, 1);
f.segment<3>(3*indices(0)) += F_tet.col(0);
f.segment<3>(3*indices(1)) += F_tet.col(1);
f.segment<3>(3*indices(2)) += F_tet.col(2);
f.segment<3>(3*indices(3)) += F_tet.col(3);
}
return;
}
VectorXd Triangle<ConcreteShape>::QuadratureIntegrationWeight(const int order) {
VectorXd wn;
if(order == 3) {
int num_pts = 12;
wn.setZero(num_pts);
quadrature_weights_p3_triangle(wn.data());
} else {
std::cerr << "ERROR: Order NOT implemented!\n";
MPI::COMM_WORLD.Abort(-1);
}
return wn;
}
const VectorXd &AniEditDM::getUforConstraint(const int frame)const{
static VectorXd tempt_u0;
if (interpolator != NULL && totalFrameNum() > 0){
assert_in(frame,0, totalFrameNum()-1);
return interpolator->getUforConstraint(frame);
}else{
if (tempt_u0.size() != fullDim()){
tempt_u0.resize(fullDim());
tempt_u0.setZero();
}
return tempt_u0;
}
}
VectorXd multivariateNormal_density(const MatrixXd &x,
const MatrixXd &Mu, const MatrixXd &Sigma) {
if ( x.rows() != Mu.rows()) {
std::cerr << "FROM multivariateNormal_density." << std::endl;
std::cerr << "ERROR: X_ROWS != MU_ROWS." << std::endl;
}
VectorXd res;
int T = x.rows();
res.setZero(T);
for(int i = 0; i<x.rows(); i++) {
res(i) = double_multivariateNormal_density(x.row(i),
Mu.row(i),
Sigma);
}
return res;
}
const VectorXd &AniEditDM::getInputU(const int frame, const bool translate)const{
static VectorXd tempt_u0;
assert_in (frame, 0, totalFrameNum() );
if (interpolator != NULL && totalFrameNum() > 0){
if (!translate){
return interpolator->getInputU(frame);
}else{
tempt_u0 = interpolator->getInputU(frame);
this->translate(tempt_u0, frame);
return tempt_u0;
}
}else{
if (tempt_u0.size() != fullDim()){
tempt_u0.resize(fullDim());
tempt_u0.setZero();
}
return tempt_u0;
}
}
void ImplicitEuler::ImplicitTVtoX(VectorXd& x_tv, MatrixXd& TVk){
x_tv.setZero();
for(unsigned int i = 0; i < M.tets.size(); i++){
Vector4i indices = M.tets[i].verticesIndex;
x_tv(3*indices(0)) = TVk.row(indices(0))[0];
x_tv(3*indices(0)+1) = TVk.row(indices(0))[1];
x_tv(3*indices(0)+2) = TVk.row(indices(0))[2];
x_tv(3*indices(1)) = TVk.row(indices(1))[0];
x_tv(3*indices(1)+1) = TVk.row(indices(1))[1];
x_tv(3*indices(1)+2) = TVk.row(indices(1))[2];
x_tv(3*indices(2)) = TVk.row(indices(2))[0];
x_tv(3*indices(2)+1) = TVk.row(indices(2))[1];
x_tv(3*indices(2)+2) = TVk.row(indices(2))[2];
x_tv(3*indices(3)) = TVk.row(indices(3))[0];
x_tv(3*indices(3)+1) = TVk.row(indices(3))[1];
x_tv(3*indices(3)+2) = TVk.row(indices(3))[2];
}
}
void read_SparseMatrix(SparseMatrix<double,0,int>& M, string path,string name){
FILE * pFile;
pFile = fopen((path + name+"i.bin").c_str(), "rb");
if (pFile==NULL) {fputs ("File error",stderr); exit (1);}
int n,m,ni;
if(1 != fread(&n,sizeof(int),1,pFile)){
fputs ("Read error\n",stderr); exit (1);
}
if(1 != fread(&m,sizeof(int),1,pFile)){
fputs ("Read error\n",stderr); exit (1);
}
if(1 != fread(&ni,sizeof(int),1,pFile)){
fputs ("Read error\n",stderr); exit (1);
}
M.resize(n,m);
MatrixXi ij;
ij.setZero(ni,2);
if(ni*2 != fread(ij.data(),sizeof(int),ni*2,pFile)){
fputs ("Read error\n",stderr); exit (1);
}
fclose(pFile);
VectorXd v;
v.setZero(ni);
pFile = fopen((path + name+"v.bin").c_str(), "rb");
if (pFile==NULL) {fputs ("File error",stderr); exit (1);}
if(ni != fread(v.data(),sizeof(double),ni,pFile)){
fputs ("Read error\n",stderr); exit (1);
}
fclose(pFile);
vector<T> coef;
for(int i=0;i<ni;i++){
coef.push_back(T(ij(i,0),ij(i,1),v(i)));
}
M.setFromTriplets(coef.begin(), coef.end());
}
int Simulation::initializeSimulation(double deltaT, int iterations, char method, MatrixXi& TT, MatrixXd& TV, MatrixXd& B, vector<int>& moveVertices, vector<int> fixVertices, double youngs, double poissons){
iters = iterations;
if (method =='e'){
integrator = new Verlet();
cout<<"Initialized Verlet"<<endl;
}else if(method == 'i'){
integrator = new ImplicitEuler();
cout<<"Initialized Implicit Euler"<<endl;
}
else if(method == 'n'){
integrator = new ImplicitNewmark();
cout<<"Initialized Implicit Newmark"<<endl;
}
else{
cout<<"Method not supported yet"<<endl;
exit(0);
}
VectorXd force;
force.resize(3*TV.rows());
force.setZero();
setInitPosition(force, fixVertices, moveVertices);
if(moveVertices.size()>0 or fixVertices.size()>0){
//cout << "DOING STUFFS" << endl;
MatrixXd newTV;
newTV.resize(TV.rows(), TV.cols());
newTV.setZero();
MatrixXi newTT;
newTT.resize(TT.rows(), TT.cols());
newTT.setZero();
//cout << "MoveVertsSize :: " << moveVertices.size() << endl;
//TODO: Make this shit more efficient
//Hash maps or something
vector<int> vertexNewIndices;
for(int i=0; i<TV.rows(); i++){
bool flag =false;
for(unsigned int j=0; j<fixVertices.size(); j++){
if(i==fixVertices[j]){
flag = true;
}
}
for(unsigned int j=0; j<moveVertices.size(); j++){
if(i==moveVertices[j]){
flag = true;
}
}
// if vertex not fixed or moved, re-index to front
//[v, v, v, v...., f, f, f...., m, m, m...,m]
if(!flag){
vertexNewIndices.push_back(i);
}
}
//re-index fixed verts
for(unsigned int j=0; j<fixVertices.size(); j++){
vertexNewIndices.push_back(fixVertices[j]);
}
//re-index move verts
for(unsigned int j=0; j<moveVertices.size(); j++){
vertexNewIndices.push_back(moveVertices[j]);
}
//these are the new indices for the fixed verts
vector<int> newfixIndices;
for(unsigned int i= vertexNewIndices.size() - (moveVertices.size() + fixVertices.size()); i<(vertexNewIndices.size()-moveVertices.size()); i++){
newfixIndices.push_back(i);
}
//new indices for the moving verts
vector<int> newMoveIndices;
for(unsigned int i= vertexNewIndices.size() - moveVertices.size(); i<vertexNewIndices.size(); i++){
newMoveIndices.push_back(i);
}
//cout << "NewMoveIndicesSize :: " << newMoveIndices.size() << endl;
VectorXd new_force;
new_force.resize(3*TV.rows());
reIndexTVandTT(vertexNewIndices, fixVertices.size(), moveVertices.size(), TV, TT, force, newTV, newTT, new_force);
igl::barycenter(newTV, newTT, B);
//Initialize Solid Mesh
M.initializeMesh(newTT, newTV, youngs, poissons);
if(moveVertices.size() != 0){
// cout<<"Move vertices "<<moveVertices.size()<<endl;
// cout<<"fix verts "<<fixVertices.size()<<endl;
binarySearchYoungs(newMoveIndices, newTV, newTT, fixVertices.size(), B);
// syntheticTests(newMoveIndices, newTV, newTT, fixVertices.size(), B);
}
integrator->initializeIntegrator(deltaT, M, newTV, newTT);
this->external_force = new_force;
integrator->fixVertices(newfixIndices);
int ignorePastIndex = newTV.rows() - newfixIndices.size();
staticSolveNewtonsForces(newTV, newTT, B, new_force, ignorePastIndex);
}else{
//cout << "Doing Other Stuffs" << endl;
igl::barycenter(TV, TT, B);
M.initializeMesh(TT, TV, youngs, poissons);
integrator->initializeIntegrator(deltaT, M, TV, TT);
this->external_force = force;
integrator->fixVertices(fixVertices);
}
return 1;
}
int main(int argc, char *argv[])
{
FEM *clip_;
clip_ = new GMSH("ClipCpp.inp");
clip_->assembleK();
clip_->sortK();
clip_->sortT();
clip_->computeInverseForceCondensedK();
clip_->computeDisplacementCondensedK();
DeformableObject *defClip_;
defClip_ = new DeformableObject(*clip_);
VectorXd eF (defClip_->getSizeF()); // applied forces
VectorXd uD (defClip_->getSizeD()); // applied displacements
eF.setZero();
uD.setZero();
std::cout << "size of eF = " << eF.size() << std::endl;
std::cout << "size of uD = " << uD.size() << std::endl;
defClip_->setDisplayMode(DeformableObject::REGULAR_RENDERING);
//defClip_->setDisplayMode(DeformableObject::VON_MISES_STRESS);
defClip_->setNormalsMode(DeformableObject::NORMAL_PER_TRIANGLE);
//defClip_->setNormalsMode(DeformableObject::NORMAL_PER_VERTEX);
double t_ = 0.00;
glfwInit();
glfwOpenWindow(DEFAULT_W,DEFAULT_H,0,0,0,0,24,0, GLFW_WINDOW);
GLfloat light0pos[] = { 0.0, 4.0, 6.0, 1.0 };
GLfloat light1pos[] = { 6.0, 4.0, 0.0, 1.0 };
GLfloat white[] = { 0.6, 0.6, 0.6, 1.0 };
glfwSetKeyCallback(keyboard);
glClearColor(0, 0, 0, 1.0);
glEnable(GL_DEPTH_TEST);
glEnable(GL_CULL_FACE);
glCullFace(GL_BACK);
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
glEnable(GL_LIGHT1);
glLightfv(GL_LIGHT0, GL_DIFFUSE, white);
glLightfv(GL_LIGHT0, GL_SPECULAR, white);
glLightfv(GL_LIGHT1, GL_DIFFUSE, white);
glLightfv(GL_LIGHT1, GL_SPECULAR, white);
glLightfv(GL_LIGHT0, GL_POSITION, light0pos);
glLightfv(GL_LIGHT1, GL_POSITION, light1pos);
std::cout << std::endl;
CvVideoWriter *videoWriter;
IplImage *image;
while (!quit_flag){
std::cout << "\r t = " << t_ << std::flush;
if (movie_flag && t_ == 0.){
videoWriter = cvCreateVideoWriter(
"clip.avi",
CV_FOURCC('D','I','V','X'),
20,
cvSize(DEFAULT_W, DEFAULT_H));
image = cvCreateImage(
cvSize(DEFAULT_W, DEFAULT_H),
IPL_DEPTH_8U, 3);
}
eF(0)=sin(M_PI*t_)/50000;
eF(6)=-sin(M_PI*t_)/50000;
// the three calls for the deformation
defClip_->setForce(eF);
defClip_->setDisplacement(uD);
struct timeval t1, t2;
gettimeofday(&t1, NULL);
defClip_->solveU();
gettimeofday(&t2, NULL);
std::cout << "solveU() : " << (t2.tv_sec - t1.tv_sec)*1000 + (t2.tv_usec - t1.tv_usec)/1000.0 << "[ms]" << std::endl;
defClip_->updateNodes();
if (start_flag || movie_flag) t_ += 0.05;
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(45,
(double)DEFAULT_W / (double)DEFAULT_H,
0.1, 100);
double radius = 0.2, eye[3];
eye[0] = radius*cos(UDangle)*cos(LRangle);
eye[1] = radius*cos(UDangle)*sin(LRangle);
eye[2] = radius*sin(UDangle);
gluLookAt(eye[0], eye[1], eye[2],
0,0,0,
0,0,1);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_MODELVIEW);
defClip_->draw();
if (movie_flag){
unsigned char rgb[DEFAULT_W*DEFAULT_H*3];
glReadBuffer(GL_BACK);
glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
for (int i=0; i<DEFAULT_H; i++){
glReadPixels(0,(DEFAULT_H-1-i),DEFAULT_W,
1,GL_RGB,GL_UNSIGNED_BYTE,
rgb + i*3*DEFAULT_W);
}
char *bgr = image->imageData;
for (int i=0; i<DEFAULT_W*DEFAULT_H; i++){
bgr[i*3 ] = rgb[i*3+2];
bgr[i*3+1] = rgb[i*3+1];
bgr[i*3+2] = rgb[i*3 ];
}
cvWriteFrame(videoWriter, image);
if (t_ > 10){
cvReleaseVideoWriter(&videoWriter);
cvReleaseImage(&image);
movie_flag = false;
}
}
glfwSwapBuffers();
usleep(10000);
}
std::cout << std::endl;
return 0;
}
void Mesh::elasticEnergy(const VectorXd &q,
const VectorXd &g,
double &energyB,
double &energyS,
VectorXd &gradq,
Eigen::SparseMatrix<double> &hessq,
Eigen::SparseMatrix<double> &gradggradq,
int derivativesRequested) const
{
assert(q.size() == numdofs());
assert(g.size() == numedges());
energyB = energyS = 0;
if(derivativesRequested & ElasticEnergy::DR_DQ)
{
gradq.resize(numdofs());
gradq.setZero();
if(derivativesRequested & ElasticEnergy::DR_HQ)
hessq.resize(numdofs(), numdofs());
}
if(derivativesRequested & ElasticEnergy::DR_DGDQ)
{
gradggradq.resize(numedges(), numdofs());
}
vector<Tr> Hqcoeffs;
vector<Tr> Hgcoeffs;
vector<Tr> dgdqcoeffs;
// bending energy
for(OMMesh::VertexIter vi = mesh_->vertices_begin(); vi != mesh_->vertices_end(); ++vi)
{
if(mesh_->is_boundary(vi.handle()))
continue;
vector<int> spokeidx;
vector<int> rightoppidx;
vector<int> nbidx;
for(OMMesh::VertexOHalfedgeIter voh = mesh_->voh_iter(vi.handle()); voh; ++voh)
{
OMMesh::HalfedgeHandle heh = voh.handle();
int eidx = mesh_->edge_handle(heh).idx();
spokeidx.push_back(eidx);
OMMesh::VertexOHalfedgeIter nextoh = voh;
++nextoh;
if(!nextoh)
nextoh = mesh_->voh_iter(vi.handle());
OMMesh::VertexHandle nextvert = mesh_->to_vertex_handle(nextoh.handle());
OMMesh::HalfedgeHandle opp = mesh_->next_halfedge_handle(heh);;
if(mesh_->to_vertex_handle(opp) != nextvert)
{
opp = mesh_->prev_halfedge_handle(mesh_->opposite_halfedge_handle(heh));
assert(mesh_->from_vertex_handle(opp) == nextvert);
}
int oidx = mesh_->edge_handle(opp).idx();
rightoppidx.push_back(oidx);
OMMesh::VertexHandle vh = mesh_->to_vertex_handle(heh);
nbidx.push_back(vh.idx());
}
int centidx = vi.handle().idx();
energyB += ElasticEnergy::bendingEnergy(q, g, centidx, nbidx, spokeidx, rightoppidx, gradq, Hqcoeffs, dgdqcoeffs, params_, derivativesRequested);
}
// Stretching energy
for(OMMesh::FaceIter it = mesh_->faces_begin(); it != mesh_->faces_end(); ++it)
{
int qidx[3];
int gidx[3];
int idx=0;
for(OMMesh::FaceHalfedgeIter fhi = mesh_->fh_iter(it.handle()); fhi; ++fhi)
{
assert(idx < 3);
OMMesh::HalfedgeHandle heh = fhi.handle();
OMMesh::EdgeHandle eh = mesh_->edge_handle(heh);
OMMesh::VertexHandle from = mesh_->from_vertex_handle(heh);
gidx[idx] = eh.idx();
qidx[(idx+1)%3] = from.idx();
idx++;
}
assert(idx == 3);
energyS += ElasticEnergy::stretchingEnergy(q, g, qidx, gidx, gradq, Hqcoeffs, dgdqcoeffs, params_, derivativesRequested);
}
if(derivativesRequested & ElasticEnergy::DR_HQ)
hessq.setFromTriplets(Hqcoeffs.begin(), Hqcoeffs.end());
if(derivativesRequested & ElasticEnergy::DR_DGDQ)
gradggradq.setFromTriplets(dgdqcoeffs.begin(), dgdqcoeffs.end());
}
double RationalSuperShape2D :: XiSquare7D(
const vector < Vector2d, aligned_allocator< Vector2d> > Data,
MatrixXd &alpha,
VectorXd &beta,
int functionused,
bool update) {
VectorXd dj; dj = VectorXd::Zero(7);
Matrix3d Tr,Rot, dTrdx0, dTrdy0;
//functions pointer
double (RationalSuperShape2D ::*pt2ConstMember)(const Vector2d P, vector<double> &Dffinal) = NULL;
switch (functionused){
case 1 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction1; }break;
case 2 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction2; }break;
case 3 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction3; }break;
default : pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction1;
}
vector<double> Df;
double x, y, tht, dthtdx, dthtdy,dthtdx0, dthtdy0, drdth,
ChiSquare(1e15),
f(0),
x0(Get_xoffset()),y0(Get_yoffset()),tht0(Get_thtoffset()),
dxdx0,dxdy0, dydx0,dydy0;
//clean memory
if(update) {
alpha.setZero();
beta.setZero();
}
//evaluate ChiSquare, components for the beta and matrix alpha
ChiSquare=0;
//First define inverse translation T-1
Tr.setZero();
Rot.setZero();
Tr << 1 , 0 , -x0 ,
0 , 1 , -y0 ,
0 , 0 , 1;
//derivatives of translation matrix are constant and can be computed once for all
dTrdx0 << 0 , 0 , -1 , 0 , 0 , 0 , 0 , 0 , 1;
dTrdy0 << 0 , 0 , 0 , 0 , 0 , -1 , 0 , 0 , 1;
//Then define inverse rotation, i.e. transposed rotation
Rot << cos(tht0) , sin(tht0) , 0 ,
-sin(tht0) , cos(tht0) , 0 ,
0 , 0 , 1;
for(size_t i=0; i<Data.size(); i++){
double DfDr;
//global inverse transform is T * R
Vector2d dum(Data[i]);
Vector3d dum2(dum[0],dum[1],1);
//apply inverse transform
Vector3d dum3 ( Rot * (Tr * dum2));
Vector2d P(dum3[0],dum3[1]); //2D point in canonical ref
//get partial derivatives for canonical point
Vector3d dPdx0 ( Rot * (dTrdx0 * dum2) );
Vector3d dPdy0 ( Rot * (dTrdy0 * dum2) );
//simplify notations
dxdx0 = dPdx0[0];
dxdy0 = dPdy0[0];
dydx0 = dPdx0[1];
dydy0 = dPdy0[1];
x = P[0]; y = P[1];
// avoid division by 0 ==> numerical stability
if (P.norm()<EPSILON) continue; // avoids division by zero
tht = atan2(y,x); if( tht < 0 ) tht+=2.*M_PI;
//theta = Arctan(Y/X)
dthtdx = -sin(tht) ;//-y / (x*x+y*y);
dthtdy = cos(tht); //x / (x*x+y*y);
//partial derivatives of theta regarding x offset, y offset, and angular offset
dthtdx0 = dthtdx * dxdx0 + dthtdy * dydx0;
dthtdy0 = dthtdx * dxdy0 + dthtdy * dydy0;
//avoid non differentiable cases
drdth = DrDtheta(tht);
f = (*this.*pt2ConstMember)(P, Df); // call to the implicit function
//
//compute elements beta[i][0] and alpha[i][j]
//
//==> requires partial derivatives!!
DfDr = Df[2]; //Df/Dr stored at index 2 in array Df during the call to ImplicitFunction1-2-3
// F1 = R-PL ==> DfDr = 1. ;
// F2 = 1-PL/R ==> DfDr = PL/R\B2 ;
// F3 = log ( R\B2/PSL) ==> DfDr = 2/R
dj.setZero();
//df/da = df/dr * dr/da
dj[0] = DfDr * DrDa(tht) ;
//df/db = df/dr * dr/db
dj[1] = DfDr * DrDb(tht) ;
//df/dn1 = df/dr * dr/dn1
dj[2] = DfDr * DrDn1(tht);
//df/dn2 = df/dr * dr/dn2
dj[3] = DfDr * DrDn2(tht);
//df/dn3 = df/dr * dr/dn3
dj[4] = DfDr * DrDn3(tht);
//df/dx0 = df/dr * dr/dtht *dtht/dx0
dj[5]= DfDr * drdth*dthtdx0;
//df/dy0 = df/dr * dr/dtht *dtht/dy0
dj[6]= DfDr * drdth*dthtdy0;
//squared summation
ChiSquare += f*f;
if(update){
for(int k=0; k<7; k++)
{
beta[k] -= f*dj[k];
}
//compute approximation of Hessian matrix
for(int k=0; k<7; k++)
for(int j=0; j<7; j++)
alpha(k,j) += dj[k]*dj[j];
}
}//for all vertices
return ChiSquare;
}
double RationalSuperShape2D :: XiSquare5D(
const vector < Vector2d, aligned_allocator< Vector2d> > Data,
MatrixXd &alpha,
VectorXd &beta,
int functionused,
bool update) {
//five dimensional optimization: a, b, n1, n2, n3 are optimized
VectorXd dj; dj = VectorXd::Zero(5);
Matrix3d Tr,Rot;
//functions pointer
double (RationalSuperShape2D ::*pt2ConstMember)(const Vector2d P, vector<double> &Dffinal) = NULL;
switch (functionused){
case 1 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction1; }break;
case 2 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction2; }break;
case 3 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction3; }break;
default : pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction1;
}
vector<double> Df;
double x, y, tht, ChiSquare(1e15), f(0),
x0(Get_xoffset()),y0(Get_yoffset()),tht0(Get_thtoffset());
//clean memory
if ( update ) {
alpha.setZero();
beta.setZero();
}
//evaluate ChiSquare, components for the beta and matrix alpha
ChiSquare=0;
//First define inverse translation T-1
Tr.setZero();
Rot.setZero();
Tr << 1 , 0 , -x0 ,
0 , 1 , -y0 ,
0 , 0 , 1;
//Then define inverse rotation, i.e. transposed rotation
Rot << cos(tht0) , sin(tht0) , 0 ,
-sin(tht0) , cos(tht0) , 0 ,
0 , 0 , 1;
//summation of squared potentials
for(size_t i=0; i<Data.size(); i++ ){
double DfDr;
//global inverse transform is T * R
Vector2d dum(Data[i]);
Vector3d dum2(dum[0],dum[1],1);
//apply inverse transform
Vector3d dum3 ( Rot * (Tr * dum2));
Vector2d P( dum3[0], dum3[1]); //2D point in canonical referential
//get partial derivatives for canonical point
x = P[0]; y = P[1];
// avoid division by 0 ==> numerical stability
if (P.norm()<EPSILON) continue; // avoids division by zero
tht = atan2(y,x); if( tht<0) tht+=2.*M_PI;
//theta = Arctan(Y/X)
//avoid non differentiable cases
f = (*this.*pt2ConstMember)(P, Df); // call to the implicit function
//
//compute elements beta[i][0] and alpha[i][j]
//
//==> requires partial derivatives!!
DfDr = Df[2]; //Df/Dr stored at index 2 in array Df during the call to ImplicitFunction1-2-3
// F1 = R-PL ==> DfDr = 1. ;
// F2 = 1-PL/R ==> DfDr = PL/R\B2 ;
// F3 = log ( R\B2/PSL) ==> DfDr = 2/R
dj.setZero();
//df/da = df/dr * dr/da
dj[0] = DfDr * DrDa(tht) ;
//df/db = df/dr * dr/db
dj[1] = DfDr * DrDb(tht) ;
//df/dn1 = df/dr * dr/dn1
dj[2] = DfDr * DrDn1(tht);
//df/dn2 = df/dr * dr/dn2
dj[3] = DfDr * DrDn2(tht);
//df/dn3 = df/dr * dr/dn3
dj[4] = DfDr * DrDn3(tht);
ChiSquare += f*f;
if(update){
//gradient beta
for(int k=0;k<5; k++)
{
beta[k] -= f * dj[k];
}
//compute approximation of Hessian matrix
for(int k=0; k<5; k++)
for(int j=0; j<5; j++)
alpha(k,j) += dj[k]*dj[j];
}
}//for all vertices
return ChiSquare;
}
// barebones version of the lasso for fixed lambda
// Eigen library is used for linear algebra
// x .............. predictor matrix
// y .............. response
// lambda ......... penalty parameter
// useSubset ...... logical indicating whether lasso should be computed on a
// subset
// subset ......... indices of subset on which lasso should be computed
// normalize ...... logical indicating whether predictors should be normalized
// useIntercept ... logical indicating whether intercept should be included
// eps ............ small numerical value (effective zero)
// useGram ........ logical indicating whether Gram matrix should be computed
// in advance
// useCrit ........ logical indicating whether to compute objective function
void fastLasso(const MatrixXd& x, const VectorXd& y, const double& lambda,
const bool& useSubset, const VectorXi& subset, const bool& normalize,
const bool& useIntercept, const double& eps, const bool& useGram,
const bool& useCrit,
// intercept, coefficients, residuals and objective function are returned
// through the following parameters
double& intercept, VectorXd& beta, VectorXd& residuals, double& crit) {
// data initializations
int n, p = x.cols();
MatrixXd xs;
VectorXd ys;
if(useSubset) {
n = subset.size();
xs.resize(n, p);
ys.resize(n);
int s;
for(int i = 0; i < n; i++) {
s = subset(i);
xs.row(i) = x.row(s);
ys(i) = y(s);
}
} else {
n = x.rows();
xs = x; // does this copy memory?
ys = y; // does this copy memory?
}
double rescaledLambda = n * lambda / 2;
// center data and store means
RowVectorXd meanX;
double meanY;
if(useIntercept) {
meanX = xs.colwise().mean(); // columnwise means of predictors
xs.rowwise() -= meanX; // sweep out columnwise means
meanY = ys.mean(); // mean of response
for(int i = 0; i < n; i++) {
ys(i) -= meanY; // sweep out mean
}
} else {
meanY = 0; // just to avoid warning, this is never used
// intercept = 0; // zero intercept
}
// some initializations
VectorXi inactive(p); // inactive predictors
int m = 0; // number of inactive predictors
VectorXi ignores; // indicates variables to be ignored
int s = 0; // number of ignored variables
// normalize predictors and store norms
RowVectorXd normX;
if(normalize) {
normX = xs.colwise().norm(); // columnwise norms
double epsNorm = eps * sqrt(n); // R package 'lars' uses n, not n-1
for(int j = 0; j < p; j++) {
if(normX(j) < epsNorm) {
// variance is too small: ignore variable
ignores.append(j, s);
s++;
// set norm to tolerance to avoid numerical problems
normX(j) = epsNorm;
} else {
inactive(m) = j; // add variable to inactive set
m++; // increase number of inactive variables
}
xs.col(j) /= normX(j); // sweep out norm
}
// resize inactive set and update number of variables if necessary
if(m < p) {
inactive.conservativeResize(m);
p = m;
}
} else {
for(int j = 0; j < p; j++) inactive(j) = j; // add variable to inactive set
m = p;
}
// compute Gram matrix if requested (saves time if number of variables is
// not too large)
MatrixXd Gram;
if(useGram) {
Gram.noalias() = xs.transpose() * xs;
}
// further initializations for iterative steps
RowVectorXd corY;
corY.noalias() = ys.transpose() * xs; // current correlations
beta.resize(p+s); // final coefficients
// compute lasso solution
if(p == 1) {
// special case of only one variable (with sufficiently large norm)
int j = inactive(0);
// set maximum step size in the direction of that variable
double maxStep = corY(j);
if(maxStep < 0) maxStep = -maxStep; // absolute value
// compute coefficients for least squares solution
VectorXd betaLS = xs.col(j).householderQr().solve(ys);
// compute lasso coefficients
beta.setZero();
if(rescaledLambda < maxStep) {
// interpolate towards least squares solution
beta(j) = (maxStep - rescaledLambda) * betaLS(0) / maxStep;
}
} else {
// further initializations for iterative steps
VectorXi active; // active predictors
int k = 0; // number of active predictors
// previous and current regression coefficients
VectorXd previousBeta = VectorXd::Zero(p+s), currentBeta = VectorXd::Zero(p+s);
// previous and current penalty parameter
double previousLambda = 0, currentLambda = 0;
// indicates variables to be dropped
VectorXi drops;
// keep track of sign of correlations for the active variables
// (double precision is necessary for solve())
VectorXd signs;
// Cholesky L of Gram matrix of active variables
MatrixXd L;
int rank = 0; // rank of Cholesky L
// maximum number of variables to be sequenced
int maxActive = findMaxActive(n, p, useIntercept);
// modified LARS algorithm for lasso solution
while(k < maxActive) {
// extract current correlations of inactive variables
VectorXd corInactiveY(m);
for(int j = 0; j < m; j++) {
corInactiveY(j) = corY(inactive(j));
}
// compute absolute values of correlations and find maximum
VectorXd absCorInactiveY = corInactiveY.cwiseAbs();
double maxCor = absCorInactiveY.maxCoeff();
// update current lambda
if(k == 0) { // no active variables
previousLambda = maxCor;
} else {
previousLambda = currentLambda;
}
currentLambda = maxCor;
if(currentLambda <= rescaledLambda) break;
if(drops.size() == 0) {
// new active variables
VectorXi newActive = findNewActive(absCorInactiveY, maxCor - eps);
// do calculations for new active variables
for(int j = 0; j < newActive.size(); j++) {
// update Cholesky L of Gram matrix of active variables
// TODO: put this into void function
int newJ = inactive(newActive(j));
VectorXd xNewJ;
double newX;
if(useGram) {
newX = Gram(newJ, newJ);
} else {
xNewJ = xs.col(newJ);
newX = xNewJ.squaredNorm();
}
double normNewX = sqrt(newX);
if(k == 0) { // no active variables, L is empty
L.resize(1,1);
L(0, 0) = normNewX;
rank = 1;
} else {
VectorXd oldX(k);
if(useGram) {
for(int j = 0; j < k; j++) {
oldX(j) = Gram(active(j), newJ);
}
} else {
for(int j = 0; j < k; j++) {
oldX(j) = xNewJ.dot(xs.col(active(j)));
}
}
VectorXd l = L.triangularView<Lower>().solve(oldX);
double lkk = newX - l.squaredNorm();
// check if L is machine singular
if(lkk > eps) {
// no singularity: update Cholesky L
lkk = sqrt(lkk);
rank++;
// add new row and column to Cholesky L
// this is a little trick: sometimes we need
// lower triangular matrix, sometimes upper
// hence we define quadratic matrix and use
// triangularView() to interpret matrix the
// correct way
L.conservativeResize(k+1, k+1);
for(int j = 0; j < k; j++) {
L(k, j) = l(j);
L(j, k) = l(j);
}
L(k,k) = lkk;
}
}
// add new variable to active set or drop it for good
// in case of singularity
if(rank == k) {
// singularity: drop variable for good
ignores.append(newJ, s);
s++; // increase number of ignored variables
p--; // decrease number of variables
if(p < maxActive) {
// adjust maximum number of active variables
maxActive = p;
}
} else {
// no singularity: add variable to active set
active.append(newJ, k);
// keep track of sign of correlation for new active variable
signs.append(sign(corY(newJ)), k);
k++; // increase number of active variables
}
}
// remove new active or ignored variables from inactive variables
// and corresponding vector of current correlations
inactive.remove(newActive);
corInactiveY.remove(newActive);
m = inactive.size(); // update number of inactive variables
}
// prepare for computation of step size
// here double precision of signs is necessary
VectorXd b = L.triangularView<Lower>().solve(signs);
VectorXd G = L.triangularView<Upper>().solve(b);
// correlations of active variables with equiangular vector
double corActiveU = 1/sqrt(G.dot(signs));
// coefficients of active variables in linear combination forming the
// equiangular vector
VectorXd w = G * corActiveU; // note that this has the right signs
// equiangular vector
VectorXd u;
if(!useGram) {
// we only need equiangular vector if we don't use the precomputed
// Gram matrix, otherwise we can compute the correlations directly
// from the Gram matrix
u = VectorXd::Zero(n);
for(int i = 0; i < n; i++) {
for(int j = 0; j < k; j++) {
u(i) += xs(i, active(j)) * w(j);
}
}
}
// compute step size in equiangular direction
double step;
if(k < maxActive) {
// correlations of inactive variables with equiangular vector
VectorXd corInactiveU(m);
if(useGram) {
for(int j = 0; j < m; j++) {
corInactiveU(j) = 0;
for(int i = 0; i < k; i++) {
corInactiveU(j) += w(i) * Gram(active(i), inactive(j));
}
}
} else {
for(int j = 0; j < m; j++) {
corInactiveU(j) = u.dot(xs.col(inactive(j)));
}
}
// compute step size in the direction of the equiangular vector
step = findStep(maxCor, corInactiveY, corActiveU, corInactiveU, eps);
} else {
// last step: take maximum possible step
step = maxCor/corActiveU;
}
// adjust step size if any sign changes and drop corresponding variables
drops = findDrops(currentBeta, active, w, eps, step);
// update current regression coefficients
previousBeta = currentBeta;
for(int j = 0; j < k; j++) {
currentBeta(active(j)) += step * w(j);
}
// update current correlations
if(useGram) {
// we also need to do this for active variables, since they may be
// dropped at a later stage
// TODO: computing a vector step * w in advance may save some computation time
for(int j = 0; j < Gram.cols(); j++) {
for(int i = 0; i < k; i++) {
corY(j) -= step * w(i) * Gram(active(i), j);
}
}
} else {
ys -= step * u; // take step in equiangular direction
corY.noalias() = ys.transpose() * xs; // update correlations
}
// drop variables if necessary
if(drops.size() > 0) {
// downdate Cholesky L
// TODO: put this into void function
for(int j = drops.size()-1; j >= 0; j--) {
// variables need to be dropped in descending order
int drop = drops(j); // index with respect to active set
// modify upper triangular part as in R package 'lars'
// simply deleting columns is not enough, other computations
// necessary but complicated due to Fortran code
L.removeCol(drop);
VectorXd z = VectorXd::Constant(k, 1, 1);
k--; // decrease number of active variables
for(int i = drop; i < k; i++) {
double a = L(i,i), b = L(i+1,i);
if(b != 0.0) {
// compute the rotation
double tau, s, c;
if(std::abs(b) > std::abs(a)) {
tau = -a/b;
s = 1.0/sqrt(1.0+tau*tau);
c = s * tau;
} else {
tau = -b/a;
c = 1.0/sqrt(1.0+tau*tau);
s = c * tau;
}
// update 'L' and 'z';
L(i,i) = c*a - s*b;
for(int j = i+1; j < k; j++) {
a = L(i,j);
b = L(i+1,j);
L(i,j) = c*a - s*b;
L(i+1,j) = s*a + c*b;
}
a = z(i);
b = z(i+1);
z(i) = c*a - s*b;
z(i+1) = s*a + c*b;
}
}
// TODO: removing all rows together may save some computation time
L.conservativeResize(k, NoChange);
rank--;
}
// mirror lower triangular part
for(int j = 0; j < k; j++) {
for(int i = j+1; i < k; i++) {
L(i,j) = L(j,i);
}
}
// add dropped variables to inactive set and make sure
// coefficients are 0
inactive.conservativeResize(m + drops.size());
for(int j = 0; j < drops.size(); j++) {
int newInactive = active(drops(j));
inactive(m + j) = newInactive;
currentBeta(newInactive) = 0; // make sure coefficient is 0
}
m = inactive.size(); // update number of inactive variables
// drop variables from active set and sign vector
// number of active variables is already updated above
active.remove(drops);
signs.remove(drops);
}
}
// interpolate coefficients for given lambda
if(k == 0) {
// lambda larger than largest lambda from steps of LARS algorithm
beta.setZero();
} else {
// penalty parameter within two steps
if(k == maxActive) {
// current coefficients are the least squares solution (in the
// high-dimensional case, as far along the solution path as possible)
// current and previous values of the penalty parameter need to be
// reset for interpolation
previousLambda = currentLambda;
currentLambda = 0;
}
// interpolate coefficients
beta = ((rescaledLambda - currentLambda) * previousBeta +
(previousLambda - rescaledLambda) * currentBeta) /
(previousLambda - currentLambda);
}
}
// transform coefficients back
VectorXd normedBeta;
if(normalize) {
if(useCrit) normedBeta = beta;
for(int j = 0; j < p; j++) beta(j) /= normX(j);
}
if(useIntercept) intercept = meanY - beta.dot(meanX);
// compute residuals for all observations
n = x.rows();
residuals = y - x * beta;
if(useIntercept) {
for(int i = 0; i < n; i++) residuals(i) -= intercept;
}
// compute value of objective function on the subset
if(useCrit && useSubset) {
if(normalize) crit = objective(normedBeta, residuals, subset, lambda);
else crit = objective(beta, residuals, subset, lambda);
}
}
int main(int argc, char *argv[])
{
FEM *stopper_;
struct timeval t1, t2;
stopper_ = new GMSH("09_part_g_free.inp");
gettimeofday(&t1, NULL);
stopper_->assembleK();
gettimeofday(&t2, NULL);
printf("assembleK() : %6.3f[ms]\n", (t2.tv_sec - t1.tv_sec)*1000 + (t2.tv_usec - t1.tv_usec)/1000.0);
gettimeofday(&t1, NULL);
stopper_->sortK();
gettimeofday(&t2, NULL);
printf("sortK() : %6.3f[ms]\n", (t2.tv_sec - t1.tv_sec)*1000 + (t2.tv_usec - t1.tv_usec)/1000.0);
std::cout << "after sortK():" << memused() << "[MB]" << std::endl;
stopper_->sortT();
std::cout << "after sortT():" << memused() << "[MB]" << std::endl;
gettimeofday(&t1, NULL);
stopper_->computeInverseForceCondensedK();
std::cout << "after computing cK:" << memused() << "[MB]" << std::endl;
gettimeofday(&t2, NULL);
printf("computeInverseForceCondensedK() : %6.3f[ms]\n", (t2.tv_sec - t1.tv_sec)*1000 + (t2.tv_usec - t1.tv_usec)/1000.0);
stopper_->computeDisplacementCondensedK();
std::cout << "before creating a DeformableObject:" << memused() << "[MB]" << std::endl;
DeformableObject *defstopper_;
defstopper_ = new DeformableObject(*stopper_);
VectorXd eF (defstopper_->getSizeF()); // applied forces
VectorXd uD (defstopper_->getSizeD()); // applied displacements
eF.setZero();
uD.setZero();
std::cout << "size of eF = " << eF.size() << std::endl;
std::cout << "size of uD = " << uD.size() << std::endl;
defstopper_->setDisplayMode(DeformableObject::VON_MISES_STRESS);
defstopper_->setNormalsMode(DeformableObject::NORMAL_PER_TRIANGLE);
//defstopper_->setNormalsMode(DeformableObject::NORMAL_PER_VERTEX);
double t_ = 0.00;
glfwInit();
glfwOpenWindow(DEFAULT_W,DEFAULT_H,0,0,0,0,24,0, GLFW_WINDOW);
GLfloat light0pos[] = { 0.0, 4.0, 6.0, 1.0 };
GLfloat light1pos[] = { 6.0, 4.0, 0.0, 1.0 };
GLfloat white[] = { 0.6, 0.6, 0.6, 1.0 };
glfwSetKeyCallback(keyboard);
glClearColor(0, 0, 0, 1.0);
glEnable(GL_DEPTH_TEST);
glEnable(GL_CULL_FACE);
glCullFace(GL_BACK);
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
glEnable(GL_LIGHT1);
glLightfv(GL_LIGHT0, GL_DIFFUSE, white);
glLightfv(GL_LIGHT0, GL_SPECULAR, white);
glLightfv(GL_LIGHT1, GL_DIFFUSE, white);
glLightfv(GL_LIGHT1, GL_SPECULAR, white);
glLightfv(GL_LIGHT0, GL_POSITION, light0pos);
glLightfv(GL_LIGHT1, GL_POSITION, light1pos);
std::cout << std::endl;
CvVideoWriter *videoWriter;
IplImage *image;
double LRangle=M_PI/4;
double UDangle=M_PI/4;
int prevState, prevX, prevY;
prevState = glfwGetMouseButton(GLFW_MOUSE_BUTTON_LEFT);
glfwGetMousePos(&prevX, &prevY);
while (!quit_flag){
std::cout << "\r t = " << t_ << std::flush;
if (movie_flag && t_ == 0.){
videoWriter = cvCreateVideoWriter(
"04_stopper_rotation.avi",
CV_FOURCC('D','I','V','X'),
20,
cvSize(DEFAULT_W, DEFAULT_H));
image = cvCreateImage(
cvSize(DEFAULT_W, DEFAULT_H),
IPL_DEPTH_8U, 3);
}
if (edge_flag){
defstopper_->setDisplayMode(DeformableObject::WIRE_FRAME);
}else{
defstopper_->setDisplayMode(DeformableObject::REGULAR_RENDERING);
}
eF(1500)=sin(M_PI*t_)*0.1;
// the three calls for the deformation
defstopper_->setForce(eF);
defstopper_->setDisplacement(uD);
struct timeval t1, t2;
gettimeofday(&t1, NULL);
defstopper_->solveU();
gettimeofday(&t2, NULL);
//printf("\n solveU() : %6.3f[ms]", (t2.tv_sec - t1.tv_sec)*1000 + (t2.tv_usec - t1.tv_usec)/1000.0);
defstopper_->updateNodes();
if (start_flag || movie_flag) t_ += 0.05;
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(45,
(double)DEFAULT_W / (double)DEFAULT_H,
1, 1000);
int x,y,state;
glfwGetMousePos(&x, &y);
state = glfwGetMouseButton(GLFW_MOUSE_BUTTON_LEFT);
if (state == GLFW_PRESS && prevState == GLFW_PRESS){
LRangle -= (x - prevX)*0.01;
UDangle += (y - prevY)*0.01;
if (UDangle < -M_PI/2) UDangle = -M_PI/2;
if (UDangle > M_PI/2) UDangle = M_PI/2;
}
prevState = state;
prevX = x;
prevY = y;
double radius = 100, eye[3];
double center[3] = {-100.5, 0, 0};
eye[1] = radius*cos(UDangle)*cos(LRangle)+center[1];
eye[2] = radius*cos(UDangle)*sin(LRangle)+center[2];
eye[0] = radius*sin(UDangle)+center[0];
gluLookAt(eye[0], eye[1], eye[2],
center[0], center[1], center[2],
1,0,0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_MODELVIEW);
defstopper_->draw();
if (movie_flag){
unsigned char rgb[DEFAULT_W*DEFAULT_H*3];
glReadBuffer(GL_BACK);
glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
for (int i=0; i<DEFAULT_H; i++){
glReadPixels(0,(DEFAULT_H-1-i),DEFAULT_W,
1,GL_RGB,GL_UNSIGNED_BYTE,
rgb + i*3*DEFAULT_W);
}
char *bgr = image->imageData;
for (int i=0; i<DEFAULT_W*DEFAULT_H; i++){
bgr[i*3 ] = rgb[i*3+2];
bgr[i*3+1] = rgb[i*3+1];
bgr[i*3+2] = rgb[i*3 ];
}
cvWriteFrame(videoWriter, image);
if (t_ > 10){
cvReleaseVideoWriter(&videoWriter);
cvReleaseImage(&image);
movie_flag = false;
}
}
glfwSwapBuffers();
usleep(10000);
}
std::cout << std::endl;
return 0;
}
slep_result_t slep_mc_plain_lr(
CDotFeatures* features,
CMulticlassLabels* labels,
float64_t z,
const slep_options& options)
{
int i,j;
// obtain problem parameters
int n_feats = features->get_dim_feature_space();
int n_vecs = features->get_num_vectors();
int n_classes = labels->get_num_classes();
// labels vector containing values in range (0 .. n_classes)
SGVector<float64_t> labels_vector = labels->get_labels();
// initialize matrices and vectors to be used
// weight vector
MatrixXd w = MatrixXd::Zero(n_feats, n_classes);
// intercepts (biases)
VectorXd c = VectorXd::Zero(n_classes);
if (options.last_result)
{
SGMatrix<float64_t> last_w = options.last_result->w;
SGVector<float64_t> last_c = options.last_result->c;
for (i=0; i<n_classes; i++)
{
c[i] = last_c[i];
for (j=0; j<n_feats; j++)
w(j,i) = last_w(j,i);
}
}
// iterative process matrices and vectors
MatrixXd wp = w, wwp = MatrixXd::Zero(n_feats, n_classes);
VectorXd cp = c, ccp = VectorXd::Zero(n_classes);
// search point weight vector
MatrixXd search_w = MatrixXd::Zero(n_feats, n_classes);
// search point intercepts
VectorXd search_c = VectorXd::Zero(n_classes);
// dot products
MatrixXd Aw = MatrixXd::Zero(n_vecs, n_classes);
for (j=0; j<n_classes; j++)
features->dense_dot_range(Aw.col(j).data(), 0, n_vecs, NULL, w.col(j).data(), n_feats, 0.0);
MatrixXd As = MatrixXd::Zero(n_vecs, n_classes);
MatrixXd Awp = MatrixXd::Zero(n_vecs, n_classes);
// gradients
MatrixXd g = MatrixXd::Zero(n_feats, n_classes);
VectorXd gc = VectorXd::Zero(n_classes);
// projection
MatrixXd v = MatrixXd::Zero(n_feats, n_classes);
// Lipschitz continuous gradient parameter for line search
double L = 1.0/(n_vecs*n_classes);
// coefficients for search point computation
double alphap = 0, alpha = 1;
// lambda regularization parameter
double lambda = z;
// objective values
double objective = 0.0;
double objective_p = 0.0;
int iter = 0;
bool done = false;
CTime time;
//internal::set_is_malloc_allowed(false);
while ((!done) && (iter<options.max_iter) && (!CSignal::cancel_computations()))
{
double beta = (alphap-1)/alpha;
// compute search points
search_w = w + beta*wwp;
search_c = c + beta*ccp;
// update dot products with search point
As = Aw + beta*(Aw-Awp);
// compute objective and gradient at search point
double fun_s = 0;
g.setZero();
gc.setZero();
// for each vector
for (i=0; i<n_vecs; i++)
{
// class of current vector
int vec_class = labels_vector[i];
// for each class
for (j=0; j<n_classes; j++)
{
// compute logistic loss
double aa = ((vec_class == j) ? -1.0 : 1.0)*(As(i,j) + search_c(j));
double bb = aa > 0.0 ? aa : 0.0;
// avoid underflow via log-sum-exp trick
fun_s += CMath::log(CMath::exp(-bb) + CMath::exp(aa-bb)) + bb;
double prob = 1.0/(1+CMath::exp(aa));
double b = ((vec_class == j) ? -1.0 : 1.0)*(1-prob);///(n_vecs*n_classes);
// update gradient of intercepts
gc[j] += b;
// update gradient of weight vectors
features->add_to_dense_vec(b, i, g.col(j).data(), n_feats);
}
}
//fun_s /= (n_vecs*n_classes);
wp = w;
Awp = Aw;
cp = c;
int inner_iter = 0;
double fun_x = 0;
// line search process
while (inner_iter<5000)
{
// compute line search point
v = search_w - g/L;
c = search_c - gc/L;
// compute projection of gradient
eppMatrix(w.data(),v.data(),n_feats,n_classes,lambda/L,options.q);
v = w - search_w;
// update dot products
for (j=0; j<n_classes; j++)
features->dense_dot_range(Aw.col(j).data(), 0, n_vecs, NULL, w.col(j).data(), n_feats, 0.0);
// compute objective at search point
fun_x = 0;
for (i=0; i<n_vecs; i++)
{
int vec_class = labels_vector[i];
for (j=0; j<n_classes; j++)
{
double aa = ((vec_class == j) ? -1.0 : 1.0)*(Aw(i,j) + c(j));
double bb = aa > 0.0 ? aa : 0.0;
fun_x += CMath::log(CMath::exp(-bb) + CMath::exp(aa-bb)) + bb;
}
}
//fun_x /= (n_vecs*n_classes);
// check for termination of line search
double r_sum = (v.squaredNorm() + (c-search_c).squaredNorm())/2;
double l_sum = fun_x - fun_s - v.cwiseProduct(g).sum() - (c-search_c).dot(gc);
// stop if projected gradient is less than 1e-20
if (r_sum <= 1e-20)
{
SG_SINFO("Gradient step makes little improvement (%f)\n",r_sum)
done = true;
break;
}
if (l_sum <= r_sum*L)
break;
else
L = CMath::max(2*L, l_sum/r_sum);
inner_iter++;
}
// update alpha coefficients
alphap = alpha;
alpha = (1+CMath::sqrt(4*alpha*alpha+1))/2;
// update wwp and ccp
wwp = w - wp;
ccp = c - cp;
// update objectives
objective_p = objective;
objective = fun_x;
// regularize objective with tree norm
double L1q_norm = 0.0;
for (int m=0; m<n_classes; m++)
L1q_norm += w.col(m).norm();
objective += lambda*L1q_norm;
//cout << "Objective = " << objective << endl;
// check for termination of whole process
if ((CMath::abs(objective - objective_p) < options.tolerance*CMath::abs(objective_p)) && (iter>2))
{
SG_SINFO("Objective changes less than tolerance\n")
done = true;
}
iter++;
}
SG_SINFO("%d iterations passed, objective = %f\n",iter,objective)
//internal::set_is_malloc_allowed(true);
// output computed weight vectors and intercepts
SGMatrix<float64_t> r_w(n_feats,n_classes);
for (j=0; j<n_classes; j++)
{
for (i=0; i<n_feats; i++)
r_w(i,j) = w(i,j);
}
//r_w.display_matrix();
SGVector<float64_t> r_c(n_classes);
for (j=0; j<n_classes; j++)
r_c[j] = c[j];
return slep_result_t(r_w, r_c);
};
