int main(int argc, const char *argv[]) { float x1, y1, x2, y2; while (scanf("%f %f %f %f", &x1, &y1, &x2, &y2)) { printf("%.2f\n", sqrt(sqare(x1 - x2) + sqare(y1 - y2))); } return 0; }
LaplacianOperator::LaplacianOperator( ObjEntity* mesh,MeshDrawerImpl* drawer ):mState(STATE_CHOOSING_STATIC),OperatorImpl(mesh,drawer) { mVerticesController = new VerticesController(mesh,drawer); assert(mesh != nullptr); clock_t clock_start; clock_start = clock(); mMeshVertexCount = mesh->getVertexCount(); initAdjacentMatrix(); // printMatrix(adjacentMatrix,"adjacentMatrix"); // printMatrix(degreeMatrix,"degreeMatrix"); Eigen::SparseMatrix<double> laplacianOperator; computeLaplacianOperator(laplacianOperator); #ifdef MY_DEBUG ofstream out("debug.txt",ios::app); out << "laplacianOperator:"; for (Eigen::SparseMatrix<double>::InnerIterator it(laplacianOperator,283); it; it++) { out << it.row() << ' '; } out << endl; #endif // MY_DEBUG //printMatrix(laplacianOperator,"laplacianOperator"); Eigen::Matrix<double,Eigen::Dynamic,3> verInput(mMeshVertexCount,3); for (int i = 0; i < mMeshVertexCount; i++) { Vertex* v = mesh->m_vertexList->at(i); verInput(i,0) = v->x; verInput(i,1) = v->y; verInput(i,2) = v->z; } Eigen::Matrix<double,Eigen::Dynamic,3> deltaInput = laplacianOperator * verInput; #ifdef MY_DEBUG out << "verInput 283:" << verInput(283,0) << ' ' << verInput(283,1) << ' ' << verInput(283,2) << endl; out << "deltaInput 283:" << deltaInput(283,0) << ' ' << deltaInput(283,1) << ' ' << deltaInput(283,2) << endl; #endif // MY_DEBUG //printMatrix(deltaInput,"deltaInput"); Eigen::SparseMatrix<double> laplacianOperator3D(6*mMeshVertexCount,3*mMeshVertexCount); laplacianOperator3D.reserve(Eigen::VectorXi::Constant(3*mMeshVertexCount,40)); //Eigen::MatrixXd laplacianOperator(laplacianOperator); vector<Vertex*>* meshVertex = mesh->m_vertexList; /* operate laplacianOperator3D */ for (int i = 0; i < mMeshVertexCount; i++) { //printf("%d ",i); /* Generate Matrix A*/ Vertex* v = meshVertex->at(i); vector<Vertex*> vertexInA; vertexInA.push_back(v); vector<int> adjIndex; getAdjVertex(i,adjIndex); for (int j = 0; j < adjIndex.size(); j++) { vertexInA.push_back(meshVertex->at(adjIndex.at(j))); } Eigen::Matrix<double,Eigen::Dynamic,7> matrixA; matrixA.resize(vertexInA.size()*3,7); for (int j = 0; j < vertexInA.size(); j++) { Eigen::Matrix<double,3,7> m; constructMatrix(vertexInA.at(j),m); int r = matrixA.rows(); matrixA.middleRows<3>(j*3) = m; // printMatrix(m,"m"); } Eigen::Matrix<double,7,Eigen::Dynamic> pinvA; /* 当该顶点是孤立的的时候,transposeA*matrixA可能是奇异的,因此pinv就会出现-1.#IND*/ if(adjIndex.size() == 0) { pinvA.resize(7,3); pinvA.setZero(); } else { Eigen::Matrix<double,7,Eigen::Dynamic> transposeA = matrixA.transpose(); pinvA = (transposeA * matrixA).inverse()*transposeA; } #ifdef MY_DEBUG cout << "MatrixA" << endl << matrixA << endl << endl << "PinvA:" << endl; cout << pinvA << endl; ofstream sqare("transposeAmatrixA.txt"); sqare << (transposeA * matrixA) << endl << endl; sqare << (transposeA * matrixA).inverse() << endl; sqare.close(); #endif // MY_DEBUG Eigen::Matrix<double,1,Eigen::Dynamic> s = pinvA.row(0); Eigen::Matrix<double,1,Eigen::Dynamic> h1 = pinvA.row(1); Eigen::Matrix<double,1,Eigen::Dynamic> h2 = pinvA.row(2); Eigen::Matrix<double,1,Eigen::Dynamic> h3 = pinvA.row(3); Eigen::Matrix<double,1,Eigen::Dynamic> TDeltaX = s*deltaInput(i,0)-h3*deltaInput(i,1)+h2*deltaInput(i,2); Eigen::Matrix<double,1,Eigen::Dynamic> TDeltaY = h3*deltaInput(i,0)+s*deltaInput(i,1)-h1*deltaInput(i,2); Eigen::Matrix<double,1,Eigen::Dynamic> TDeltaZ = -h2*deltaInput(i,0)-h1*deltaInput(i,1)+s*deltaInput(i,2); Eigen::Matrix<double,3,Eigen::Dynamic> TDelta; TDelta.resize(3,TDeltaX.cols()); TDelta.row(0) = TDeltaX; TDelta.row(1) = TDeltaY; TDelta.row(2) = TDeltaZ; for (int j = 0; j < 3; j++) { int opRow = j*mMeshVertexCount+i; laplacianOperator3D.insert(opRow,i) = j == 0 ? -TDelta(j,0) + laplacianOperator.coeff(i,i) : -TDelta(j,0); laplacianOperator3D.insert(opRow,i+mMeshVertexCount) = j == 1 ? -TDelta(j,1) + laplacianOperator.coeff(i,i) : -TDelta(j,1); laplacianOperator3D.insert(opRow,i+2*mMeshVertexCount) = j == 2 ? -TDelta(j,2) + laplacianOperator.coeff(i,i) : -TDelta(j,2); int curTDeltaCol = 3; for (int a = 0; a < adjIndex.size(); a++) { int adj = adjIndex.at(a); laplacianOperator3D.insert(opRow,adj) = j == 0 ? -TDelta(j,curTDeltaCol++) + laplacianOperator.coeff(i,adj) : -TDelta(j,curTDeltaCol++); laplacianOperator3D.insert(opRow,adj+mMeshVertexCount) = j == 1 ? -TDelta(j,curTDeltaCol++) + laplacianOperator.coeff(i,adj) : -TDelta(j,curTDeltaCol++); laplacianOperator3D.insert(opRow,adj+2*mMeshVertexCount) = j == 2 ? -TDelta(j,curTDeltaCol++) + laplacianOperator.coeff(i,adj) : -TDelta(j,curTDeltaCol++); } } } #ifdef MY_DEBUG ofstream outL3d("laplacianOperator3D.txt"); outL3d << laplacianOperator3D << endl; outL3d.close(); #endif int mMeshVertexCount_X3 = 3*mMeshVertexCount; Eigen::SparseMatrix<double>& A_prime = laplacianOperator3D; // printMatrix(laplacianOperator3D,"laplacianOperator3D"); // printMatrix(A_prime,"A_prime"); int offset = 0; for(int j = 0; j < mMeshVertexCount_X3; j+=3) { A_prime.insert(mMeshVertexCount_X3+j,offset) = 1; A_prime.insert(mMeshVertexCount_X3+j+1,offset+mMeshVertexCount) = 1; A_prime.insert(mMeshVertexCount_X3+j+2,offset+2*mMeshVertexCount) = 1; offset++; } Eigen::VectorXd b(mMeshVertexCount_X3*2); b.setZero(); int ret; defoMesh = ObjUtility::createObjEntity("defo.obj",ret); vector<Vertex*>* defoVertex = defoMesh->m_vertexList; int defoSize = defoVertex->size(); int nodefoSize = mMeshVertexCount; for (int j = 0; j < mMeshVertexCount; j++) { b(mMeshVertexCount_X3+3*j) = defoVertex->at(j)->x; b(mMeshVertexCount_X3+3*j+1) = defoVertex->at(j)->y; b(mMeshVertexCount_X3+3*j+2) = defoVertex->at(j)->z; } A_prime.makeCompressed(); Eigen::SparseMatrix<double> A_prime_T = Eigen::SparseMatrix<double>(A_prime.transpose()); Eigen::VectorXd A_prime_T_b = A_prime_T*b; Eigen::SparseMatrix<double> sym = A_prime_T*A_prime; long t = (clock()-clock_start); printf("constructTime:%d\n",t); #ifdef MY_DEBUG ofstream outSym("sym.txt"); outSym << sym << endl; outSym.close(); ofstream outAprimeT("AprimeT.txt"); outAprimeT << A_prime_T << endl; outAprimeT.close(); ofstream outAprime("A_prime.txt"); outAprime << A_prime << endl; outAprime.close(); #endif #ifdef MY_DEBUG Eigen::EigenSolver<Eigen::MatrixXd > es(sym); ofstream outEigen("eigenvalues.txt",ios::app); outEigen << es.eigenvalues() << endl; outEigen.close(); #endif // MY_DEBUG clock_start = clock(); //Eigen::SimplicialLLT<Eigen::SparseMatrix<double> > solver; Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > solver; solver.compute(sym); t = (clock()-clock_start); printf("factorizationTime:%d\n",t); clock_start = clock(); // ofstream outLU("LU.txt",ios::app); // outLU << "L:" << endl; // outLU << solver.matrixL() << endl; // outLU << "------------------------------------------------------------------" << endl; // outLU << "U:" << endl; // outLU << solver.matrixU() << endl; // outLU.close(); if(solver.info()!= Eigen::Success) { // decomposition failed return; } Eigen::VectorXd v_p = solver.solve(A_prime_T_b); t = (clock()-clock_start); printf("SolveTime:%d\n",t); //printMatrix(v_p,"v_p"); writeToDisk(v_p); if(solver.info()!= Eigen::Success) { // solving failed return; } // Eigen::VectorXd v_p = A_prime.householderQr().solve(b); // //Eigen::VectorXd v_p = A_prime.jacobiSvd().solve(b); // // // printMatrix(laplacianOperator3D,"laplacianOperator3D"); // printMatrix(A_prime,"A_prime"); // printMatrix(adjacentMatrix,"adjacentMatrix"); // printMatrix(b,"b"); // printMatrix(v_p,"v_p"); // //cout << laplacianOperator; }