void m_xpose(float ax,float ay,float az,matrix &m) { m_identity(m); m[0][3] = ax; m[1][3] = ay; m[2][3] = az; }
void readmatrix(unsigned char **file, unsigned char floatbytes,matrix &m) { m_identity(m); for (int x=0;x<=2;x++) for (int y=0;y<=3;y++) m[x][y]=readfloat(file,floatbytes); }
void m_scale(float ax, float ay, float az, matrix &m) { m_identity(m); m[0][0] = ax; m[1][1] = ay; m[2][2] = az; }
//nearest points between two segments given by pi+veci, results given in ratio of vec [0 1] int Reaching::FindSegmentsNearestPoints(cart_vec_t& p1,cart_vec_t& vec1,cart_vec_t& p2,cart_vec_t& vec2, float *nearest_point1, float *nearest_point2, float *dist){ CMatrix3_t mat; CMatrix3_t invmat; cart_vec_t& vec3,tmp,tmp1,tmp2,k,v2; int i; m_identity(mat); v_cross(vec1,vec2,vec3);// check if vec1 and vec2 are not // if(v_squ_length(vec3)<0.00001){ return 0; } v_scale(vec2,-1,v2); m_set_v3_column(vec1,0,mat); m_set_v3_column(v2,1,mat); m_set_v3_column(vec3,2,mat); m_inverse(mat,invmat); v_sub(p2,p1,tmp); v_transform_normal(tmp,invmat,k); for(i=0;i<2;i++){ k[i] = max(min(1,k[i]),0); } v_scale(vec1,k[0],tmp); v_add(p1,tmp,tmp1); v_scale(vec2,k[1],tmp); v_add(p2,tmp,tmp2); v_sub(tmp2,tmp1,tmp); *dist=v_length(tmp); *nearest_point1 = k[0]; *nearest_point2 = k[1]; return 1; }
/** * Test identity matrix. */ void test_m_identity() { matrix_t *I = m_identity(4); printf("I = eye(%d) = \n", I->rows); m_fprint(stdout, I); m_free(I); }
/** * Implementation of the learning rule described in Bell & Sejnowski, * Vision Research, in press for 1997, that contained the natural * gradient (W' * W). * * Bell & Sejnowski hold the patent for this learning rule. * * SEP goes once through the mixed signals X in batch blocks of size B, * adjusting weights W at the end of each block. * * sepout is called every F counts. * * I suggest a learning rate (L) of 0.006, and a block size (B) of * 300, at least for 2->2 separation. When annealing to the right * solution for 10->10, however, L < 0.0001 and B = 10 were most successful. * * @param X "sphered" input matrix * @param W weight matrix * @param B block size * @param L learning rate * @param F interval to print training stats */ void sep96(matrix_t *X, matrix_t *W, int B, double L, int F) { matrix_t *BI = m_identity(X->rows); m_elem_mult(BI, B); int t; for ( t = 0; t < X->cols; t += B ) { int end = (t + B < X->cols) ? t + B : X->cols; matrix_t *W0 = m_copy(W); matrix_t *X_batch = m_copy_columns(X, t, end); matrix_t *U = m_product(W0, X_batch); // compute Y' = 1 - 2 * f(U), f(u) = 1 / (1 + e^(-u)) matrix_t *Y_p = m_initialize(U->rows, U->cols); int i, j; for ( i = 0; i < Y_p->rows; i++ ) { for ( j = 0; j < Y_p->cols; j++ ) { elem(Y_p, i, j) = 1 - 2 * (1 / (1 + exp(-elem(U, i, j)))); } } // compute dW = L * (BI + Y'U') * W0 matrix_t *U_tr = m_transpose(U); matrix_t *dW_temp1 = m_product(Y_p, U_tr); m_add(dW_temp1, BI); matrix_t *dW = m_product(dW_temp1, W0); m_elem_mult(dW, L); // compute W = W0 + dW m_add(W, dW); // print training stats if ( t % F == 0 ) { precision_t norm = m_norm(dW); precision_t angle = m_angle(W0, W); printf("*** norm(dW) = %.4lf, angle(W0, W) = %.1lf deg\n", norm, 180 * angle / M_PI); } // cleanup m_free(W0); m_free(X_batch); m_free(U); m_free(Y_p); m_free(U_tr); m_free(dW_temp1); m_free(dW); } }
void calculateobjhierarchy(scene *actualscene,int parentid, object *parent) { for (int on=0;on<actualscene->objectnum;on++) { object *o=&(actualscene->objects[on]); if (o->data.primitive<100 && o->parent==parentid) { matrix relative,parentmatrix; memcpy(relative,o->xformmatrix,sizeof(matrix)); m_identity(parentmatrix); if (parentid!=-1) { memcpy(parentmatrix,parent->xformmatrix,sizeof(matrix)); matrix i; m_invert(parentmatrix,i); memcpy(parentmatrix,i,sizeof(matrix)); m_mult(relative,parentmatrix,relative); //relative-ban az xformmatrix-ok relativ matrixa m_mult(i,relative,relative); m_mult(parent->currentmatrix,relative,relative); m_mult(parent->xformmatrix,relative,relative); memcpy(parentmatrix,parent->xformmatrix,sizeof(matrix)); } memcpy(o->buffermatrix,o->xformmatrix,sizeof(matrix)); matrix m; m_mult(relative,parentmatrix,m); m_mult(m,o->currentmatrix,m); obj_transform(o,m); if (parentid!=-1) { matrix i; //memcpy(i,o->buffermatrix,sizeof(matrix)); m_invert(o->buffermatrix,i); m_mult(parent->currentmatrix,o->buffermatrix,m); m_mult(m,o->currentmatrix,m); m_mult(i,m,o->currentmatrix); } memcpy(o->xformmatrix,o->buffermatrix,sizeof(matrix)); calculateobjhierarchy(actualscene,o->number,o); } } }
void m_rotate(float ax,float ay,float az,float phi,matrix &m) { matrix m1; vector a; if (ax==0 && ay==0 && az==0) {m_identity(m); return;} v3_make(ax, ay, az, a); v3_normalize(a, a); m_identity(m); m_mults(m, (float)cos(phi), m); m_diadic3(a, a, m1); m_mults(m1, (float)(1-cos(phi)), m1); m_add(m, m1, m); m_cross(a, m1); m_mults(m1, (float)sin(phi), m1); m_add(m, m1, m); m[3][3] = 1.0; }
/** * Compute the ICA weight matrix W_I for an input matrix X. * * @param X mean-subtracted input matrix * @return weight matrix W_I */ matrix_t * run_ica(matrix_t *X) { // compute whitening matrix W_z matrix_t *W_z = sphere(X); matrix_t *X_sph = m_product(W_z, X); // shuffle the columns of X_sph m_shuffle_columns(X_sph); // train the weight matrix W matrix_t *W = m_identity(X->rows); sep96_params_t params[] = { { 50, 0.0005, 5000, 1000 }, { 50, 0.0003, 5000, 200 }, { 50, 0.0002, 5000, 200 }, { 50, 0.0001, 5000, 200 } }; int num_sweeps = sizeof(params) / sizeof(sep96_params_t); int i, j; for ( i = 0; i < num_sweeps; i++ ) { printf("sweep %d: B = %d, L = %lf\n", i + 1, params[i].B, params[i].L); for ( j = 0; j < params[i].N; j++ ) { sep96(X_sph, W, params[i].B, params[i].L, params[i].F); } } // compute W_I = W * W_z matrix_t *W_I = m_product(W, W_z); // cleanup m_free(W_z); m_free(X_sph); m_free(W); return W_I; }
//******************************************************* // Here is a Heat Bath update kernel... // This routine is based on the algorithm presented by Cabbibo and // Marrinari for updating a SU(n) link element by seperately // updating SU(2) subgroups of that matrix using for each the // SU(2) heat bath algorithm of creutz. //******************************************************* void cmhb_kernel( Float *sigma, Float *u) { // LRG.SetInterval(1, -1); #if 1 // This tells which subblock is being updated: int subblock=0; // Here is the product of the link to be updated and the environment, // which is the sum of the six plquettes which contain u. Float u_sigma[MATRIXSIZE]; // Choosing an SU(2) subblock of u_sigma, it can be expressed // in terms of four real numbers r[0] through r[3], where // R = 1*r[0] + I*r_vector DOT sigma_vector, // where sigma_vector refers to the three pauli matrices. Float r[4]; // The subblock of u_sigma will not in general be SU(2), it will // have to be seperated into a real SU(2) part and an imaginary // SU(2), both needed to be rescaled to be SU(2). k is the // rescaling parameter used there. Float k; Float alp; Float R, R_, R__, R___, X, X_; Float C, A, delta; // A boltzman distributed vector which represents an SU(2) matrix. Float bd[4]; // Variables required to get a random 3-vector on a sphere. Float phi, sin_theta, cos_theta, sin_phi; // and a normalization for those vectors Float scale; // Once a boltzman distributed SU(2) matrix has been constructed, // it is necessary to imbed it into an SU(3) matrix (unit on the last // diagonal) // so that it can be multiplied by the origional link. Float alpha[MATRIXSIZE]; Float mtx_r[MATRIXSIZE]; Float mtx_bd[MATRIXSIZE]; for(subblock=0; subblock<=1; subblock++) { // Construct the sum of plaquettes containing the link "u". m_multiply3( u_sigma, u, sigma ); // Sort out the first SU2 subblock of that sum matrix and // taking the upper 2x2 subblock of u_sigma, it can be expressed // as: k_1 ( r[0] + r_vector DOT sigma_vector ) // + k_2 ( s[0] + s_vector DOT sigma_vector ) // extract the SU2 part called r and the coefficiant k. if( subblock==0 ) { r[0] = ( u_sigma[0] + u_sigma[4] ) / 2.; r[1] = ( u_sigma[10] + u_sigma[12] ) / 2.; r[2] = ( u_sigma[1] - u_sigma[3] ) / 2.; r[3] = ( u_sigma[9] - u_sigma[13] ) / 2.; } else { r[0] = ( u_sigma[4] + u_sigma[8] ) / 2.; r[1] = ( u_sigma[14] + u_sigma[16] ) / 2.; r[2] = ( u_sigma[5] - u_sigma[7] ) / 2.; r[3] = ( u_sigma[13] - u_sigma[17] ) / 2.; } k = sqrt( r[0]*r[0] + r[1]*r[1] + r[2]*r[2] + r[3]*r[3] ); r[0] = r[0]/k; r[1] = r[1]/k; r[2] = r[2]/k; r[3] = r[3]/k; // Float lambda = (2. * GJP.Beta() * k) / 3.; // The 3 is the three of SU(3). // Generate the zero-th component of the boltzman distributed // using the Kennedy Pendleton algorithm: Phys Lett 156B (393). int good_z = 0; while( good_z == 0 ) { Float my_pi = 3.14159265358979323846; alp = (2./3.)* GJP.Beta() * k; R__ = absR(LRG.Urand() ); R_ = absR(LRG.Urand() ); R = absR(LRG.Urand() ); R___ = absR(LRG.Urand() ); X = -(log(R ))/alp ; X_ = -(log(R_))/alp ; C = cos( 2*my_pi*R__ )*cos( 2*my_pi*R__ ); A = X * C; delta = X_ + A; if( (R___*R___) <= (1.-delta/2.) ) good_z = 1; } /* --------- For Debugging purposes int good_z = 0; int counter = 0; while( good_z == 0 ) { Float my_pi = 3.14159265358979323846; alp = (2./3.)* GJP.Beta() * k; R__ = absR(0.553422 ); R_ = absR(-0.098343 ); R = absR(-0.873420 ); R___ = absR(0.204563 ); X = -(log(R ))/alp ; X_ = -(log(R_))/alp ; C = cos( 2*my_pi*R__ )*cos( 2*my_pi*R__ ); A = X * C; delta = X_ + A; if(counter++ == 3) good_z = 1; } // ----------------------------------- */ bd[0] = 1 - delta; // Generate points on the surface of a sphere by directly // generating theta and phi with the apropriate weighting. scale = sqrt( 1. - bd[0]*bd[0] ); // Generate the three components of bd on a unit sphere. cos_theta = LRG.Urand(); Float my_pi = 3.14159265358979323846; phi = LRG.Urand()*my_pi; // phi e [-pi,pi) /* //-------------- For Debugging Purposes cos_theta = -0.912342; Float my_pi = 3.14159265358979323846; phi = 0.243134*my_pi; // phi e [-pi,pi) // --------------------------------------------- */ sin_theta = sqrt( 1. - cos_theta*cos_theta ); // sin(Theta) > 0 sin_phi = sqrt( 1. - cos(phi)*cos(phi) ); if( phi < 0 ) sin_phi *= -1; // x = sin(Theta) x cos(Phi) bd[1] = sin_theta * cos(phi); // y = sin(Theta) x sin(Phi) bd[2] = sin_theta * sin_phi; // z = cos(Theta) bd[3] = cos_theta; bd[1]*=scale; bd[2]*=scale; bd[3]*=scale; m_identity( mtx_r ); if(subblock==0) { *(mtx_r + 0) = r[0]; mtx_r[9] = r[3]; *(mtx_r + 1) = r[2]; mtx_r[10] = r[1]; *(mtx_r + 3) =-r[2]; mtx_r[12] = r[1]; *(mtx_r + 4) = r[0]; mtx_r[13] =-r[3]; } else { *(mtx_r + 4) = r[0]; mtx_r[13] = r[3]; *(mtx_r + 5) = r[2]; mtx_r[14] = r[1]; *(mtx_r + 7) =-r[2]; mtx_r[16] = r[1]; *(mtx_r + 8) = r[0]; mtx_r[17] =-r[3]; } m_invert( mtx_r ); m_identity( mtx_bd ); if(subblock==0) { *(mtx_bd + 0) = bd[0]; mtx_bd[9] = bd[3]; *(mtx_bd + 1) = bd[2]; mtx_bd[10] = bd[1]; *(mtx_bd + 3) =-bd[2]; mtx_bd[12] = bd[1]; *(mtx_bd + 4) = bd[0]; mtx_bd[13] =-bd[3]; } else { *(mtx_bd + 4) = bd[0]; mtx_bd[13] = bd[3]; *(mtx_bd + 5) = bd[2]; mtx_bd[14] = bd[1]; *(mtx_bd + 7) =-bd[2]; mtx_bd[16] = bd[1]; *(mtx_bd + 8) = bd[0]; mtx_bd[17] =-bd[3]; } m_multiply3( alpha, mtx_bd, mtx_r ); // Now that the update factor is computed multiply U by that // factor. m_multiply2l( u, alpha ); } #endif }
void getscenestate(scene *pf,float animtimer,int ianim) { for (int o=0; o<pf->objectnum; o++) if (pf->objects[o].data.primitive!=9 && pf->objects[o].data.primitive!=11 && pf->objects[o].data.primitive<100) { m_identity(pf->objects[o].currentmatrix); memset(&pf->objects[o].orient,0,sizeof(orientation)); pf->objects[o].orient.stretch.x=1; pf->objects[o].orient.stretch.y=1; pf->objects[o].orient.stretch.z=1; objanim *oa=findobjanim(pf->objects[o].anims,ianim); if (oa->posx && oa->posx->numkey) { pf->objects[o].orient.position.x=oa->posx->GetKey(animtimer); pf->objects[o].orient.position.y=oa->posy->GetKey(animtimer); pf->objects[o].orient.position.z=oa->posz->GetKey(animtimer); pf->objects[o].orient.rotaxis.x=oa->rotx->GetKey(animtimer); pf->objects[o].orient.rotaxis.y=oa->roty->GetKey(animtimer); pf->objects[o].orient.rotaxis.z=oa->rotz->GetKey(animtimer); pf->objects[o].orient.rotangle=oa->rota->GetKey(animtimer); if (pf->objects[o].orient.rotaxis.x==0 && pf->objects[o].orient.rotaxis.y==0 && pf->objects[o].orient.rotaxis.z==0) { pf->objects[o].orient.rotaxis.x=1; pf->objects[o].orient.rotangle=0; } pf->objects[o].orient.stretch.x=oa->strx->GetKey(animtimer); pf->objects[o].orient.stretch.y=oa->stry->GetKey(animtimer); pf->objects[o].orient.stretch.z=oa->strz->GetKey(animtimer); /*for (int x=0; x<pf->objects[o].polygonnum; x++) { pf->objects[o].polygons[x].color.x=oa->colr->GetKey(animtimer); pf->objects[o].polygons[x].color.y=oa->colg->GetKey(animtimer); pf->objects[o].polygons[x].color.z=oa->colb->GetKey(animtimer); pf->objects[o].polygons[x].color.w=oa->cola->GetKey(animtimer); }*/ pf->objects[o].color[0]=oa->colr->GetKey(animtimer); pf->objects[o].color[1]=oa->colg->GetKey(animtimer); pf->objects[o].color[2]=oa->colb->GetKey(animtimer); pf->objects[o].color[3]=oa->cola->GetKey(animtimer); object *obj=&(pf->objects[o]); matrix m,a,b; m_identity(a); m_identity(b); //m_scale(obj->orient.stretch.x,obj->orient.stretch.y,obj->orient.stretch.z,a); m_scale(obj->orient.stretch.x,obj->orient.stretch.y,obj->orient.stretch.z,b); m_mult(a,b,m); m_identity(a); m_identity(b); //m_rotate(obj->orient.rotaxis.x,obj->orient.rotaxis.y,obj->orient.rotaxis.z,obj->orient.rotangle*(float)radtheta,a); m_rotate(obj->orient.rotaxis.x,obj->orient.rotaxis.y,obj->orient.rotaxis.z,obj->orient.rotangle*(float)radtheta,b); m_mult(a,b,a); m_mult(m,a,m); m_identity(a); m_identity(b); //m_xpose(obj->orient.position.x,obj->orient.position.y,obj->orient.position.z,a); m_xpose(obj->orient.position.x,obj->orient.position.y,obj->orient.position.z,b); m_mult(a,b,a); m_mult(m,a,obj->currentmatrix); } } }
void sm_bone_transform(sm_t *sm,int num,float *matrix) { if(num < 0) m_identity(matrix); else memcpy(matrix,sm->bone[num].matrix,sizeof(float) * 16); }