int wrap_gsl_linalg_SV_solve(gsl_matrix* U, gsl_matrix* V, gsl_matrix* S,
const gsl_matrix* b, gsl_matrix* x)
{
gsl_vector_view _S = gsl_matrix_diagonal(S);
gsl_vector_const_view _b = gsl_matrix_const_column(b, 0);
gsl_vector_view _x = gsl_matrix_column(x, 0);
return gsl_linalg_SV_solve(U, V, &_S.vector, &_b.vector, &_x.vector);
}
CAMLprim value ml_gsl_linalg_SV_solve(value U, value V, value S, value B, value X)
{
_DECLARE_MATRIX2(U, V);
_DECLARE_VECTOR3(S, B, X);
_CONVERT_MATRIX2(U, V);
_CONVERT_VECTOR3(S, B, X);
gsl_linalg_SV_solve(&m_U, &m_V, &v_S, &v_B, &v_X);
return Val_unit;
}
int ap_gsl_linalg_SV_solve(gsl_matrix * x, gsl_matrix const * u,
gsl_vector const * s, gsl_matrix const * v,
gsl_matrix const * b){
const size_t p = x->size2;
for(size_t i = 0; i < p; ++i){
gsl_vector_view xcol = gsl_matrix_column(x,i);
gsl_vector_const_view bcol = gsl_matrix_const_column(b, i);
gsl_linalg_SV_solve(u, v, s, &bcol.vector, &xcol.vector);
}
return GSL_SUCCESS;
}
//------------------------------------------------------------------------------------------
int svd_solve(gsl_matrix * x, gsl_matrix const * a, gsl_matrix const * b,
double const tol){
// solve for X in AX=B by SVD decomposition method.
// m == numRows A == numRows B
// n == numCols A == numRows X
// p == numCols X == numCols B
std::cout << "\n" __FILE__ << " --> " << __func__ << "() --> " << __LINE__ << "\n";
const size_t m = a->size1;
const size_t n = a->size2;
const size_t p = b->size2;
gsl_matrix * U = gsl_matrix_alloc(m, n);
gsl_matrix * V = gsl_matrix_alloc(n, n);
gsl_vector * s = gsl_vector_alloc(n);
gsl_vector * w = gsl_vector_alloc(n);
gsl_matrix_memcpy(U, a);
assert(GSL_SUCCESS == gsl_linalg_SV_decomp(U, V, s, w));
{
//double norm = gsl_vector_get(s, 0); // largest singular value
//SHOW(std::max(m,n)*norm*std::numeric_limits<double>::epsilon());
//SHOW(gsl_vector_get(s, 0)/gsl_vector_get(s, n-1));
}
//AP_GSL_SHOW(s);
for(size_t i = 0; i < n; ++i){
if(tol > gsl_vector_get(s, i)){
gsl_vector_set(s, i, 0.0);
}
}
for(size_t i = 0; i < p; ++i){
gsl_vector_view xcol = gsl_matrix_column(x,i);
gsl_vector_const_view bcol = gsl_matrix_const_column(b, i);
gsl_linalg_SV_solve(U, V, s, &bcol.vector, &xcol.vector);
}
gsl_matrix_free(U);
gsl_matrix_free(V);
gsl_vector_free(s);
gsl_vector_free(w);
return GSL_SUCCESS;
}
/**
* C++ version of gsl_linalg_SV_solve().
* @param U An orthogonal matrix
* @param Q A matrix
* @param S A vector
* @param b A vector
* @param x A vector
* @return Error code on failure
*/
inline int SV_solve( matrix const& U, matrix const& Q, vector const& S, vector const& b, vector& x ){
return gsl_linalg_SV_solve( U.get(), Q.get(), S.get(), b.get(), x.get() ); }
/** From a vector-field dataset, compute the vector-valued weighting factors,
* \f$\vec{c}_j\f$. Info is returned in the rbf structure.
*
*
* \param[in] v - pointer to an array of position vectors.
* \param[in] B - pointer to array of corresponding field vectors.
* \param[in] n - number of (v, B) pairs defined.
* \param[in] eps - smoothing factor in scalar RBF.
* \param[in] RadialBasisFunction - RBF to use. Can be LGM_RBF_GAUSSIAN, LGM_RBF_MULTIQUADRIC
*
* \return pointer to structure containing info for RBF interpolation. User
* is responsible for freeing with Lgm_DFI_RBF_Free().
*
* \author M. G. Henderson
* date January 24, 2012
*
*
*/
Lgm_DFI_RBF_Info *Lgm_DFI_RBF_Init( unsigned long int *I_data, Lgm_Vector *v, Lgm_Vector *B, int n, double eps, int RadialBasisFunction ) {
int i, j, ii, jj, p, q, n3, s;
double *d, **a, Phi[3][3], val;
gsl_matrix *A, *V;
gsl_vector *D, *c, *S, *Work;
Lgm_DFI_RBF_Info *rbf;
n3 = 3*n;
A = gsl_matrix_calloc( n3, n3 );
c = gsl_vector_alloc( n3 );
D = gsl_vector_calloc( n3 );
/*
* Save info needed to do an evaluation.
*/
rbf = ( Lgm_DFI_RBF_Info *)calloc( 1, sizeof(*rbf) );
rbf->RadialBasisFunction = RadialBasisFunction;
rbf->eps = eps;
rbf->n = n;
rbf->n3 = n3;
LGM_ARRAY_1D( rbf->LookUpKey, n, unsigned long int);
LGM_ARRAY_1D( rbf->v, n, Lgm_Vector);
LGM_ARRAY_1D( rbf->c, n, Lgm_Vector);
for ( i=0; i<n; i++ ) {
rbf->LookUpKey[i] = I_data[i];
rbf->v[i] = v[i];
}
// This subtraction doesntm seem to work out very well...
// rbf->Bx0 = B[0].x;
// rbf->By0 = B[0].y;
// rbf->Bz0 = B[0].z;
double Bbkg;
for ( Bbkg = 0.0, i=0; i<n; i++ ) Bbkg += B[i].x; rbf->Bx0 = Bbkg/(double)n;
for ( Bbkg = 0.0, i=0; i<n; i++ ) Bbkg += B[i].y; rbf->By0 = Bbkg/(double)n;
for ( Bbkg = 0.0, i=0; i<n; i++ ) Bbkg += B[i].z; rbf->Bz0 = Bbkg/(double)n;
rbf->Bx0 = 0.0;
rbf->By0 = 0.0;
rbf->Bz0 = 0.0;
/*
* Fill d array. (Subtract off the field at the nearest point v[0] -- See
* McNally [2011].) We add this field back on later.
*/
for (i=0; i<n; i++){
gsl_vector_set( D, 3*i+0, B[i].x - rbf->Bx0 );
gsl_vector_set( D, 3*i+1, B[i].y - rbf->By0 );
gsl_vector_set( D, 3*i+2, B[i].z - rbf->Bz0 );
}
/*
* [ row0 ]
* Fill A matrix. In C, order is A[row][col] = [ row1 ]
* [ row2 ]
*/
for ( i=0; i<n; i++ ) { // locate start row for subarray
ii = 3*i;
for ( j=0; j<n; j++ ) { // locate start column for subarray
jj = 3*j;
// Get Phi( v_i - v_j )
Lgm_DFI_RBF_Phi( &v[i], &v[j], Phi, rbf );
for ( p=0; p<3; p++ ){ // subarray row
for ( q=0; q<3; q++ ){ // subarray column
gsl_matrix_set( A, ii+p, jj+q, Phi[p][q] );
}
}
}
}
/*
for (i=0; i<n; i++ ) {
printf("v%02d = %8g %8g %8g B%02d = %8g %8g %8g\n", i, v[i].x, v[i].y, v[i].z, i, B[i].x, B[i].y, B[i].z );
}
for (i=0; i<n3; i++){
for (j=0; j<n3; j++){
printf("%8g ", gsl_matrix_get(A, i, j ) );
}
printf("\n");
}
*/
/*
* Now we need to solve the system of equation;
*
* d = ac
*
* for c.
*
* First create gsl_vector and gsl_matrix views of the d and A arrays.
* Then compute Cholesky decomposition of the a array. Then solve the
* system to get c.
*
*/
if ( LGM_DFI_RBF_SOLVER == LGM_CHOLESKY_DECOMP ){
gsl_linalg_cholesky_decomp( A );
gsl_linalg_cholesky_solve( A, D, c );
} else if ( LGM_DFI_RBF_SOLVER == LGM_PLU_DECOMP ){
gsl_permutation *P = gsl_permutation_alloc( n3 );
gsl_linalg_LU_decomp( A, P, &s );
gsl_linalg_LU_solve( A, P, D, c );
gsl_permutation_free( P );
} else if ( LGM_DFI_RBF_SOLVER == LGM_SVD ){
V = gsl_matrix_calloc( n3, n3 );
S = gsl_vector_alloc( n3 );
Work = gsl_vector_alloc( n3 );
gsl_linalg_SV_decomp( A, V, S, Work );
gsl_linalg_SV_solve( A, V, S, D, c );
gsl_vector_free( Work );
gsl_vector_free( S );
gsl_matrix_free( V );
}
for (i=0; i<n; i++){
rbf->c[i].x = gsl_vector_get( c, 3*i+0 );
rbf->c[i].y = gsl_vector_get( c, 3*i+1 );
rbf->c[i].z = gsl_vector_get( c, 3*i+2 );
}
gsl_vector_free( D );
gsl_vector_free( c );
gsl_matrix_free( A );
return( rbf );
}
int SolveSVD (double a[], double b[], double x[], int neq, int nvar)
{
// get A
gsl_matrix_view av = gsl_matrix_view_array (a, neq, nvar);
if (neq < nvar) { // M < N ... do the transposed matrix
gsl_matrix *atrans = gsl_matrix_alloc (nvar, neq);
gsl_matrix_transpose_memcpy (atrans, &av.matrix);
gsl_matrix *v = gsl_matrix_alloc (neq, neq);
gsl_vector *s = gsl_vector_alloc (neq);
gsl_vector *work = gsl_vector_alloc (neq);
gsl_linalg_SV_decomp (atrans, v, s, work);
// x = A+ b
gsl_matrix *splus = gsl_matrix_calloc (neq, neq);
// compute sigma_plus
for (int i = 0; i < neq; i++) {
double sigma;
if ((sigma = gsl_vector_get (s,i)) != 0.0)
gsl_matrix_set (splus, i,i, 1.0/sigma);
}
gsl_linalg_matmult (atrans, splus, atrans);
gsl_linalg_matmult_mod (atrans, GSL_LINALG_MOD_NONE, v, GSL_LINALG_MOD_TRANSPOSE, atrans);
gsl_vector_view bv = gsl_vector_view_array (b, neq);
gsl_matrix_view bmv = gsl_matrix_view_vector (&bv.vector, neq, 1);
gsl_matrix *xx = gsl_matrix_alloc (nvar,1);
gsl_linalg_matmult (atrans, &bmv.matrix, xx);
// gsl_matrix_fprintf (stdout, xx, "%g");
for (int i = 0; i < nvar; i++) {
x[i] = gsl_matrix_get(xx,i,0);
}
gsl_matrix_free (splus); gsl_matrix_free (xx);
gsl_matrix_free (atrans);
gsl_matrix_free (v); gsl_vector_free (s); gsl_vector_free (work);
} else { // M >= N
gsl_matrix *v = gsl_matrix_alloc (nvar, nvar);
gsl_vector *s = gsl_vector_alloc (nvar);
gsl_vector *work = gsl_vector_alloc (nvar);
gsl_linalg_SV_decomp (&av.matrix, v, s, work);
// x = A+ b
gsl_vector_view bv = gsl_vector_view_array (b, neq);
gsl_vector *xx = gsl_vector_alloc (nvar);
gsl_linalg_SV_solve (&av.matrix, v, s, &bv.vector, xx);
// gsl_vector_fprintf (stdout, xx, "%g");
for (int i = 0; i < nvar; i++)
x[i] = gsl_vector_get (xx, i);
gsl_vector_free (xx);
gsl_matrix_free (v); gsl_vector_free (s); gsl_vector_free (work);
}
return 1;
}
/** Solves A*x = B using SVD
* @param A :: [input] The matrix A
* @param B :: [input] The vector B
* @return :: The solution x
*/
std::vector<double> MaxEnt::solveSVD(const DblMatrix &A, const DblMatrix &B) {
size_t dim = A.size().first;
gsl_matrix *a = gsl_matrix_alloc(dim, dim);
gsl_matrix *v = gsl_matrix_alloc(dim, dim);
gsl_vector *s = gsl_vector_alloc(dim);
gsl_vector *w = gsl_vector_alloc(dim);
gsl_vector *x = gsl_vector_alloc(dim);
gsl_vector *b = gsl_vector_alloc(dim);
// Need to copy from DblMatrix to gsl matrix
for (size_t k = 0; k < dim; k++)
for (size_t l = 0; l < dim; l++)
gsl_matrix_set(a, k, l, A[k][l]);
for (size_t k = 0; k < dim; k++)
gsl_vector_set(b, k, B[k][0]);
// Singular value decomposition
gsl_linalg_SV_decomp(a, v, s, w);
// A could be singular or ill-conditioned. We can use SVD to obtain a least
// squares
// solution by setting the small (compared to the maximum) singular values to
// zero
// Find largest sing value
double max = gsl_vector_get(s, 0);
for (size_t i = 0; i < dim; i++) {
if (max < gsl_vector_get(s, i))
max = gsl_vector_get(s, i);
}
// Apply a threshold to small singular values
const double THRESHOLD = 1E-6;
double threshold = THRESHOLD * max;
for (size_t i = 0; i < dim; i++)
if (gsl_vector_get(s, i) > threshold)
gsl_vector_set(s, i, gsl_vector_get(s, i));
else
gsl_vector_set(s, i, 0);
// Solve A*x = B
gsl_linalg_SV_solve(a, v, s, b, x);
// From gsl_vector to vector
std::vector<double> delta(dim);
for (size_t k = 0; k < dim; k++)
delta[k] = gsl_vector_get(x, k);
gsl_matrix_free(a);
gsl_matrix_free(v);
gsl_vector_free(s);
gsl_vector_free(w);
gsl_vector_free(x);
gsl_vector_free(b);
return delta;
}
void SurfacePropagation::applyConstraint()
{
if (!imp_int) return;
Implicit *imp = imp_int->getImplicit();
if (!imp) return;
if (speedfac == 0.0)
return;
unsigned int i,j;
unsigned int qlen = imp->qlen(); // # of parameters
unsigned int n = ps->size(); // # of control particles
// qdotPrev is the previous qdot. We use it for a backwards difference
// approximation to q'' in order to ensure stability and accuracy
bool qPrevValid(true);
if ( !qdotPrev || qdotPrev->size() != qlen )
{
// reset qdotPrev
delete qdotPrev;
qdotPrev = new DoubleArray(0.0, qlen);
qPrevValid = false;
}
// Allocate some derivative vectors
DoubleArray dqdt(0.0, qlen);
double* A = new double[n * qlen];
double* b = new double[n];
for(i = 0; i < n; i++) {
imp->procq(position->x[i], A + i * qlen);
b[i] = -speedfac*speed(i)*imp->grad(position->x[i]).length();
gmVector3 m = speedfac*speed(i)*imp->grad(position->x[i]);
m.normalize();
gmVector3 mm = velocity->v[i];
mm.normalize();
if ( dot(mm, m) < 0.90 )
perr->setScalar(i, 0.5);
else
perr->setScalar(i, 1.0);
// perr->setScalar(i, dot(mm, m));
}
#if 0
std::ofstream out("c:\\xinlai\\pointclouds\\A.txt");
std::ofstream out2("c:\\xinlai\\pointclouds\\b.txt");
for(unsignedint i = 0; i < n; i++)
{
for(int j = 0; j < qlen; j++)
out << A[i * qlen + j] << " ";
out << std::endl;
out2 << b[i] << std::endl;
}
#endif
gsl_matrix_view gsl_A = gsl_matrix_view_array(A, n, qlen);
gsl_matrix* V = gsl_matrix_alloc(qlen, qlen);
gsl_matrix* work_X = gsl_matrix_alloc(qlen, qlen);
gsl_vector* S = gsl_vector_alloc(qlen);
gsl_vector* work = gsl_vector_alloc(qlen);
gsl_linalg_SV_decomp(&gsl_A.matrix, V, S, work);
//gsl_linalg_SV_decomp_mod(&gsl_A.matrix, work_X, V, S, work);
//gsl_linalg_SV_decomp_jacobi(&gsl_A.matrix, V, S);
gsl_vector_view gsl_b = gsl_vector_view_array(b, n);
gsl_vector* x = gsl_vector_alloc(qlen);
for(unsigned int i = 0; i < qlen; i++) {
x->data[i] = 0.0;
if( S->data[i] < 0.01 )
S->data[i] = 0.0;
}
gsl_linalg_SV_solve(&gsl_A.matrix, V, S, &gsl_b.vector, x);
#if 0
double norm = 0.0;
for (j=0; j < qlen; j++) {
double dq = (double)gsl_vector_get(x, j);
norm += dq * dq;
}
norm = sqrt(norm);
#endif
for(j = 0; j < qlen; j++) {
dqdt[j] = (double)gsl_vector_get(x, j);
}
gsl_vector_free(x);
gsl_vector_free(S);
gsl_vector_free(work);
gsl_matrix_free(work_X);
gsl_matrix_free(V);
delete[] A;
delete[] b;
// Apply the changes to the implicit surface parameter
std::valarray<double> q(qlen);
imp->getq(q);
double stepsize;
if ( qPrevValid )
{
// backward difference approximation to q''
double qddNorm(0.0);
for (unsigned int i=0; i < qlen; i++) {
double x = dqdt[i] - (*qdotPrev)[i];
qddNorm += x * x;
}
qddNorm = sqrt(qddNorm) / ps->dt;
stepsize = sqrt(2 * tolerance / qddNorm);
} else
stepsize = ps->dt;
q += dqdt * stepsize;
imp->setq(q);
for (unsigned int i=0; i < qlen; i++)
(*qdotPrev)[i] = dqdt[i];
}
/* Computes acceleration to every control point of the jello cube,
which is in state given by 'jello'.
Returns result in array 'a'. */
void computeAcceleration(struct world * jello)
{
/*
printf("V start\n");
for (int i=0; i<particleNum; i++) {
printf("a[%d].x = %f a.y = %f a.z = %f\n",i,jello->v[i][0][0].x,jello->v[i][0][0].y, jello->v[i][0][0].z);
}
printf("\n");
*/
int ArraySize = 8 + (particleNum - 2) * 3;
int row = 0;
if (CRING) {
row = particleNum+2;
}else {
row = particleNum+1;
}
int column = ArraySize - row;
double *a_data = new double [ArraySize * ArraySize];
double *b_data = new double [ArraySize];
//Mass
for (int i=0; i<column; i++) {
for (int j=0; j<column; j++) {
if (i == j) {
a_data[i*ArraySize + j] = particleMass;
}else {
a_data[i*ArraySize + j] = 0;
}
}
}
//Empty Matrix
for (int i=0; i<row; i++) {
for (int j=0; j<row; j++) {
a_data[ArraySize*column + column + (i*ArraySize + j)] = 0;
}
}
//DEL_C
for (int i=0; i<row; i++) {
for (int j=0; j<column; j++) {
if(i==0){
if (j==0) {
a_data[ArraySize*column] = 1;
}else {
a_data[ArraySize*column + j] = 0;
}
}else if (i==1) {
if (j==1) {
a_data[ArraySize*column + (i*ArraySize + j)] = 1;
}else {
a_data[ArraySize*column + (i*ArraySize + j)] = 0;
}
}else if (i == row-1 && CRING == 1) {
if (j == column-2) {
a_data[ArraySize*column + (i*ArraySize + j)] = 2 * (jello->p[i-2].x);
}else if (j == column-1) {
a_data[ArraySize*column + (i*ArraySize + j)] = 2 * (jello->p[i-2].y);
}else {
a_data[ArraySize*column + (i*ArraySize + j)] = 0;
}
}else {
if (j == (i-2)*2) {
a_data[ArraySize*column + (i*ArraySize + j)] = (-2) * (jello->p[i-1].x - jello->p[i-2].x);
}else if (j == (i-2)*2 + 1) {
a_data[ArraySize*column + (i*ArraySize + j)] = (-2) * (jello->p[i-1].y - jello->p[i-2].y);
}else if (j == (i-2)*2 + 2) {
a_data[ArraySize*column + (i*ArraySize + j)] = 2 * (jello->p[i-1].x - jello->p[i-2].x);
}else if (j == (i-2)*2 + 3) {
a_data[ArraySize*column + (i*ArraySize + j)] = 2 * (jello->p[i-1].y - jello->p[i-2].y);
}else {
a_data[ArraySize*column + (i*ArraySize + j)] = 0;
}
}
}
}
//Transpose of DEL_C
for (int i=0; i<column; i++) {
for (int j=0; j<row; j++) {
a_data[column + ArraySize*i + j] = a_data[ArraySize * (row-j-1+column) + i];
}
}
//set F value(Gravity)
for (int i=0; i<column; i++) {
if (i%2) {
b_data[i] = jello->Force.y - (alpha * jello->v[(i-1)/2].y);
}else {
b_data[i] = jello->Force.x - (alpha * jello->v[i/2].x);
}
}
gsl_matrix *dcdq = gsl_matrix_alloc(row, column);
gsl_matrix *dcdqdot = gsl_matrix_alloc(row, column);
gsl_matrix *velocity = gsl_matrix_alloc(column, 1);
gsl_vector *C = gsl_vector_alloc(row);
gsl_matrix *temp1 = gsl_matrix_alloc(row, 1);
gsl_matrix *temp2 = gsl_matrix_alloc(row, 1);
//set dzdqdot
for (int i=0; i<row; i++) {
for (int j=0; j<column; j++) {
if(i==0){
gsl_matrix_set(dcdqdot, i, j, 0);
}else if (i==1) {
gsl_matrix_set(dcdqdot, i, j, 0);
}else if (i == row-1 && CRING == 1) {
if (j == column-2) {
gsl_matrix_set(dcdqdot, i, j, 2 * (jello->v[i-2].x));
}else if (j == column-1) {
gsl_matrix_set(dcdqdot, i, j, 2 * (jello->v[i-2].y));
}else {
gsl_matrix_set(dcdqdot, i, j, 0);
}
}else {
if (j == (i-2)*2) {
gsl_matrix_set(dcdqdot, i, j, (-2) * (jello->v[i-1].x - jello->v[i-2].x));
}else if (j == (i-2)*2 + 1) {
gsl_matrix_set(dcdqdot, i, j, (-2) * (jello->v[i-1].y - jello->v[i-2].y));
}else if (j == (i-2)*2 + 2) {
gsl_matrix_set(dcdqdot, i, j, 2 * (jello->v[i-1].x - jello->v[i-2].x));
}else if (j == (i-2)*2 + 3) {
gsl_matrix_set(dcdqdot, i, j, 2 * (jello->v[i-1].y - jello->v[i-2].y));
}else {
gsl_matrix_set(dcdqdot, i, j, 0);
}
}
}
}
//set dcdq
for (int i=0; i<row; i++) {
for (int j=0; j<column; j++) {
if(i==0){
if (j==0) {
gsl_matrix_set(dcdq, i, j, 1);
}else {
gsl_matrix_set(dcdq, i, j, 0);
}
}else if (i==1) {
if (j==1) {
gsl_matrix_set(dcdq, i, j, 1);
}else {
gsl_matrix_set(dcdq, i, j, 0);
}
}else if (i == row-1 && CRING == 1) {
if (j == column-2) {
gsl_matrix_set(dcdq, i, j, 2 * (jello->p[i-2].x));
}else if (j == column-1) {
gsl_matrix_set(dcdq, i, j, 2 * (jello->p[i-2].y));
}else {
gsl_matrix_set(dcdq, i, j, 0);
}
}else {
if (j == (i-2)*2) {
gsl_matrix_set(dcdq, i, j, (-2) * (jello->p[i-1].x - jello->p[i-2].x));
}else if (j == (i-2)*2 + 1) {
gsl_matrix_set(dcdq, i, j, (-2) * (jello->p[i-1].y - jello->p[i-2].y));
}else if (j == (i-2)*2 + 2) {
gsl_matrix_set(dcdq, i, j, 2 * (jello->p[i-1].x - jello->p[i-2].x));
}else if (j == (i-2)*2 + 3) {
gsl_matrix_set(dcdq, i, j, 2 * (jello->p[i-1].y - jello->p[i-2].y));
}else {
gsl_matrix_set(dcdq, i, j, 0);
}
}
}
}
//set velocity
for (int i=0; i<column; i++) {
if (i == 0 || i == 1) {
gsl_matrix_set(velocity, i, 0, 0);
}else if (i%2) {
gsl_matrix_set(velocity, i-1, 0, jello->v[(i-1)/2].x);
gsl_matrix_set(velocity, i, 0, jello->v[(i-1)/2].y);
}
}
//set C
for (int i=0; i<row; i++) {
if (i==0) {
gsl_vector_set(C, i, jello->p[0].x);
}else if (i==1) {
gsl_vector_set(C, i, jello->p[0].y - 0.5);
}else if (i==row-1 && CRING == 1) {
gsl_vector_set(C, i, pow(jello->p[i-2].x, 2) + pow(jello->p[i-2].y, 2) - 0.25);
}else {
gsl_vector_set(C, i, pow(jello->p[i-1].x - jello->p[i-2].x ,2) + pow(jello->p[i-1].y - jello->p[i-2].y, 2) - pow(particleLength, 2));
}
}
//get the error of cpnstraints and print it
constraintsError = 0;
for (int i =0; i<row; i++) {
constraintsError += gsl_vector_get(C, i);
}
//printf("%g\n",constraintsError);
//set temp
for (int i=0; i<row; i++) {
gsl_matrix_set(temp1, i, 0, 0.0);
gsl_matrix_set(temp2, i, 0, 0.0);
}
//get temp1
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, dcdqdot, velocity, 0.0, temp1);
//get temp2
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, dcdq, velocity, 0.0, temp2);
//st b_data
for (int i=0; i<row; i++) {
b_data[column+i] = (-1 * gsl_matrix_get(temp1, i, 0)) - (2* beta * gsl_matrix_get(temp2, i, 0)) - (pow(beta, 2) * gsl_vector_get(C, i)) ;
}
/*
printf("dcdqdot start\n");
for (int i=0; i<row; i++) {
for (int j=0; j<column; j++) {
printf("%f ",gsl_matrix_get(dcdqdot, i, j));
}
printf("\n");
}
printf("velocity start\n");
for (int j=0; j<column; j++) {
printf("%f ",gsl_matrix_get(velocity, j,0));
}
printf("\n");
printf("temp1 start\n");
for (int j=0; j<row; j++) {
printf("%f ",gsl_matrix_get(temp1, j,0));
}
printf("\n");
//print a_data value
printf("a_data start\n");
for (int i=0; i<ArraySize; i++) {
for (int j=0; j<ArraySize; j++) {
printf("%f ",a_data[i*ArraySize+j]);
}
printf("\n");
}
//print b_data value
printf("b_data start\n");
for (int i=0; i<ArraySize; i++) {
printf("%f ",b_data[i]);
}
printf("\n");
*/
gsl_matrix *A = gsl_matrix_alloc(ArraySize, ArraySize);
gsl_matrix *V = gsl_matrix_alloc(ArraySize, ArraySize);
gsl_vector *S = gsl_vector_alloc(ArraySize);
gsl_vector *work = gsl_vector_alloc(ArraySize);
gsl_vector *B = gsl_vector_alloc(ArraySize);
gsl_vector *X = gsl_vector_alloc (ArraySize);
//set A matrix
for (int i=0; i<ArraySize; i++) {
for (int j=0; j<ArraySize; j++) {
gsl_matrix_set(A, i, j, a_data[i*ArraySize + j]);
}
}
//set B vector
for (int i=0; i<ArraySize; i++) {
gsl_vector_set(B, i, b_data[i]);
}
gsl_linalg_SV_decomp(A, V, S, work);
//Check the singular value
double eps = 1e-6;
double MaxSValue = gsl_vector_get(S, 0);
double CheckValue = MaxSValue * eps;
for (int i=0; i<ArraySize; i++) {
if (gsl_vector_get(S, i) < CheckValue) {
gsl_vector_set(S, i, 0.0);
}
}
gsl_linalg_SV_solve(A, V, S, B, X);
/*
//Check Value
printf("A start\n");
for (int i=0; i<ArraySize; i++) {
for (int j=0; j<ArraySize; j++) {
printf("%f ", gsl_matrix_get (A, i, j));
}
printf("\n");
}
printf("S start\n");
for (int i=0; i<ArraySize; i++) {
printf("%f ", gsl_vector_get (S, i));
}
printf("\n");
printf("X start\n");
for (int i=0; i<ArraySize; i++) {
printf("%f ", gsl_vector_get (X, i));
}
printf("\n");
*/
for (int i=0; i<particleNum; i++) {
jello->a[i].x = gsl_vector_get(X, i*2);
jello->a[i].y = gsl_vector_get(X, i*2 +1);
jello->a[i].z = 0;
}
/*
printf("A start\n");
for (int i=0; i<particleNum; i++) {
printf("a[%d].x = %f a.y = %f a.z = %f\n",i,jello->a[i][0][0].x,jello->a[i][0][0].y, jello->a[i][0][0].z);
}
printf("\n");
*/
//printf("Fx: %f Fy: %f\n", jello->Force.x, jello->Force.y);
//release data
delete [] a_data;
delete [] b_data;
gsl_matrix_free(A);
gsl_matrix_free(V);
gsl_vector_free(S);
gsl_vector_free(work);
gsl_vector_free(B);
gsl_vector_free(X);
}
void computePhysics(struct chain * chain, struct point a[])
{
int n = chain->number;
double mass = chain->totalMass/(chain->number+1.0);
gsl_matrix *U = gsl_matrix_alloc (n*3+1, n*3+1);
gsl_matrix_set_zero(U);
gsl_matrix_view M = gsl_matrix_submatrix(U, 0, 0, n*2, n*2);
gsl_matrix_view nC = gsl_matrix_submatrix(U, n*2, 0, n+1, n*2);
gsl_matrix_view nCt = gsl_matrix_submatrix(U, 0, n*2, n*2, n+1);
//Set Matrix M
for(int i = 0; i < n*2; i++)
gsl_matrix_set(&M.matrix, i, i, mass/n);
//Set Matrix NablaC
gsl_matrix_set(&nC.matrix, 0, 0, chain->p[1].x*2.0);
gsl_matrix_set(&nC.matrix, 0, 1, chain->p[1].y*2.0);
for(int i = 1; i < n; i++)
{
gsl_matrix_set(&nC.matrix, i, i*2-2, (chain->p[i].x - chain->p[i+1].x)*2.0);
gsl_matrix_set(&nC.matrix, i, i*2-1, (chain->p[i].y - chain->p[i+1].y)*2.0);
gsl_matrix_set(&nC.matrix, i, i*2, (chain->p[i+1].x - chain->p[i].x)*2.0);
gsl_matrix_set(&nC.matrix, i, i*2+1, (chain->p[i+1].y - chain->p[i].y)*2.0);
}
if(isConstraint)
{
gsl_matrix_set(&nC.matrix, n, n*2-2, chain->p[n].x*2.0);
gsl_matrix_set(&nC.matrix, n, n*2-1, chain->p[n].y*2.0 + 1.0);
}else
{
gsl_matrix_set(&nC.matrix, n, n*2-2, 0.0);
gsl_matrix_set(&nC.matrix, n, n*2-1, 0.0);
}
//Set Matrix NablaCt
gsl_matrix_set(&nCt.matrix, 0, 0, chain->p[1].x*2.0);
gsl_matrix_set(&nCt.matrix, 1, 0, chain->p[1].y*2.0);
for(int i = 1; i < n; i++)
{
gsl_matrix_set(&nCt.matrix, i*2-2, i, (chain->p[i].x - chain->p[i+1].x)*2.0);
gsl_matrix_set(&nCt.matrix, i*2-1, i, (chain->p[i].y - chain->p[i+1].y)*2.0);
gsl_matrix_set(&nCt.matrix, i*2, i, (chain->p[i+1].x - chain->p[i].x)*2.0);
gsl_matrix_set(&nCt.matrix, i*2+1, i, (chain->p[i+1].y - chain->p[i].y)*2.0);
}
if(isConstraint)
{
gsl_matrix_set(&nCt.matrix, n*2-2, n, chain->p[n].x*2.0);
gsl_matrix_set(&nCt.matrix, n*2-1, n, chain->p[n].y*2.0 + 1.0);
}else
{
gsl_matrix_set(&nCt.matrix, n*2-2, n, 0.0);
gsl_matrix_set(&nCt.matrix, n*2-1, n, 0.0);
}
gsl_matrix *V = gsl_matrix_alloc(n*3+1, n*3+1);
gsl_vector *s = gsl_vector_alloc(n*3+1);
gsl_vector *workvec = gsl_vector_alloc(n*3+1);
// for (int i = 0; i < n*2+n+1; i++)
// {
// for (int j = 0; j < n*2+n+1; j++)
// printf ("%.1f ", gsl_matrix_get (U, i, j));
// printf ("\n");
// }
gsl_linalg_SV_decomp(U, V, s, workvec);
//Filter
double max = gsl_vector_max(s);
for(int i=0; i<n*3+1; i++)
if(gsl_vector_get(s, i) < max * 0.000001)
gsl_vector_set(s, i, 0.0);
gsl_vector *b = gsl_vector_alloc(n*3+1);
gsl_vector *b1 = gsl_vector_alloc(n+1);
gsl_vector *x = gsl_vector_alloc(n*3+1);
gsl_vector_set_zero(b);
gsl_vector_set_zero(b1);
gsl_vector_view fext = gsl_vector_subvector(b, 0, n*2);
gsl_vector_view bs = gsl_vector_subvector(b, n*2, n+1);
//Set Vector fext
for(int i = 0; i < n; i++)
{
gsl_vector_set(&fext.vector, i*2, chain->f[i+1].x);
gsl_vector_set(&fext.vector, i*2+1, chain->f[i+1].y);
}
//Set Vector bs
gsl_vector_set(&bs.vector, 0, - chain->v[1].x*chain->v[1].x*2.0 - chain->v[1].y*chain->v[1].y*2.0);
for(int i = 1; i < n; i++)
gsl_vector_set(&bs.vector, i,
- (chain->v[i].x-chain->v[i+1].x)*chain->v[i].x*2.0 -
(chain->v[i].y-chain->v[i+1].y)*chain->v[i].y*2.0 -
(chain->v[i+1].x-chain->v[i].x)*chain->v[i+1].x*2.0 -
(chain->v[i+1].y-chain->v[i].y)*chain->v[i+1].y*2.0
);
if(isConstraint)
gsl_vector_set(&bs.vector, n, -chain->v[n].x*chain->v[n].x*2.0 - (chain->v[n].y*2.0+1.0)* chain->v[n].y);
else gsl_vector_set(&bs.vector, n, 0.0);
//Compute Baumgarte stabilization
gsl_vector_set(b1, 0, - chain->p[1].x*chain->v[1].x*2.0 - chain->p[1].y*chain->v[1].y*2.0);
for(int i = 1; i < n; i++)
gsl_vector_set(b1, i,
- (chain->p[i].x-chain->p[i+1].x)*chain->v[i].x*2.0 -
(chain->p[i].y-chain->p[i+1].y)*chain->v[i].y*2.0 -
(chain->p[i+1].x-chain->p[i].x)*chain->v[i+1].x*2.0 -
(chain->p[i+1].y-chain->p[i].y)*chain->v[i+1].y*2.0
);
if(isConstraint)
gsl_vector_set(b1, n, -chain->p[n].x*chain->v[n].x*2.0 - (chain->p[n].y*2.0+1.0)* chain->v[n].y);
else gsl_vector_set(b1, n, 0.0);
gsl_vector_scale(b1, BSALPHA * 2.0);
gsl_vector_add(&bs.vector, b1);
gsl_vector_set_zero(b1);
gsl_vector_set(b1, 0, - chain->p[1].x*chain->p[1].x - chain->p[1].y*chain->p[1].y + 0.01);
for(int i = 1; i < n; i++)
gsl_vector_set(b1, i,
- (chain->p[i+1].x-chain->p[i].x) * (chain->p[i+1].x-chain->p[i].x)
- (chain->p[i+1].y-chain->p[i].y) * (chain->p[i+1].y-chain->p[i].y) + 0.01
);
if(isConstraint)
gsl_vector_set(b1, n, - chain->p[n].x*chain->p[n].x - (chain->p[n].y+0.5) * (chain->p[n].y+0.5) + 0.25);
else gsl_vector_set(b1, n, 0.0);
gsl_vector_scale(b1, BSALPHA * BSALPHA);
gsl_vector_add(&bs.vector, b1);
//Solve The Equation
gsl_linalg_SV_solve(U, V, s, b, x);
for(int i=0; i<chain->number; i++)
{
a[i].x = gsl_vector_get(x, i*2);
a[i].y = gsl_vector_get(x, i*2+1);
}
// for (int i = n*2; i < n*3+1; i++)
// {
// printf ("%g ", gsl_vector_get(b, i));
// printf ("\n");
// }
// printf ("\n");
gsl_vector_free(x);
gsl_vector_free(b1);
gsl_vector_free(b);
gsl_vector_free(workvec);
gsl_vector_free(s);
gsl_matrix_free(V);
gsl_matrix_free(U);
}
