/* Transpose matrix B to both:
*
* - increase cache hits
* - simd GCC vector extensions which is made possible.
* by the transposition, to increase likelyhood of SIMDs.
*
* Note that GCC 6 O=3 is smart enough to use SIMD
* even for the naive CPU method. However this was still way faster.
* */
void mat_mul_cpu_trans_vec(const F *A, const F *B, F *C, size_t n, Cache *cache) {
F tmpf;
size_t i, j, k, k_max, ai, bi;
Vec tmp, a, b;
UNUSED(cache);
mat_trans((F*)B, n);
k_max = (n / VECTOR_NELEMS) * VECTOR_NELEMS;
for (i = 0; i < n; ++i) {
for (j = 0; j < n; ++j) {
vec_zero(&tmp, VECTOR_NELEMS);
for (k = 0; k < k_max; k += VECTOR_NELEMS) {
ai = i * n + k;
bi = j * n + k;
vec_load(&a, VECTOR_NELEMS, A, ai);
vec_load(&b, VECTOR_NELEMS, B, bi);
tmp += a * b;
}
tmpf = 0.0;
for (; k < n; ++k) {
tmpf += A[i*n+k] * B[j*n+k];
}
C[i*n+j] = vec_sum(tmp, VECTOR_NELEMS) + tmpf;
}
}
mat_trans((F*)B, n);
}
/* Transpose matrix B to increase cache hits. */
void mat_mul_cpu_trans(const F *A, const F *B, F *C, size_t n, Cache *cache) {
F tmp;
size_t i, j, k;
UNUSED(cache);
mat_trans((F*)B, n);
for (i = 0; i < n; ++i) {
for (j = 0; j < n; ++j) {
tmp = 0.0;
for (k = 0; k < n; ++k) {
tmp += A[i*n+k] * B[j*n+k];
}
C[i*n+j] = tmp;
}
}
mat_trans((F*)B, n);
}
/*!*****************************************************************************
*******************************************************************************
\note my_inv_ludcmp_gen
\date Nov 2008 (Modified by Mrinal)
\remarks
uses the LUDCMP to invert a square matrix.
Also calculates the determinant if needed
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] mat : matrix to be inverted
\param[in] size : mat is a size x size matrix
\param[out] inv_mat : inverse of this matrix
\param[in] calculate_determinant : one of CALC_NO_DET, CALC_DET, CALC_DET_ONLY
\param[out] det : determinant of the matrix (if requested)
note: - return code FALSE is given if not invertable
- mat and inv_mat can be the same matrices; the program takes
care of this
- matrices are according to numerical recipes convention
******************************************************************************/
int
my_inv_ludcmp_gen(double **mat, int size, double **inv_mat, int calculate_determinant, double* det)
{
int i,j,n;
int rc = 1;
double info;
MY_MATRIX(aux, 1, size, 1, size);
MY_IVECTOR(indx, 1, size);
for (i=1; i<=size; ++i) for (j=1; j<=size; ++j) aux[i][j]=mat[i][j];
if (!my_ludcmp(aux,size,indx,&info)) {
rc = 0;
} else {
if (calculate_determinant != CALC_DET_ONLY) {
/* make zuu_inv the identity matrix */
for (i=1; i<=size; ++i) {
for (j=1; j<=size; ++j) {
inv_mat[i][j] = 0.0;
if (i==j) inv_mat[i][j] = 1.0;
}
}
/* now generate the inverse. Due to the setup of matrices, I will
actuall create the transpose of the inverse */
for (i=1; i<=size; ++i) {
my_lubksb(aux, size, indx, inv_mat[i]);
}
/* now take the transpose of this inverse to make it what I want */
mat_trans(inv_mat, inv_mat);
}
if (calculate_determinant && det!=NULL) {
calculate_determinant = CALC_NO_DET;
*det = info;
for (i=1; i<=size; ++i) *det *= aux[i][i];
}
}
return rc;
}
std::pair<unsigned int, Real>
EigenSparseLinearSolver<T>::adjoint_solve (SparseMatrix<T> &matrix_in,
NumericVector<T> &solution_in,
NumericVector<T> &rhs_in,
const double tol,
const unsigned int m_its)
{
START_LOG("adjoint_solve()", "EigenSparseLinearSolver");
libmesh_experimental();
EigenSparseMatrix<T> mat_trans(this->comm());
matrix_in.get_transpose(mat_trans);
std::pair<unsigned int, Real> retval = this->solve (mat_trans,
solution_in,
rhs_in,
tol,
m_its);
STOP_LOG("adjoint_solve()", "EigenSparseLinearSolver");
return retval;
}
/*!*****************************************************************************
*******************************************************************************
\note compute_inertial
\date Oct 2005
\remarks
This functions computes simulated inertial signals, i.e., body
quaternion, angular velocity, and linear acceleration, and
linear acceleration at the feet
*******************************************************************************
Function Parameters: [in]=input,[out]=output
All results can be computed based on global variables. Results are
assigned to the misc_sensor structures.
******************************************************************************/
static void
compute_inertial(void)
{
int i,j;
static int firsttime = TRUE;
static Matrix R;
static Matrix RT;
static Vector ad;
static Vector xdd;
static Vector r;
static Vector r0;
static Vector t1;
static Vector t2;
static Vector t3;
static Vector lv;
static Vector rv;
static Vector lv_last;
static Vector rv_last;
static Vector lacc;
static Vector racc;
if (firsttime) {
R = my_matrix(1,N_CART,1,N_CART);
RT = my_matrix(1,N_CART,1,N_CART);
ad = my_vector(1,N_CART);
xdd = my_vector(1,N_CART);
r = my_vector(1,N_CART);
r0 = my_vector(1,N_CART);
t1 = my_vector(1,N_CART);
t2 = my_vector(1,N_CART);
t3 = my_vector(1,N_CART);
lv = my_vector(1,N_CART);
rv = my_vector(1,N_CART);
lacc= my_vector(1,N_CART);
racc= my_vector(1,N_CART);
lv_last = my_vector(1,N_CART);
rv_last = my_vector(1,N_CART);
firsttime = FALSE;
}
// copy quaternion from base coordinates (perfect data)
misc_sim_sensor[B_Q0_IMU] = base_orient.q[_Q0_];
misc_sim_sensor[B_Q1_IMU] = base_orient.q[_Q1_];
misc_sim_sensor[B_Q2_IMU] = base_orient.q[_Q2_];
misc_sim_sensor[B_Q3_IMU] = base_orient.q[_Q3_];
// the angular vel. and translatory acc are first computed in world coordinates
// and then transformed to base coordinates.
// need the base coordinate rotatin matrix, which transforms world to base
quatToRotMat(&base_orient,R);
mat_trans(R,RT);
// offset is IMU_X_OFFSET and -IMU_Y_OFFSET
r[_X_] = IMU_X_OFFSET;
r[_Y_] = -IMU_Y_OFFSET;
r[_Z_] = 0.0;
// get offset vector in global coordinates
mat_vec_mult(RT,r,r0);
// w x r0
vec_mult_outer_size(base_orient.ad,r0,N_CART,t1);
// w x (w x r0) = w x t1 (which is one term of xdd)
vec_mult_outer_size(base_orient.ad,t1,N_CART,t2);
// wd x r0
vec_mult_outer_size(base_orient.add,r0,N_CART,t1);
// add to xdd
vec_add(t1,t2,t1);
// and add the base acceleration
vec_add_size(t1,base_state.xdd,N_CART,t1);
// add gravity to t1
t1[_Z_] -= gravity;
// rotate all into base coordinates
mat_vec_mult_size(R,N_CART,N_CART,base_orient.ad,N_CART,t3);
mat_vec_mult_size(R,N_CART,N_CART,t1,N_CART,t2);
// and rotate this information into IMU coordinates
// IMU_X = -Z_BASE; IMU_Y = -X_BASE; IMU_Z = Y_BASE
t1[_A_] = -PI/2.;
t1[_B_] = 0.0;
t1[_G_] = PI/2.;
eulerToRotMat(t1,R);
mat_vec_mult(R,t2,xdd);
mat_vec_mult(R,t3,ad);
// the final result
misc_sim_sensor[B_AD_A_IMU] = ad[_A_];
misc_sim_sensor[B_AD_B_IMU] = ad[_B_];
misc_sim_sensor[B_AD_G_IMU] = ad[_G_];
misc_sim_sensor[B_XACC_IMU] = xdd[_X_];
misc_sim_sensor[B_YACC_IMU] = xdd[_Y_];
misc_sim_sensor[B_ZACC_IMU] = xdd[_Z_];
// now get the foot accelerations in WORLD coordinate, computed at the L_FOOT position,
// which is not exactly the same as the load cell offset, but very close.
computeLinkVelocity(L_FOOT, link_pos_sim, joint_origin_pos_sim,
joint_axis_pos_sim, joint_sim_state, lv);
computeLinkVelocity(R_FOOT, link_pos_sim, joint_origin_pos_sim,
joint_axis_pos_sim, joint_sim_state, rv);
for (i=1; i<=N_CART; ++i) {
lacc[i] = (lv[i]-lv_last[i])*(double)simulation_servo_rate;
lv_last[i] = lv[i];
racc[i] = (rv[i]-rv_last[i])*(double)simulation_servo_rate;
rv_last[i] = rv[i];
}
// transform to local foot coordinates after adding gravity
lacc[_Z_] -= gravity;
racc[_Z_] -= gravity;
// rotation matrix from world to L_AAA coordinates
mat_trans_size(Alink_sim[L_FOOT],N_CART,N_CART,R);
mat_vec_mult(R,lacc,t1);
// rotation matrix from world to R_AAA coordinates
mat_trans_size(Alink_sim[R_FOOT],N_CART,N_CART,R);
mat_vec_mult(R,racc,t2);
#ifdef HAS_LOWER_BODY
// the final result
misc_sim_sensor[L_FOOT_XACC] = t1[_X_];
misc_sim_sensor[L_FOOT_YACC] = t1[_Y_];
misc_sim_sensor[L_FOOT_ZACC] = t1[_Z_];
misc_sim_sensor[R_FOOT_XACC] = t2[_X_];
misc_sim_sensor[R_FOOT_YACC] = t2[_Y_];
misc_sim_sensor[R_FOOT_ZACC] = t2[_Z_];
#endif
}
/*!*****************************************************************************
*******************************************************************************
\note parm_opt
\date 10/20/91
\remarks
this is the major optimzation program
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] tol : error tolernance to be achieved
\param[in] n_parm : number of parameters to be optimzed
\param[in] n_con : number of contraints to be taken into account
\param[in] f_dLda : function which calculates the derivative of the
optimziation criterion with respect to the parameters;
must return vector
\param[in] f_dMda : function which calculates the derivate of the
constraints with respect to parameters
must return matrix
\param[in] f_M : constraints function, must always be formulted to
return 0 for properly fulfilled constraints
\param[in] f_L : function to calculate simple cost (i.e., constraint
cost NOT included), the constraint costs are added
by this program automatically, the function returns
a double scalar
\param[in] f_dLdada : second derivative of L with respect to the parameters,
must be a matrix of dim n_parm x n_parm
\param[in] f_dMdada : second derivative of M with respect to parameters,
must be a matrix n_con*n_parm x n_parm
\param[in] use_newton: TRUE or FALSE to indicate that second derivatives
are given and can be used for Newton algorithm
\param[in,out] a : initial setting of parameters and also return of
optimal value (must be a vector, even if scalar)
\param[out] final_cost: the final cost
\param[out] err : the sqrt of the squared error of all constraints
NOTE: - program returns TRUE if everything correct, otherwise FALSE
- always minimizes the cost!!!
- algorithms come from Dyer McReynolds
NOTE: besides the possiblity of a bug, the Newton method seems to
sacrifice the validity of the constraint a little up to
quite a bit and should be used prudently
******************************************************************************/
int
parm_opt(double *a,int n_parm, int n_con, double *tol, void (*f_dLda)(),
void (*f_dMda)(), void (*f_M)(), double (*f_L)(), void (*f_dMdada)(),
void (*f_dLdada)(), int use_newton, double *final_cost, double *err)
{
register int i,j,n;
double cost= 999.e30;
double last_cost = 0.0;
double *mult=NULL, *new_mult=NULL; /* this is the vector of Lagrange mulitplier */
double **dMda=NULL, **dMda_t=NULL;
double *dLda;
double *K=NULL; /* the error in the constraints */
double eps = 0.025; /* the learning rate */
double **aux_mat=NULL; /* needed for inversion of matrix */
double *aux_vec=NULL;
double *new_a;
double **dMdada=NULL;
double **dLdada=NULL;
double **A=NULL; /* big matrix, a combination of several other matrices */
double *B=NULL; /* a big vector */
double **A_inv=NULL;
int rc=TRUE;
long count = 0;
int last_sign = 1;
int pending1 = FALSE, pending2 = FALSE;
int firsttime = TRUE;
int newton_active = FALSE;
dLda = my_vector(1,n_parm);
new_a = my_vector(1,n_parm);
if (n_con > 0) {
mult = my_vector(1,n_con);
dMda = my_matrix(1,n_con,1,n_parm);
dMda_t = my_matrix(1,n_parm,1,n_con);
K = my_vector(1,n_con);
aux_mat = my_matrix(1,n_con,1,n_con);
aux_vec = my_vector(1,n_con);
}
if (use_newton) {
dLdada = my_matrix(1,n_parm,1,n_parm);
A = my_matrix(1,n_parm+n_con,1,n_parm+n_con);
A_inv = my_matrix(1,n_parm+n_con,1,n_parm+n_con);
B = my_vector(1,n_parm+n_con);
if (n_con > 0) {
dMdada = my_matrix(1,n_con*n_parm,1,n_parm);
new_mult = my_vector(1,n_con);
}
for (i=1+n_parm; i<=n_con+n_parm; ++i) {
for (j=1+n_parm; j<=n_con+n_parm; ++j) {
A[i][j] = 0.0;
}
}
}
while (fabs(cost-last_cost) > *tol) {
++count;
pending1 = FALSE;
pending2 = FALSE;
AGAIN:
/* calculate the current Lagrange multipliers */
if (n_con > 0) {
(*f_M)(a,K); /* takes the parameters, returns residuals */
(*f_dMda)(a,dMda); /* takes the parameters, returns the Jacobian */
}
(*f_dLda)(a,dLda); /* takes the parameters, returns the gradient */
if (n_con > 0) {
mat_trans(dMda,dMda_t);
}
if (newton_active) {
if (n_con > 0) {
(*f_dMdada)(a,dMdada);
}
(*f_dLdada)(a,dLdada);
}
/* the first step is always a gradient step */
if (newton_active) {
if (firsttime) {
firsttime = FALSE;
eps = 0.1;
}
/* build the A matrix */
for (i=1; i<=n_parm; ++i) {
for (j=1; j<=n_parm; ++j) {
A[i][j] = dLdada[i][j];
for (n=1; n<=n_con; ++n) {
A[i][j] += mult[n]*dMdada[n+(i-1)*n_con][j];
}
}
}
for (i=1+n_parm; i<=n_con+n_parm; ++i) {
for (j=1; j<=n_parm; ++j) {
A[j][i] = A[i][j] = dMda[i-n_parm][j];
}
}
/* build the B vector */
if (n_con > 0) {
mat_vec_mult(dMda_t,mult,B);
}
for (i=1; i<=n_con; ++i) {
B[i+n_parm] = K[i];
}
/* invert the A matrix */
if (!my_inv_ludcmp(A, n_con+n_parm, A_inv)) {
rc = FALSE;
break;
}
mat_vec_mult(A_inv,B,B);
vec_mult_scalar(B,eps,B);
for (i=1; i<=n_parm; ++i) {
new_a[i] = a[i] + B[i];
}
for (i=1; i<=n_con; ++i) {
new_mult[i] = mult[i] + B[n_parm+i];
}
} else {
if (n_con > 0) {
/* the mulitpliers are updated according:
mult = (dMda dMda_t)^(-1) (K/esp - dMda dLda_t) */
mat_mult(dMda,dMda_t,aux_mat);
if (!my_inv_ludcmp(aux_mat, n_con, aux_mat)) {
rc = FALSE;
break;
}
mat_vec_mult(dMda,dLda,aux_vec);
vec_mult_scalar(K,1./eps,K);
vec_sub(K,aux_vec,aux_vec);
mat_vec_mult(aux_mat,aux_vec,mult);
}
/* the update step looks the following:
a_new = a - eps * (dLda + mult_t * dMda)_t */
if (n_con > 0) {
vec_mat_mult(mult,dMda,new_a);
vec_add(dLda,new_a,new_a);
} else {
vec_equal(dLda,new_a);
}
vec_mult_scalar(new_a,eps,new_a);
vec_sub(a,new_a,new_a);
}
if (count == 1 && !pending1) {
last_cost = (*f_L)(a);
if (n_con > 0) {
(*f_M)(a,K);
last_cost += vec_mult_inner(K,mult);
}
} else {
last_cost = cost;
}
/* calculate the updated cost */
cost = (*f_L)(new_a);
/*printf(" %f\n",cost);*/
if (n_con > 0) {
(*f_M)(new_a,K);
if (newton_active) {
cost += vec_mult_inner(K,new_mult);
} else {
cost += vec_mult_inner(K,mult);
}
}
/* printf("last=%f new=%f\n",last_cost,cost); */
/* check out whether we reduced the cost */
if (cost > last_cost && fabs(cost-last_cost) > *tol) {
/* reduce the gradient climbing rate: sometimes a reduction of eps
causes an increase in cost, thus leave an option to increase
eps */
cost = last_cost; /* reset last_cost */
if (pending1 && pending2) {
/* this means that either increase nor decrease
of eps helps, ==> leave the program */
rc = TRUE;
break;
} else if (pending1) {
eps *= 4.0; /* the last cutting by half did not help, thus
multiply by 2 to get to previous value, and
one more time by 2 to get new value */
pending2 = TRUE;
} else {
eps /= 2.0;
pending1 = TRUE;
}
goto AGAIN;
} else {
vec_equal(new_a,a);
if (newton_active && n_con > 0) {
vec_equal(new_mult,mult);
}
if (use_newton && fabs(cost-last_cost) < NEWTON_THRESHOLD)
newton_active = TRUE;
}
}
my_free_vector(dLda,1,n_parm);
my_free_vector(new_a,1,n_parm);
if (n_con > 0) {
my_free_vector(mult,1,n_con);
my_free_matrix(dMda,1,n_con,1,n_parm);
my_free_matrix(dMda_t,1,n_parm,1,n_con);
my_free_vector(K,1,n_con);
my_free_matrix(aux_mat,1,n_con,1,n_con);
my_free_vector(aux_vec,1,n_con);
}
if (use_newton) {
my_free_matrix(dLdada,1,n_parm,1,n_parm);
my_free_matrix(A,1,n_parm+n_con,1,n_parm+n_con);
my_free_matrix(A_inv,1,n_parm+n_con,1,n_parm+n_con);
my_free_vector(B,1,n_parm+n_con);
if (n_con > 0) {
my_free_matrix(dMdada,1,n_con*n_parm,1,n_parm);
my_free_vector(new_mult,1,n_con);
}
}
*final_cost = cost;
*tol = fabs(cost-last_cost);
if (n_con > 0) {
*err = sqrt(vec_mult_inner(K,K));
} else {
*err = 0.0;
}
/*
printf("count=%ld rc=%d\n",count,rc);
*/
return rc;
}
/// render
void view::render() {
// Clear Screen And Depth Buffer
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
if (!m_world) {
return;
}
if(ortho_sh)
{
glClearColor(0,0,0,1);
modifiedmesh_drawable->set_render_faces(true);
modifiedmesh_drawable->set_render_edges(false);
modifiedmesh_drawable->set_render_normals(false);
modifiedmesh_drawable->set_render_vertices(false);
modifiedmesh_drawable->render();
glFlush();
int size = w()*h()*3;
unsigned char *data = new unsigned char[size];
glReadPixels(0,0, w(), h(),GL_RGB,GL_UNSIGNED_BYTE,data);
ilInit();
iluInit();
ilutInit();
ILuint ImageName;
ilGenImages(1, &ImageName);
ilBindImage(ImageName);
ilTexImage(w(),h(),1,3,IL_RGB,IL_UNSIGNED_BYTE,data);
//iluFlipImage();
ilEnable(IL_FILE_OVERWRITE);
#ifdef _WIN32
// Did not compile in linux:
ilSaveImage(reinterpret_cast<const wchar_t *>(filename_screenshot.c_str()));
#else
ilSaveImage(filename_screenshot.c_str());
#endif
delete[] data;
glClearColor(0.80f, 0.80f, 0.90f, 1.0f);
}
else
{
// Reset Modelview:
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glEnable(GL_BLEND);
glEnable(GL_DEPTH_TEST);
// Set modelviewmatrix
m = translate_44(0.0f, 0.0f, -cam_distance)
* rotate_x_44(cam_polar)
* rotate_y_44(cam_azimut)
* translate_44(-cam_target.x(), -cam_target.y(), -cam_target.z() );
// Transpose (here: invert) matrix:
Mat44f mat_trans = transpose(m);
point3f camera_pos(0.0, 0.0, 0.0);
camera_pos = invert(m) * camera_pos;
glLoadMatrixf( &mat_trans(0,0) );
// Create light components
GLfloat ambientLight[] = { 0.0f, 0.0f, 0.0f, 1.0f };
GLfloat diffuseLight[] = { 1.0f, 1.0f, 1.0f, 1.0f };
GLfloat specularLight[] = { 1.0f, 1.0f, 1.0f, 1.0f };
GLfloat position[] = { 1.0f, 1.0f, 1.0f, 0.0f };
GLfloat globalAmbient[] = { 0.2f, 0.2f, 0.2f, 1.0f};
glLightModelfv(GL_LIGHT_MODEL_AMBIENT, globalAmbient);
// Assign created components to GL_LIGHT1
glLightfv(GL_LIGHT0, GL_AMBIENT, ambientLight);
glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuseLight);
glLightfv(GL_LIGHT0, GL_SPECULAR, specularLight);
glLightfv(GL_LIGHT0, GL_POSITION, position);
Rendering::RenderingInfo info( camera_pos, m_noise_tex );
/* render the world */
m_world->render( info );
renderCoordinateSystem();
//render_noise_texture();
}
}
mat mars(Agraph_t* g, struct marsopts opts)
{
int i, j, n = agnnodes(g), k = MIN(n, MAX(opts.k, 2)), iter = 0;
mat dij, u, u_trans, q, r, q_t, tmp, tmp2, z;
double* s = (double*) malloc(sizeof(double)*k);
double* ones = (double*) malloc(sizeof(double)*n);
double* d;
int* anchors = (int*) malloc(sizeof(int)*k);
int* clusters = NULL;
double change = 1, old_stress = -1;
dij = mat_new(k, n);
u = mat_new(n,k);
tmp = mat_new(n,k);
darrset(ones,n,-1);
select_anchors(g, dij, anchors, k);
if(opts.color) {
for(i = 0; i < k; i++) {
Agnode_t* anchor = get_node(anchors[i]);
agset(anchor, "color", "red");
}
}
if(opts.power != 1) {
clusters = graph_cluster(g,dij,anchors);
}
singular_vectors(g, dij, opts.power, u, s);
vec_scalar_mult(s, k, -1);
u_trans = mat_trans(u);
d = mat_mult_for_d(u, s, u_trans, ones);
for(i = 0; i < u->c; i++) {
double* col = mat_col(u,i);
double* b = inv_mul_ax(d,col,u->r);
for(j = 0; j < u->r; j++) {
tmp->m[mindex(j,i,tmp)] = b[j];
}
free(b);
free(col);
}
tmp2 = mat_mult(u_trans,tmp);
for(i = 0; i < k; i++) {
tmp2->m[mindex(i,i,tmp2)] += (1.0/s[i]);
}
q = mat_new(tmp2->r, tmp2->c);
r = mat_new(tmp2->c, tmp2->c);
qr_factorize(tmp2,q,r);
q_t = mat_trans(q);
if(opts.given) {
z = get_positions(g, opts.dim);
} else {
z = mat_rand(n, opts.dim);
}
translate_by_centroid(z);
if(opts.viewer) {
init_viewer(g, opts.max_iter);
append_layout(z);
}
old_stress = stress(z, dij, anchors, opts.power);
while(change > EPSILON && iter < opts.max_iter) {
mat right_side;
double new_stress;
if(opts.power == 1) {
right_side = barnes_hut(z);
} else {
right_side = barnes_hut_cluster(z, dij, clusters, opts.power);
}
for(i = 0; i < opts.dim; i++) {
double sum = 0;
double* x;
double* b = mat_col(right_side,i);
for(j = 0; j < right_side->r; j++) {
sum += b[j];
}
x = inv_mul_full(d, b, right_side->r, u, u_trans, q_t, r);
for(j = 0; j < z->r; j++) {
z->m[mindex(j,i,z)] = x[j] - sum/right_side->r;
}
free(x);
free(b);
}
adjust_anchors(g, anchors, k, z);
update_anchors(z, dij, anchors, opts.power);
translate_by_centroid(z);
if(opts.viewer) {
append_layout(z);
}
new_stress = stress(z, dij, anchors, opts.power);
change = fabs(new_stress-old_stress)/old_stress;
old_stress = new_stress;
mat_free(right_side);
iter++;
}
mat_free(dij);
mat_free(u);
mat_free(u_trans);
mat_free(q);
mat_free(r);
mat_free(q_t);
mat_free(tmp);
mat_free(tmp2);
free(s);
free(ones);
free(d);
free(anchors);
free(clusters);
return z;
}
void
test_constraint(void)
{
int i,j,n,m;
static Matrix Jbig;
static Matrix dJbigdt;
static Vector dsbigdt;
static Matrix JbigMinvJbigt;
static Matrix JbigMinvJbigtinv;
static Matrix Minv;
static Matrix MinvJbigt;
static Matrix Jbigt;
static Vector dJbigdtdsbigdt;
static Matrix Jbart;
static Matrix Jbar;
static Vector lv;
static Vector lambda;
static Vector f;
static Matrix R;
static Vector v;
static int firsttime = TRUE;
int debug_print = TRUE;
if (firsttime) {
firsttime = FALSE;
Jbig = my_matrix(1,N_ENDEFFS*6,1,N_DOFS+6);
Jbigt = my_matrix(1,N_DOFS+6,1,N_ENDEFFS*6);
dJbigdt = my_matrix(1,N_ENDEFFS*6,1,N_DOFS+6);
JbigMinvJbigt = my_matrix(1,N_ENDEFFS*6,1,N_ENDEFFS*6);
JbigMinvJbigtinv = my_matrix(1,N_ENDEFFS*6,1,N_ENDEFFS*6);
Minv = my_matrix(1,N_DOFS+6,1,N_DOFS+6);
MinvJbigt = my_matrix(1,N_DOFS+6,1,N_ENDEFFS*6);
dsbigdt = my_vector(1,N_DOFS+6);
dJbigdtdsbigdt = my_vector(1,N_ENDEFFS*6);
Jbar = my_matrix(1,N_DOFS+6,1,N_ENDEFFS*6);
Jbart = my_matrix(1,N_ENDEFFS*6,1,N_DOFS+6);
lv = my_vector(1,N_ENDEFFS*6);
lambda = my_vector(1,N_ENDEFFS*6);
f = my_vector(1,N_DOFS+6);
R = my_matrix(1,N_CART,1,N_CART);
v = my_vector(1,N_CART);
// the base part of Jbig is just the identity matrix for both constraints
for (i=1; i<=6; ++i)
Jbig[i][N_DOFS+i] = 1.0;
for (i=1; i<=6; ++i)
Jbig[i+6][N_DOFS+i] = 1.0;
}
// build the Jacobian including the base -- just copy J into Jbig
mat_equal_size(J,N_ENDEFFS*6,N_DOFS,Jbig);
mat_trans(Jbig,Jbigt);
// build the Jacobian derivative
mat_equal_size(dJdt,N_ENDEFFS*6,N_DOFS,dJbigdt);
// build the augmented state derivate vector
for (i=1; i<=N_DOFS; ++i)
dsbigdt[i] = joint_state[i].thd;
for (i=1; i<=N_CART; ++i)
dsbigdt[N_DOFS+i] = base_state.xd[i];
for (i=1; i<=N_CART; ++i)
dsbigdt[N_DOFS+N_CART+i] = base_orient.ad[i];
// compute J-bar transpose
my_inv_ludcmp(rbdInertiaMatrix,N_DOFS+6, Minv);
mat_mult(Minv,Jbigt,MinvJbigt);
mat_mult(Jbig,MinvJbigt,JbigMinvJbigt);
my_inv_ludcmp(JbigMinvJbigt,N_ENDEFFS*6,JbigMinvJbigtinv);
mat_mult(MinvJbigt,JbigMinvJbigtinv,Jbar);
mat_trans(Jbar,Jbart);
// compute C+G-u
for (i=1; i<=N_DOFS; ++i)
f[i] = -joint_state[i].u;
for (i=1; i<=6; ++i)
f[N_DOFS+i] = 0.0;
vec_add(f,rbdCplusGVector,f);
// compute first part of lambda
mat_vec_mult(Jbart,f,lambda);
// compute second part of lambda
mat_vec_mult(dJbigdt,dsbigdt,dJbigdtdsbigdt);
mat_vec_mult(JbigMinvJbigtinv,dJbigdtdsbigdt,lv);
// compute final lambda
vec_sub(lambda,lv,lambda);
// ----------------------------------------------------------------------
// express lambda in the same coordinate at the foot sensors
/* rotation matrix from world to L_AAA coordinates:
we can borrow this matrix from the toes, which have the same
rotation, but just a different offset vector, which is not
needed here */
mat_trans_size(Alink[L_IN_HEEL],N_CART,N_CART,R);
// transform forces
for (i=1; i<=N_CART; ++i)
v[i] = lambda[N_CART*2+i];
mat_vec_mult(R,v,v);
if (debug_print) {
printf("LEFT Force: lx=% 7.5f sx=% 7.5f\n",v[1],misc_sim_sensor[L_CFx]);
printf("LEFT Force: ly=% 7.5f sy=% 7.5f\n",v[2],misc_sim_sensor[L_CFy]);
printf("LEFT Force: lz=% 7.5f sz=% 7.5f\n",v[3],misc_sim_sensor[L_CFz]);
}
misc_sensor[L_CFx] = v[1];
misc_sensor[L_CFy] = v[2];
misc_sensor[L_CFz] = v[3];
// transform torques
for (i=1; i<=N_CART; ++i)
v[i] = lambda[N_CART*2+N_CART+i];
mat_vec_mult(R,v,v);
if (debug_print) {
printf("LEFT Torque: lx=% 7.5f sx=% 7.5f\n",v[1],misc_sim_sensor[L_CTa]);
printf("LEFT Torque: ly=% 7.5f sy=% 7.5f\n",v[2],misc_sim_sensor[L_CTb]);
printf("LEFT Torque: lz=% 7.5f sz=% 7.5f\n",v[3],misc_sim_sensor[L_CTg]);
}
misc_sensor[L_CTa] = v[1];
misc_sensor[L_CTb] = v[2];
misc_sensor[L_CTg] = v[3];
/* rotation matrix from world to R_AAA coordinates :
we can borrow this matrix from the toes, which have the same
rotation, but just a different offset vector, which is not
needed here */
mat_trans_size(Alink[R_IN_HEEL],N_CART,N_CART,R);
// transform forces
for (i=1; i<=N_CART; ++i)
v[i] = lambda[i];
mat_vec_mult(R,v,v);
if (debug_print) {
printf("RIGHT Force: lx=% 7.5f sx=% 7.5f\n",v[1],misc_sim_sensor[R_CFx]);
printf("RIGHT Force: ly=% 7.5f sy=% 7.5f\n",v[2],misc_sim_sensor[R_CFy]);
printf("RIGHT Force: lz=% 7.5f sz=% 7.5f\n",v[3],misc_sim_sensor[R_CFz]);
}
misc_sensor[R_CFx] = v[1];
misc_sensor[R_CFy] = v[2];
misc_sensor[R_CFz] = v[3];
// transform torques
for (i=1; i<=N_CART; ++i)
v[i] = lambda[N_CART+i];
mat_vec_mult(R,v,v);
if (debug_print) {
printf("RIGHT Torque: lx=% 7.5f sx=% 7.5f\n",v[1],misc_sim_sensor[R_CTa]);
printf("RIGHT Torque: ly=% 7.5f sy=% 7.5f\n",v[2],misc_sim_sensor[R_CTb]);
printf("RIGHT Torque: lz=% 7.5f sz=% 7.5f\n",v[3],misc_sim_sensor[R_CTg]);
}
misc_sensor[R_CTa] = v[1];
misc_sensor[R_CTb] = v[2];
misc_sensor[R_CTg] = v[3];
if (debug_print)
getchar();
}
//#define WINDOWED
int main()
{
SDL_Init(SDL_INIT_VIDEO);
SDL_VideoInit(NULL);
SDL_GL_SetAttribute(SDL_GL_CONTEXT_MAJOR_VERSION, 3);
SDL_GL_SetAttribute(SDL_GL_CONTEXT_MINOR_VERSION, 2);
SDL_GL_SetAttribute(SDL_GL_MULTISAMPLEBUFFERS, 1);
SDL_GL_SetAttribute(SDL_GL_MULTISAMPLESAMPLES, 8);
#ifdef WINDOWED
SDL_Window *win = SDL_CreateWindow("hello world", 0, 0,640, 480, SDL_WINDOW_OPENGL);
#define ASPECT 640.0/480
#else
SDL_Window *win = SDL_CreateWindow("hello world", 0, 0,2560, 1440, SDL_WINDOW_OPENGL|SDL_WINDOW_FULLSCREEN);
#define ASPECT 2560.0/1440
#endif
SDL_ShowWindow(win);
SDL_GLContext context_dontneedtosavethis = SDL_GL_CreateContext(win);
SDL_GL_SetSwapInterval(0);
#ifdef WINDOWED
sm_screensize(640,480);
// glViewport(0,0,640,480);
#else
sm_screensize(2560,1440);
// glViewport(0,0,1366,768);
#endif
glEnable(GL_DEPTH_TEST);
glEnable(GL_BLEND);
glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glClearColor(0.3,0.5,1,0);
glClear(GL_COLOR_BUFFER_BIT);
if(sm_load("example"))return;
smm_open("examplemodel.tmd");
GLuint examplemodelvao = smm_upload();
int examplemodelnumtris =smm_numtris();
//========================feedback==============================
if(sm_load("examplefeedback"))return;
smm_open("exampleinstancing.tmd");//load the model to be instanced
GLuint instancedobjectsvao = smm_upload();
int instancedobjectsnumtris = smm_numtris();
glBindVertexArray(instancedobjectsvao);
GLuint instancedobjectspositionvbo;
glGenBuffers(1, &instancedobjectspositionvbo);//this is where we'll put the position offsets generated by the examplefeedback shader
glBindBuffer(GL_ARRAY_BUFFER, instancedobjectspositionvbo);
glBufferData(GL_ARRAY_BUFFER, sizeof(float)*3*500, NULL, GL_DYNAMIC_COPY);//allocate memory on gpu for 500 position vectors
glEnableVertexAttribArray(2);//enables the third attribute (offset)
glVertexAttribPointer(2, 3, GL_FLOAT, GL_FALSE, sizeof(float)*3, 0);//associates the attribute with the location and with the active vbo (instancedobjectspositionvbo)
glVertexAttribDivisor(2, 1);//set attribute 2 (offset) to change per instance instead of per vertex
glBindBuffer(GL_ARRAY_BUFFER, 0);
GLuint feedbackquery;//to count how many instanced objects got through the geometry shader (if used)
glGenQueries(1, &feedbackquery);
//========================/feedback==============================
if(sm_load("examplepp"))return;
if(sm_load("drawtext"))return;
unsigned char printtext[100] = {0};
GLuint textvao, textvbo;//manually setup the vao
glGenVertexArrays(1,&textvao);
glBindVertexArray(textvao);
glGenBuffers(1,&textvbo);
glBindBuffer(GL_ARRAY_BUFFER, textvbo);
glBufferData(GL_ARRAY_BUFFER, /*sizeof(vertexdata)*/strlen(printtext), printtext, GL_STREAM_DRAW);
glEnableVertexAttribArray(0);
glVertexAttribPointer(0, 1, GL_UNSIGNED_BYTE, GL_FALSE, 0, 0);//associates first attribute with vertexdata, also sets source to vbo
float modelviewproj[16];
mat_identity(modelviewproj);
mat_proj2(modelviewproj, 1.7453, ASPECT, 300, 0.1);
long tstart = SDL_GetTicks();
int i;
float rot =0;
float transx=0,transy=-5,transz=5;
float rotmat[16];
float wrot = 0;
float wroty = 0;
float speed = 0.001;
mat_identity(rotmat);
//SDL_SetRelativeMouseMode(SDL_TRUE);//this is not supported on all platforms, warping the mouse is more portable
for(i=0;i>=0;i++)
{
sm_use("example");
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
glBindVertexArray(examplemodelvao);
glDrawElements(GL_TRIANGLES, examplemodelnumtris*3, GL_UNSIGNED_SHORT, NULL);
sm_use("examplefeedback");
float wavetime = SDL_GetTicks() * 0.01;
glUniform1fv(sm_uniloc("wavetime"), 1,&wavetime);
glBindBufferBase(GL_TRANSFORM_FEEDBACK_BUFFER, 0, instancedobjectspositionvbo);//set output for transform feedback
glBeginQuery(GL_TRANSFORM_FEEDBACK_PRIMITIVES_WRITTEN, feedbackquery);//start counting
{
glBeginTransformFeedback(GL_POINTS);
{
glDrawArrays(GL_POINTS, 0, 500);//generate 500 position vectors
}glEndTransformFeedback();
}glEndQuery(GL_TRANSFORM_FEEDBACK_PRIMITIVES_WRITTEN);
GLuint feedbacknumgenerated;
glGetQueryObjectuiv(feedbackquery, GL_QUERY_RESULT, &feedbacknumgenerated);//get how many position vectors were generated
sm_use("example");
glBindVertexArray(instancedobjectsvao);
glDrawElementsInstanced(GL_TRIANGLES, instancedobjectsnumtris*3, GL_UNSIGNED_SHORT, NULL, feedbacknumgenerated);//draw the instances
sm_use("examplepp");
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
glDrawArrays(GL_POINTS, 0, 1);//we're using attributeless rendering, so we don't need a vao
sm_use("drawtext");
glDisable(GL_DEPTH_TEST);
glBindVertexArray(textvao);
glEnable(GL_DEPTH_TEST);
if(i%5==0)//calculate framerate every 5 frames
{
static long timesincelast;
memset(printtext, 0, 100);
long timethisframe = timesincelast-SDL_GetTicks();
if(timethisframe != 0)
SDL_itoa(1000*5/timethisframe, printtext, 10);
strcat(printtext, "FPS");
timesincelast = SDL_GetTicks();
glBufferData(GL_ARRAY_BUFFER, strlen(printtext), printtext, GL_STREAM_DRAW);//upload the text as an ordinary string
}
glDisable(GL_DEPTH_TEST);
glDrawArrays(GL_POINTS, 0,strlen(printtext));//draw the text to the screen
glEnable(GL_DEPTH_TEST);
SDL_GL_SwapWindow(win);
SDL_Event evt;
while(SDL_PollEvent(&evt))
{
if(evt.type == SDL_QUIT)
i= -i;
}
const char *keys = SDL_GetKeyboardState(NULL);
speed = 0.1;
if(keys[SDL_SCANCODE_LSHIFT])
speed = 2.8;
if(keys[SDL_SCANCODE_W])
{
transz -=speed*cos(wrot);
transx +=speed*sin(wrot);
}
if(keys[SDL_SCANCODE_A])
{
transz +=speed*cos(wrot-1.570796);
transx -=speed*sin(wrot-1.570796);
}
if(keys[SDL_SCANCODE_S])
{
transz +=speed*cos(wrot);
transx -=speed*sin(wrot);
}
if(keys[SDL_SCANCODE_D])
{
transz -=speed*cos(wrot-1.570796);
transx +=speed*sin(wrot-1.570796);
}
if(keys[SDL_SCANCODE_SPACE])
transy -=speed;
if(keys[SDL_SCANCODE_LCTRL])
transy +=speed;
int mousex, mousey;
//SDL_GetRelativeMouseState(&mousex, &mousey);
SDL_GetMouseState(&mousex, &mousey);
SDL_WarpMouseInWindow(win, 400,400);
mousex = mousex-400;
mousey = mousey-400;
wrot -= mousex/1000.0;
wroty-= mousey/1000.0;
wroty = wroty > 1.57?1.57:wroty<-1.57?-1.57:wroty;
float temp[16];
mat_identity(temp);
mat_mul(temp, modelviewproj);
mat_rot(temp, 1,0,0,wroty);
mat_rot(temp, 0,1,0,wrot);
mat_trans(temp, transx, transy, transz);
mat_mul(temp, rotmat);
mat_scale(temp, 10, 10, 10);
sm_use("example");
glUniformMatrix4fv(sm_uniloc("modelviewproj"), 1, GL_TRUE, temp);//upload the model-view-projection matrix
}
i = -i;
printf("last sdl error:%s\n", SDL_GetError());
printf("vendor:%s\n", glGetString(GL_RENDERER));
}
void main()
{
//programa para utilizacion de funciones
//definicion de variables
int i,j,m,n,sdpa,sdpb,sdsa,sdsb,pro,sum;
int X1[20],X2[20],aX1[20],bX2[20];
int a[20][20],b[20][20],at[20][20],bt[20][20];
cout<<"digite el número de filas y columnas de a\n";
cin>>n;
cout<<"digite el número de filas y columnas de b\n";
cin>>m
;
//leer la matriz a
cout<<"digite los valores de la matriz a\n";
leer_matriz(a,n);
//leer la matriz b
cout<<"digite los valores de la matriz b\n";
leer_matriz(b,m);
//suma de la diagonal principal de la matriz a
sdpa=sum_diag_ppal(a,n);
//suma de la diagonal principal de la matriz b
sdpb=sum_diag_ppal(b,m);
//suma de la diagonal secundaria de a
sdsa=sum_diag_sec(a,n);
//suma de la diagonal secundaria de b
sdsb=sum_diag_sec(b,m);
//matriz transpuesta de a
mat_trans(a,n,at);
//matriz transpuesta de b
mat_trans(b,m,bt);
/*hallar vector X1 tal que c/u de los elementos del vector sea igual a
la suma de cada una de las filas de a*/
for (i=1;i<=n;i++){
sum=0;
for(j=1;j<=n;j++){
sum+=a[i][j];
}
X1[i]=sum;
}
/*hallar vector X2 tal que c/u de los elementos del vector sea igual a
la multiplicacion de las posiciones pares de c/u de las filas de b*/
for(i=1;i<=m;i++){
pro=1;
for(j=1;j<=m;j++){
pro+=b[i][j];
}
X2[i]=pro;
}
//multiplicar vector X1 a la matriz a
mult_vec_mat(a,X1,n,aX1);
//multiplicar vector X2 a la matriz b
mult_vec_mat(b,X2,m,bX2);
//salidas del programa
//escribir matrices a y b
cout<<"la matriz a es:\n";
esc_matriz(a,n);
cout<<"la matriz b es:\n";
esc_matriz(b,m);
//escribir las transpuestas
cout<<"la matriz transpuesta de a es:\n";
esc_matriz(at,n);
cout<<"la matriz transpuesta de b es:\n";
esc_matriz(bt,m);
//escribir los vectores
cout<<"el vector resultante X1:\n";
esc_vector(X1,n);
cout<<"el vector resultante X2:\n";
esc_vector(X2,m);
cout<<"la multiplicacion del vector X1 a la matriz a es:\n";
esc_vector(aX1,n);
cout<<"la multiplicacion del vector X2 a la matriz b es:\n";
esc_vector(bX2,m);
//escribir las sumatorias de las diagonales
cout<<"la sumatoria de la diagonal principal de a es:"<<sdpa<<"\n";
cout<<"la sumatoria de la diagonal principal de b es:"<<sdpb<<"\n";
cout<<"la sumatoria de la diagonal secundaria de a es:"<<sdsa<<"\n";
cout<<"la sumatoria de la diagonal secundaria de b es:"<<sdsb<<"\n";
}
