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);
}
