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