NDMask::NDMask(const NDShape& shape, const DeviceDescriptor& device/* = DeviceDescriptor::DefaultDevice()*/) : NDMask(shape, AllocateMatrix(shape, device)) { if (shape.NumAxes() > 2) LogicError("NDMask instances with more than 2 axes are currently unsupported"); Clear(); }
Matrix AdjustJacob(Matrix m, int n) /* Kreiert eine Matrix oder vergroessert sie falls noetig auf die Groesse n*/ { static int size; if (m) { if (n > size) FreeMatrix(m); else return (m); } size = n; return (AllocateMatrix(n, n)); }
CPhysicalSAInterface::CPhysicalSAInterface( void ) { m_unk2 = 0; AllocateMatrix(); #if 0 m_velocity.Reset(); m_spin.Reset(); m_vecUnk.Reset(); m_vecUnk2.Reset(); m_vecUnk3.Reset(); m_vecUnk4.Reset(); #endif m_mass = 1; m_turnMass = 1; m_massUnk = 1; m_airResistance = 0.1f; m_link = NULL; m_complexStatus = 0; m_numCollRecords = 0; memset( m_collRecords, 0, sizeof( m_collRecords ) ); m_unk6 = 0; m_damageImpulseMagnitude = 0; m_damageEntity = NULL; #if 0 m_vecUnk5.Reset(); m_vecUnk6.Reset(); m_centerOfMass.Reset(); #endif m_distanceTravelled = 0; m_pAttachedEntity = NULL; m_unk8 = 0; m_unk9 = 0; m_unk10 = 0; m_unk11 = 0; m_unk12 = 0; m_fLighting2 = 0; m_fLighting3 = 0; m_unk13 = 10; physicalFlags = 2; m_fLighting = 0; }
//////////////////////////////////////////////////////////////////////////////// // Program main //////////////////////////////////////////////////////////////////////////////// int main(int argc, char** argv) { Matrix M; Matrix N; char *str; if(argc == 5) { // Allocate and initialize the matrices srand(time(0)); M = AllocateMatrix(strtol(argv[1],&str,10), strtol(argv[2],&str,10), 1); N = AllocateMatrix(strtol(argv[3],&str,10), strtol(argv[4],&str,10), 1); WriteFile(M, (char *)"m1.in"); WriteFile(N, (char *)"m2.in"); FreeMatrix(&M); FreeMatrix(&N); return 0; } else { printf("Incorrect number of arguments\n"); exit(1); } return 0; }
/* Reorders eigenvectors so that they come with eigenvalues in * * ascending order. A->M[i][i].real is also rearanged. */ void sort_eigenvectors(Matrix *A, Matrix *V) { register int N , i, j, k ; register double fu ; register double_complex *bu ; double_complex *SaveTmp0 ; Matrix Tmp ; double *D; N = A->N ; Tmp = AllocateMatrix(N) ; /* Make V->M[i] the i th eigenvector. * * Otherwise V->M[][i] is the eigenvector which makes reordering costly */ HermitianConj(V, &Tmp) ; /* extract the eigenvalues */ D = (double *) malloc(N*sizeof(double)) ; for(i=0;i<N;i++) D[i] = A->M[i][i].real ; /* Save Tmp.M[0] so that we can deallocate the memory Tmp occuppies * * The Value of Tmp.M[0] may change with sorting */ SaveTmp0 = Tmp.M[0] ; for(i=0;i<N;i++) for (j=1;j<N-i;j++) { k = j-1 ; if(D[j]<D[k]) /* swicth them */ { /* switch the eigenvalues */ fu = D[k] ; D[k] = D[j] ; D[j] = fu ; /* switch the eigenvectors */ bu = Tmp.M[k] ; Tmp.M[k] = Tmp.M[j] ; Tmp.M[j] = bu ; } } for(i=0;i<N;i++) A->M[i][i].real = D[i]; free(D) ; HermitianConj(&Tmp,V) ; /*Restore the Tmp.M[0] for deallocation */ Tmp.M[0] = SaveTmp0 ; deAllocate(&Tmp) ; }
main() { float **matrix; int i, j; int seed; int n; printf("Enter seed for RNG: "); scanf("%d", &seed); srand48(seed); /* seed the number generator */ printf("Enter n (for an n x n matrix): "); scanf("%d", &n); matrix = AllocateMatrix(n,n); PrintMatrix(matrix,n,n); printf("\n\n"); for(i=0; i<n; i++) for(j=0; j<n; j++) matrix[i][j] = (float)drand48(); PrintMatrix(matrix,n,n); }
int Kalkreuter_qdp(QDP_ColorVector **eigVec, double *eigVal, Real Tolerance, Real RelTol, int Nvecs, int MaxIter, int Restart, int Kiters, QDP_Subset subset) { QLA_Real max_error = 1.0e+10; QLA_Real min_grad; QLA_Real *grad, *err; Matrix Array, V; QDP_ColorVector *vec; int total_iters=0; int i, j; int iter = 0; #ifdef DEBUG if(QDP_this_node==0) printf("begin Kalkreuter_qdp\n"); #endif prepare_Matrix(); Array = AllocateMatrix(Nvecs); /* Allocate the array */ V = AllocateMatrix(Nvecs); /* Allocate the Eigenvector matrix */ vec = QDP_create_V(); grad = malloc(Nvecs*sizeof(QLA_Real)); err = malloc(Nvecs*sizeof(QLA_Real)); /* Initiallize all the eigenvectors to a random vector */ for(j=0; j<Nvecs; j++) { grad[j] = 1.0e+10; QDP_V_eq_gaussian_S(eigVec[j], rand_state, QDP_all); eigVal[j] = 1.0e+16; //project_out_qdp(eigVec[j], eigVec, j, subset); //normalize_qdp(eigVec[j], subset); } #if 0 constructArray_qdp(eigVec, &Array, grad, subset); Jacobi(&Array, &V, JACOBI_TOL); sort_eigenvectors(&Array, &V); RotateBasis_qdp(eigVec, &V, subset); #endif while( (max_error>Tolerance) && (iter<Kiters) ) { iter++; min_grad = grad[0]/eigVal[0]; for(i=1; i<Nvecs; i++) { if(grad[i]<min_grad*eigVal[i]) min_grad = grad[i]/eigVal[i]; } RelTol = 0.3; for(j=0; j<Nvecs; j++) { if(grad[j]>Tolerance*eigVal[j]) { QLA_Real rt; rt = RelTol*min_grad*eigVal[j]/grad[j]; //rt = 1e-5/grad[j]; if(rt>RelTol) rt = RelTol; //rt = RelTol; QDP_V_eq_V(vec, eigVec[j], QDP_all); total_iters += Rayleigh_min_qdp(vec, eigVec, Tolerance, rt, j, MaxIter, Restart, subset); QDP_V_eq_V(eigVec[j], vec, QDP_all); } } constructArray_qdp(eigVec, &Array, grad, subset); for(i=0; i<Nvecs; i++) node0_printf("quot(%i) = %g +/- %8e |grad|=%g\n", i, Array.M[i][i].real, err[i], grad[i]); #ifdef DEBUG node0_printf("Eigenvalues before diagonalization\n"); for(i=0;i<Nvecs;i++) node0_printf("quot(%i) = %g |grad|=%g\n",i,Array.M[i][i].real,grad[i]); #endif Jacobi(&Array, &V, JACOBI_TOL); sort_eigenvectors(&Array, &V); RotateBasis_qdp(eigVec, &V, subset); constructArray_qdp(eigVec, &Array, grad, subset); /* find the maximum error */ max_error = 0.0; for(i=0; i<Nvecs; i++) { err[i] = eigVal[i]; eigVal[i] = Array.M[i][i].real; err[i] = fabs(err[i] - eigVal[i])/(1.0 - RelTol*RelTol); if(eigVal[i]>1e-10) { #ifndef STRICT_CONVERGENCE if(err[i]/eigVal[i]>max_error) max_error = err[i]/eigVal[i]; #else if(grad[i]/eigVal[i]>max_error) max_error = grad[i]/eigVal[i]; #endif } } node0_printf("\nEigenvalues after diagonalization at iteration %i\n",iter); for(i=0; i<Nvecs; i++) node0_printf("quot(%i) = %g +/- %8e |grad|=%g\n", i, eigVal[i], err[i], grad[i]); } node0_printf("BEGIN RESULTS\n"); for(i=0;i<Nvecs;i++){ node0_printf("Eigenvalue(%i) = %g +/- %8e\n", i,eigVal[i],err[i]); } #if 0 node0_printf("BEGIN EIGENVALUES\n"); for(i=0; i<Nvecs; i++) { double ev, er; ev = sqrt(eigVal[i]); er = err[i]/(2*ev); node0_printf("%.8g\t%g\n", ev, er); } node0_printf("END EIGENVALUES\n"); { QDP_Writer *qw; QDP_String *md; char evstring[100], *fn="eigenvecs.out"; md = QDP_string_create(); sprintf(evstring, "%i", Nvecs); QDP_string_set(md, evstring); qw = QDP_open_write(md, fn, QDP_SINGLEFILE); for(i=0; i<Nvecs; i++) { double ev, er; ev = sqrt(eigVal[i]); er = err[i]/(2*ev); sprintf(evstring, "%.8g\t%g", ev, er); QDP_string_set(md, evstring); QDP_write_V(qw, md, eigVec[i]); } QDP_close_write(qw); QDP_string_destroy(md); } #endif /** Deallocate the arrays **/ deAllocate(&V) ; deAllocate(&Array) ; free(err); free(grad); QDP_destroy_V(vec); cleanup_Matrix(); #ifdef DEBUG if(QDP_this_node==0) printf("end Kalkreuter_qdp\n"); #endif return total_iters; }
/* * Peform a course registration using selected corresponding points by user */ void CourseRegistration(void) { register int i; double Sxx, Sxy, Sxz, Syx, Syy, Syz, Szx, Szy, Szz; double max_eval; point_xyz *p, *q; point_xyz mean_corres_p, mean_corres_q; int num_corres; matrix *Q, *R; vector *max_evec, *t; // initialize values Sxx = Sxy = Sxz = Syx = Syy = Syz = Szx = Szy = Szz = 0.0; mean_corres_p.x = mean_corres_p.y = mean_corres_p.z = 0.0; mean_corres_q.x = mean_corres_q.y = mean_corres_q.z = 0.0; // take the smaller one num_corres = (num_corres_anc<num_corres_mov)?num_corres_anc:num_corres_mov; // allocate memory Q = AllocateMatrix(4, 4); R = AllocateMatrix(3, 3); max_evec = AllocateVector(4); t = AllocateVector(3); p = (point_xyz *) malloc (num_corres * sizeof(point_xyz)); q = (point_xyz *) malloc (num_corres * sizeof(point_xyz)); // transform for (i=0; i<num_corres; i++) { p[i].x = (rd_mov[corres_rd_mov[i]].xyz[3*corres_pt_mov[i]] * rd_mov[corres_rd_mov[i]].M[0]) + (rd_mov[corres_rd_mov[i]].xyz[3*corres_pt_mov[i]+1] * rd_mov[corres_rd_mov[i]].M[4]) + (rd_mov[corres_rd_mov[i]].xyz[3*corres_pt_mov[i]+2] * rd_mov[corres_rd_mov[i]].M[8]) + rd_mov[corres_rd_mov[i]].M[12]; p[i].y = (rd_mov[corres_rd_mov[i]].xyz[3*corres_pt_mov[i]] * rd_mov[corres_rd_mov[i]].M[1]) + (rd_mov[corres_rd_mov[i]].xyz[3*corres_pt_mov[i]+1] * rd_mov[corres_rd_mov[i]].M[5]) + (rd_mov[corres_rd_mov[i]].xyz[3*corres_pt_mov[i]+2] * rd_mov[corres_rd_mov[i]].M[9]) + rd_mov[corres_rd_mov[i]].M[13]; p[i].z = (rd_mov[corres_rd_mov[i]].xyz[3*corres_pt_mov[i]] * rd_mov[corres_rd_mov[i]].M[2]) + (rd_mov[corres_rd_mov[i]].xyz[3*corres_pt_mov[i]+1] * rd_mov[corres_rd_mov[i]].M[6]) + (rd_mov[corres_rd_mov[i]].xyz[3*corres_pt_mov[i]+2] * rd_mov[corres_rd_mov[i]].M[10]) + rd_mov[corres_rd_mov[i]].M[14]; q[i].x = (rd_anc[corres_rd_anc[i]].xyz[3*corres_pt_anc[i]] * rd_anc[corres_rd_anc[i]].M[0]) + (rd_anc[corres_rd_anc[i]].xyz[3*corres_pt_anc[i]+1] * rd_anc[corres_rd_anc[i]].M[4]) + (rd_anc[corres_rd_anc[i]].xyz[3*corres_pt_anc[i]+2] * rd_anc[corres_rd_anc[i]].M[8]) + rd_anc[corres_rd_anc[i]].M[12]; q[i].y = (rd_anc[corres_rd_anc[i]].xyz[3*corres_pt_anc[i]] * rd_anc[corres_rd_anc[i]].M[1]) + (rd_anc[corres_rd_anc[i]].xyz[3*corres_pt_anc[i]+1] * rd_anc[corres_rd_anc[i]].M[5]) + (rd_anc[corres_rd_anc[i]].xyz[3*corres_pt_anc[i]+2] * rd_anc[corres_rd_anc[i]].M[9]) + rd_anc[corres_rd_anc[i]].M[13]; q[i].z = (rd_anc[corres_rd_anc[i]].xyz[3*corres_pt_anc[i]] * rd_anc[corres_rd_anc[i]].M[2]) + (rd_anc[corres_rd_anc[i]].xyz[3*corres_pt_anc[i]+1] * rd_anc[corres_rd_anc[i]].M[6]) + (rd_anc[corres_rd_anc[i]].xyz[3*corres_pt_anc[i]+2] * rd_anc[corres_rd_anc[i]].M[10]) + rd_anc[corres_rd_anc[i]].M[14]; //printf("%d: %f %f %f\n", i, q[i].x, q[i].y, q[i].z); } for (i=0; i<num_corres; i++) { mean_corres_p.x += p[i].x; mean_corres_p.y += p[i].y; mean_corres_p.z += p[i].z; mean_corres_q.x += q[i].x; mean_corres_q.y += q[i].y; mean_corres_q.z += q[i].z; } mean_corres_p.x /= (double)num_corres; mean_corres_p.y /= (double)num_corres; mean_corres_p.z /= (double)num_corres; mean_corres_q.x /= (double)num_corres; mean_corres_q.y /= (double)num_corres; mean_corres_q.z /= (double)num_corres; for (i=0; i<num_corres; i++) { Sxx += p[i].x * q[i].x; Sxy += p[i].x * q[i].y; Sxz += p[i].x * q[i].z; Syx += p[i].y * q[i].x; Syy += p[i].y * q[i].y; Syz += p[i].y * q[i].z; Szx += p[i].z * q[i].x; Szy += p[i].z * q[i].y; Szz += p[i].z * q[i].z; } Sxx = Sxx / (double)num_corres - (mean_corres_p.x * mean_corres_q.x); Sxy = Sxy / (double)num_corres - (mean_corres_p.x * mean_corres_q.y); Sxz = Sxz / (double)num_corres - (mean_corres_p.x * mean_corres_q.z); Syx = Syx / (double)num_corres - (mean_corres_p.y * mean_corres_q.x); Syy = Syy / (double)num_corres - (mean_corres_p.y * mean_corres_q.y); Syz = Syz / (double)num_corres - (mean_corres_p.y * mean_corres_q.z); Szx = Szx / (double)num_corres - (mean_corres_p.z * mean_corres_q.x); Szy = Szy / (double)num_corres - (mean_corres_p.z * mean_corres_q.y); Szz = Szz / (double)num_corres - (mean_corres_p.z * mean_corres_q.z); // construct N Q->entry[0][0] = Sxx + Syy + Szz; Q->entry[1][0] = Q->entry[0][1] = Syz - Szy; Q->entry[2][0] = Q->entry[0][2] = Szx - Sxz; Q->entry[3][0] = Q->entry[0][3] = Sxy - Syx; Q->entry[1][1] = Sxx - Syy - Szz; Q->entry[1][2] = Q->entry[2][1] = Sxy + Syx; Q->entry[1][3] = Q->entry[3][1] = Szx + Sxz; Q->entry[2][2] = -Sxx + Syy - Szz; Q->entry[2][3] = Q->entry[3][2] = Syz + Szy; Q->entry[3][3] = -Sxx - Syy + Szz; // --- compute largest eigenvalues and eigenvectors of Q --- SymmetricLargestEigens(Q, max_evec, &max_eval); // make sure max_evec[0] > 0 if (max_evec->entry[0] < 0) { for (i=0; i<4; i++) max_evec->entry[i] *= -1.0; } // --- compute rotation matrix --- RotationQuaternion(max_evec, R); // --- compute translation vector --- t->entry[0] = mean_corres_q.x - R->entry[0][0] * mean_corres_p.x - R->entry[0][1] * mean_corres_p.y - R->entry[0][2] * mean_corres_p.z; t->entry[1] = mean_corres_q.y - R->entry[1][0] * mean_corres_p.x - R->entry[1][1] * mean_corres_p.y - R->entry[1][2] * mean_corres_p.z; t->entry[2] = mean_corres_q.z - R->entry[2][0] * mean_corres_p.x - R->entry[2][1] * mean_corres_p.y - R->entry[2][2] * mean_corres_p.z; //PrintMatrix(R); //PrintVector(t); new_M[0] = R->entry[0][0]; new_M[4] = R->entry[0][1]; new_M[8] = R->entry[0][2]; new_M[1] = R->entry[1][0]; new_M[5] = R->entry[1][1]; new_M[9] = R->entry[1][2]; new_M[2] = R->entry[2][0]; new_M[6] = R->entry[2][1]; new_M[10] = R->entry[2][2]; new_M[12] = t->entry[0]; new_M[13] = t->entry[1]; new_M[14] = t->entry[2]; new_M[3] = new_M[7] = new_M[11] = 0; new_M[15] = 1; // free memory FreeMatrix(Q); FreeMatrix(R); FreeVector(max_evec); FreeVector(t); free(p); free(q); }
/* * Perfrom a fine registration using the ICP algorithm */ void FineRegistration(void) { int i, j, count; int num_p, num_q; point_xyz *p, *q, cp; matrix *R; vector *t; double Mf[16]; // compute total number of points num_p = num_q = 0; for (i=0; i<num_rdata_anc; i++) num_q += rd_anc[i].num_pt; for (i=0; i<num_rdata_mov; i++) num_p += rd_mov[i].num_pt; // allocate memory R = AllocateMatrix(3, 3); t = AllocateVector(3); p = (point_xyz *) malloc (num_p * sizeof(point_xyz)); q = (point_xyz *) malloc (num_q * sizeof(point_xyz)); // copy xyz values count = 0; for (i=0; i<num_rdata_anc; i++) { for (j=0; j<rd_anc[i].num_pt; j++) { cp.x = rd_anc[i].xyz[3*j]; cp.y = rd_anc[i].xyz[3*j+1]; cp.z = rd_anc[i].xyz[3*j+2]; // transform with its modeling transformation matrix q[count].x = cp.x * rd_anc[i].M[0] + cp.y * rd_anc[i].M[4] + cp.z * rd_anc[i].M[8] + rd_anc[i].M[12]; q[count].y = cp.x * rd_anc[i].M[1] + cp.y * rd_anc[i].M[5] + cp.z * rd_anc[i].M[9] + rd_anc[i].M[13]; q[count].z = cp.x * rd_anc[i].M[2] + cp.y * rd_anc[i].M[6] + cp.z * rd_anc[i].M[10] + rd_anc[i].M[14]; count++; } } count = 0; for (i=0; i<num_rdata_mov; i++) { // use modelview just for matrix multiplication glMatrixMode(GL_MODELVIEW); glPushMatrix(); glLoadIdentity(); glMultMatrixd(new_M); glMultMatrixd(rd_mov[i].M); glGetDoublev(GL_MODELVIEW_MATRIX, Mf); glPopMatrix(); for (j=0; j<rd_mov[i].num_pt; j++) { // transform cp.x = rd_mov[i].xyz[3*j]; cp.y = rd_mov[i].xyz[3*j+1]; cp.z = rd_mov[i].xyz[3*j+2]; p[count].x = cp.x*Mf[0] + cp.y*Mf[4] + cp.z*Mf[8] + Mf[12]; p[count].y = cp.x*Mf[1] + cp.y*Mf[5] + cp.z*Mf[9] + Mf[13]; p[count].z = cp.x*Mf[2] + cp.y*Mf[6] + cp.z*Mf[10] + Mf[14]; count++; } } // perform ICP //ICPalgorithm(R, t, p, num_p, q, num_q, 5, 5.0, 100, 0.10, 0.0005); ICPalgorithm(R, t, p, num_p, q, num_q, 5, 5.0, 1000, 0.010, 0.000005); Mf[0] = R->entry[0][0]; Mf[4] = R->entry[0][1]; Mf[8] = R->entry[0][2]; Mf[1] = R->entry[1][0]; Mf[5] = R->entry[1][1]; Mf[9] = R->entry[1][2]; Mf[2] = R->entry[2][0]; Mf[6] = R->entry[2][1]; Mf[10] = R->entry[2][2]; Mf[12] = t->entry[0]; Mf[13] = t->entry[1]; Mf[14] = t->entry[2]; Mf[3] = Mf[7] = Mf[11] = 0; Mf[15] = 1; // update new_M glMatrixMode(GL_MODELVIEW); glPushMatrix(); glLoadIdentity(); glMultMatrixd(Mf); glMultMatrixd(new_M); glGetDoublev(GL_MODELVIEW_MATRIX, new_M); glPopMatrix(); // free memory FreeMatrix(R); FreeVector(t); free(p); free(q); }