/*******************************************************************************
* int fit_ellipsoid(matrix_t points, vector_t* center, vector_t* lengths)
*
* Fits an ellipsoid to a set of points in 3D space. The principle axes of the
* fitted ellipsoid align with the global coordinate system. Therefore there are
* 6 degrees of freedom defining the ellipsoid: the x,y,z coordinates of the
* centroid and the lengths from the centroid to the surfance in each of the 3
* directions.
*
* matrix_t points is a tall matrix with 3 columns and at least 6 rows. Each row
* must contain the xy&z components of each individual point to be fit. If only
* 6 rows are provided, the resulting ellipsoid will be an exact fit. Otherwise
* the result is a least-squares fit to the overdefined dataset.
*
* vector_t* center is a pointer to a user-created vector which will contain the
* x,y,z position of the centroid of the fit ellipsoid.
*
* vector_t* lengths is a pointer to a user-created vector which will be
* populated with the 3 distances from the surface to the centroid in each of the
* 3 directions.
*******************************************************************************/
int fit_ellipsoid(matrix_t points, vector_t* center, vector_t* lengths){
int i,p;
matrix_t A;
vector_t b;
if(!points.initialized){
printf("ERROR: matrix_t points not initialized\n");
return -1;
}
if(points.cols!=3){
printf("ERROR: matrix_t points must have 3 columns\n");
return -1;
}
p = points.rows;
if(p<6){
printf("ERROR: matrix_t points must have at least 6 rows\n");
return -1;
}
b = create_vector_of_ones(p);
A = create_matrix(p,6);
for(i=0;i<p;i++){
A.data[i][0] = points.data[i][0] * points.data[i][0];
A.data[i][1] = points.data[i][0];
A.data[i][2] = points.data[i][1] * points.data[i][1];
A.data[i][3] = points.data[i][1];
A.data[i][4] = points.data[i][2] * points.data[i][2];
A.data[i][5] = points.data[i][2];
}
vector_t f = lin_system_solve_qr(A,b);
destroy_matrix(&A);
destroy_vector(&b);
// compute center
*center = create_vector(3);
center->data[0] = -f.data[1]/(2*f.data[0]);
center->data[1] = -f.data[3]/(2*f.data[2]);
center->data[2] = -f.data[5]/(2*f.data[4]);
// Solve for lengths
A = create_square_matrix(3);
b = create_vector(3);
// fill in A
A.data[0][0] = (f.data[0] * center->data[0] * center->data[0]) + 1.0;
A.data[0][1] = (f.data[0] * center->data[1] * center->data[1]);
A.data[0][2] = (f.data[0] * center->data[2] * center->data[2]);
A.data[1][0] = (f.data[2] * center->data[0] * center->data[0]);
A.data[1][1] = (f.data[2] * center->data[1] * center->data[1]) + 1.0;
A.data[1][2] = (f.data[2] * center->data[2] * center->data[2]);
A.data[2][0] = (f.data[4] * center->data[0] * center->data[0]);
A.data[2][1] = (f.data[4] * center->data[1] * center->data[1]);
A.data[2][2] = (f.data[4] * center->data[2] * center->data[2]) + 1.0;
// fill in b
b.data[0] = f.data[0];
b.data[1] = f.data[2];
b.data[2] = f.data[4];
// solve for lengths
vector_t scales = lin_system_solve(A, b);
*lengths = create_vector(3);
lengths->data[0] = 1.0/sqrt(scales.data[0]);
lengths->data[1] = 1.0/sqrt(scales.data[1]);
lengths->data[2] = 1.0/sqrt(scales.data[2]);
// cleanup
destroy_vector(&scales);
destroy_matrix(&A);
destroy_vector(&b);
return 0;
}
void compute_trajectory(Space *space, DiscreteProblem *dp)
{
info("alpha = (%g, %g, %g, %g), zeta = (%g, %g, %g, %g)",
alpha_ctrl[0], alpha_ctrl[1],
alpha_ctrl[2], alpha_ctrl[3], zeta_ctrl[0],
zeta_ctrl[1], zeta_ctrl[2], zeta_ctrl[3]);
// Newton's loop.
// Fill vector coeff_vec using dof and coeffs arrays in elements.
double *coeff_vec = new double[Space::get_num_dofs(space)];
get_coeff_vector(space, coeff_vec);
// Set up the solver, matrix, and rhs according to the solver selection.
SparseMatrix* matrix = create_matrix(matrix_solver);
Vector* rhs = create_vector(matrix_solver);
Solver* solver = create_linear_solver(matrix_solver, matrix, rhs);
int it = 1;
while (true)
{
// Obtain the number of degrees of freedom.
int ndof = Space::get_num_dofs(space);
// Assemble the Jacobian matrix and residual vector.
dp->assemble(coeff_vec, matrix, rhs);
// Calculate the l2-norm of residual vector.
double res_l2_norm = get_l2_norm(rhs);
// Info for user.
info("---- Newton iter %d, ndof %d, res. l2 norm %g", it, Space::get_num_dofs(space), res_l2_norm);
// If l2 norm of the residual vector is within tolerance, then quit.
// NOTE: at least one full iteration forced
// here because sometimes the initial
// residual on fine mesh is too small.
if(res_l2_norm < NEWTON_TOL && it > 1) break;
// Multiply the residual vector with -1 since the matrix
// equation reads J(Y^n) \deltaY^{n+1} = -F(Y^n).
for(int i = 0; i < ndof; i++) rhs->set(i, -rhs->get(i));
// Solve the linear system.
if(!solver->solve()) error ("Matrix solver failed.\n");
// Add \deltaY^{n+1} to Y^n.
for (int i = 0; i < ndof; i++) coeff_vec[i] += solver->get_solution()[i];
// If the maximum number of iteration has been reached, then quit.
if (it >= NEWTON_MAX_ITER) error ("Newton method did not converge.");
// Copy coefficients from vector y to elements.
set_coeff_vector(coeff_vec, space);
it++;
}
// Cleanup.
delete matrix;
delete rhs;
delete solver;
delete [] coeff_vec;
}
开发者ID:colman01，项目名称:hermes，代码行数:63，代码来源:main.cpp
示例5: QR_decomposition
/*******************************************************************************
* int QR_decomposition(matrix_t A, matrix_t* Q, matrix_t* R)
*
*
*******************************************************************************/
int QR_decomposition(matrix_t A, matrix_t* Q, matrix_t* R){
int i, j, k, s;
int m = A.rows;
int n = A.cols;
vector_t xtemp;
matrix_t Qt, Rt, Qi, F, temp;
if(!A.initialized){
printf("ERROR: matrix not initialized yet\n");
return -1;
}
destroy_matrix(Q);
destroy_matrix(R);
Qt = create_matrix(m,m);
for(i=0;i<m;i++){ // initialize Qt as I
Qt.data[i][i] = 1;
}
Rt = duplicate_matrix(A); // duplicate A to Rt
for(i=0;i<n;i++){ // iterate through columns of A
xtemp = create_vector(m-i); // allocate length, decreases with i
for(j=i;j<m;j++){ // take col of -R from diag down
xtemp.data[j-i] = -Rt.data[j][i];
}
if(Rt.data[i][i] > 0) s = -1; // check the sign
else s = 1;
xtemp.data[0] += s*vector_norm(xtemp); // add norm to 1st element
Qi = create_square_matrix(m); // initialize Qi
F = create_square_matrix(m-i); // initialize shrinking householder_matrix
F = householder_matrix(xtemp); // fill in Househodor
for(j=0;j<i;j++){
Qi.data[j][j] = 1; // fill in partial I matrix
}
for(j=i;j<m;j++){ // fill in remainder (householder_matrix)
for(k=i;k<m;k++){
Qi.data[j][k] = F.data[j-i][k-i];
}
}
// multiply new Qi to old Qtemp
temp = duplicate_matrix(Qt);
destroy_matrix(&Qt);
Qt = multiply_matrices(Qi,temp);
destroy_matrix(&temp);
// same with Rtemp
temp = duplicate_matrix(Rt);
destroy_matrix(&Rt);
Rt = multiply_matrices(Qi,temp);
destroy_matrix(&temp);
// free other allocation used in this step
destroy_matrix(&Qi);
destroy_matrix(&F);
destroy_vector(&xtemp);
}
transpose_matrix(&Qt);
*Q = Qt;
*R = Rt;
return 0;
}
int main()
{
// Time measurement.
TimePeriod cpu_time;
cpu_time.tick();
// Create coarse mesh, set Dirichlet BC, enumerate basis functions.
Space* space = new Space(A, B, NELEM, DIR_BC_LEFT, DIR_BC_RIGHT, P_INIT, NEQ);
// Enumerate basis functions, info for user.
int ndof = Space::get_num_dofs(space);
info("ndof: %d", ndof);
// Initialize the weak formulation.
WeakForm wf;
wf.add_matrix_form(jacobian);
wf.add_vector_form(residual);
// Initialize the FE problem.
bool is_linear = false;
DiscreteProblem *dp_coarse = new DiscreteProblem(&wf, space, is_linear);
// Newton's loop on coarse mesh.
// Fill vector coeff_vec using dof and coeffs arrays in elements.
double *coeff_vec_coarse = new double[Space::get_num_dofs(space)];
get_coeff_vector(space, coeff_vec_coarse);
// Set up the solver, matrix, and rhs according to the solver selection.
SparseMatrix* matrix_coarse = create_matrix(matrix_solver);
Vector* rhs_coarse = create_vector(matrix_solver);
Solver* solver_coarse = create_linear_solver(matrix_solver, matrix_coarse, rhs_coarse);
int it = 1;
bool success = false;
while (1) {
// Obtain the number of degrees of freedom.
int ndof_coarse = Space::get_num_dofs(space);
// Assemble the Jacobian matrix and residual vector.
dp_coarse->assemble(coeff_vec_coarse, matrix_coarse, rhs_coarse);
// Calculate the l2-norm of residual vector.
double res_l2_norm = get_l2_norm(rhs_coarse);
// Info for user.
info("---- Newton iter %d, ndof %d, res. l2 norm %g", it, Space::get_num_dofs(space), res_l2_norm);
// If l2 norm of the residual vector is within tolerance, then quit.
// NOTE: at least one full iteration forced
// here because sometimes the initial
// residual on fine mesh is too small.
if(res_l2_norm < NEWTON_TOL_COARSE && it > 1) break;
// Multiply the residual vector with -1 since the matrix
// equation reads J(Y^n) \deltaY^{n+1} = -F(Y^n).
for(int i=0; i < ndof_coarse; i++) rhs_coarse->set(i, -rhs_coarse->get(i));
// Solve the linear system.
if(!solver_coarse->solve())
error ("Matrix solver failed.\n");
// Add \deltaY^{n+1} to Y^n.
for (int i = 0; i < ndof_coarse; i++) coeff_vec_coarse[i] += solver_coarse->get_solution()[i];
// If the maximum number of iteration has been reached, then quit.
if (it >= NEWTON_MAX_ITER) error ("Newton method did not converge.");
// Copy coefficients from vector y to elements.
set_coeff_vector(coeff_vec_coarse, space);
it++;
}
// Cleanup.
delete matrix_coarse;
delete rhs_coarse;
delete solver_coarse;
delete [] coeff_vec_coarse;
delete dp_coarse;
// DOF and CPU convergence graphs.
SimpleGraph graph_dof_est, graph_cpu_est;
SimpleGraph graph_dof_exact, graph_cpu_exact;
// Adaptivity loop:
int as = 1;
bool done = false;
do
{
info("---- Adaptivity step %d:", as);
// Construct globally refined reference mesh and setup reference space.
Space* ref_space = construct_refined_space(space);
// Initialize the FE problem.
bool is_linear = false;
DiscreteProblem* dp = new DiscreteProblem(&wf, ref_space, is_linear);
// Set up the solver, matrix, and rhs according to the solver selection.
SparseMatrix* matrix = create_matrix(matrix_solver);
//.........这里部分代码省略.........
int main() {
// Time measurement.
TimePeriod cpu_time;
cpu_time.tick();
// Create coarse mesh, set Dirichlet BC, enumerate basis functions.
Space* space = new Space(A, B, NELEM, DIR_BC_LEFT, DIR_BC_RIGHT, P_INIT, NEQ);
info("N_dof = %d.", Space::get_num_dofs(space));
// Initialize the weak formulation.
WeakForm wf;
wf.add_matrix_form(jacobian);
wf.add_vector_form(residual);
double elem_errors[MAX_ELEM_NUM]; // This array decides what
// elements will be refined.
ElemPtr2 ref_elem_pairs[MAX_ELEM_NUM]; // To store element pairs from the
// FTR solution. Decides how
// elements will be hp-refined.
for (int i=0; i < MAX_ELEM_NUM; i++) {
ref_elem_pairs[i][0] = new Element();
ref_elem_pairs[i][1] = new Element();
}
// DOF and CPU convergence graphs.
SimpleGraph graph_dof_exact, graph_cpu_exact;
// Adaptivity loop:
int as = 1;
bool done = false;
do
{
info("---- Adaptivity step %d:", as);
// Initialize the FE problem.
bool is_linear = false;
DiscreteProblem *dp_coarse = new DiscreteProblem(&wf, space, is_linear);
// Newton's loop on coarse mesh.
// Fill vector coeff_vec using dof and coeffs arrays in elements.
double *coeff_vec_coarse = new double[Space::get_num_dofs(space)];
get_coeff_vector(space, coeff_vec_coarse);
// Set up the solver, matrix, and rhs according to the solver selection.
SparseMatrix* matrix_coarse = create_matrix(matrix_solver);
Vector* rhs_coarse = create_vector(matrix_solver);
Solver* solver_coarse = create_linear_solver(matrix_solver, matrix_coarse, rhs_coarse);
int it = 1;
while (1) {
// Obtain the number of degrees of freedom.
int ndof_coarse = Space::get_num_dofs(space);
// Assemble the Jacobian matrix and residual vector.
dp_coarse->assemble(matrix_coarse, rhs_coarse);
// Calculate the l2-norm of residual vector.
double res_l2_norm = get_l2_norm(rhs_coarse);
// Info for user.
info("---- Newton iter %d, ndof %d, res. l2 norm %g", it, Space::get_num_dofs(space), res_l2_norm);
// If l2 norm of the residual vector is within tolerance, then quit.
// NOTE: at least one full iteration forced
// here because sometimes the initial
// residual on fine mesh is too small.
if(res_l2_norm < NEWTON_TOL_COARSE && it > 1) break;
// Multiply the residual vector with -1 since the matrix
// equation reads J(Y^n) \deltaY^{n+1} = -F(Y^n).
for(int i=0; i<ndof_coarse; i++) rhs_coarse->set(i, -rhs_coarse->get(i));
// Solve the linear system.
if(!solver_coarse->solve())
error ("Matrix solver failed.\n");
// Add \deltaY^{n+1} to Y^n.
for (int i = 0; i < ndof_coarse; i++) coeff_vec_coarse[i] += solver_coarse->get_solution()[i];
// If the maximum number of iteration has been reached, then quit.
if (it >= NEWTON_MAX_ITER) error ("Newton method did not converge.");
// Copy coefficients from vector y to elements.
set_coeff_vector(coeff_vec_coarse, space);
it++;
}
// Cleanup.
delete matrix_coarse;
delete rhs_coarse;
delete solver_coarse;
delete [] coeff_vec_coarse;
delete dp_coarse;
// For every element perform its fast trial refinement (FTR),
// calculate the norm of the difference between the FTR
// solution and the coarse space solution, and store the
// error in the elem_errors[] array.
int n_elem = space->get_n_active_elem();
//.........这里部分代码省略.........
客服电话
APP下载
官方微信
请发表评论