diff options
Diffstat (limited to 'dave/AffineSolve.c.CHARLES')
-rw-r--r-- | dave/AffineSolve.c.CHARLES | 643 |
1 files changed, 0 insertions, 643 deletions
diff --git a/dave/AffineSolve.c.CHARLES b/dave/AffineSolve.c.CHARLES deleted file mode 100644 index cb62ef6..0000000 --- a/dave/AffineSolve.c.CHARLES +++ /dev/null @@ -1,643 +0,0 @@ -// -// main.c -// Aff -// Created by user on 3/2/17. -// Copyright © 2017 user. All rights reserved. -// - -#include <stdio.h> -#include <string.h> -#include <stdlib.h> -#include <math.h> -#include "dclapack.h" - -#define LH_ID 1 -#define NUM_HMD 32 - -#define MAX_POINTS 128 -//#define _ABS(a) ( (a)<=0 ? -(a) : (a) ) -#define _SIGN(a) ( (a)<=0 ? -1.0f : 1.0f ) -#define RANDF ( (float)rand() / (float)RAND_MAX ) -#define PI 3.14159265358979323846264 - -#define STEP_SIZE_ROT 1.0 -#define STEP_SIZE_POS 1.0 -#define FALLOFF 0.99999 -#define NITER 2000000 -#define TOO_SMALL 0.0001 -#define ORTHOG_PENALTY 1.0 - -float hmd_pos[NUM_HMD][3]; -void ReadHmdPoints() -{ - int i; - FILE *fin = fopen("HMD_points.csv","r"); - if (fin==NULL) { - printf("ERROR: could not open HMD_points.csv for reading\n"); - exit(1); - } - - for (i=0; i<NUM_HMD; i++) { - fscanf(fin, "%f %f %f", &(hmd_pos[i][0]), &(hmd_pos[i][1]), &(hmd_pos[i][2])); - } - - fclose(fin); -} - -float hmd_angle[NUM_HMD][2]; -void ReadPtinfo() -{ - // Initialize to -9999 - int i; - for (i=0; i<NUM_HMD; i++) { hmd_angle[i][0]=-9999.0; hmd_angle[i][1]=-9999.0; } - - // Read ptinfo.csv - FILE *fin = fopen("ptinfo.csv", "r"); - if (fin==NULL) { printf("ERROR: could not open ptinfo.csv for reading\n"); exit(1); } - while (!feof(fin)) - { - // Read the angle - int sen,lh,axis,count; - float angle, avglen, stddevang, stddevlen; - float max_outlier_length, max_outlier_angle; - int rt = fscanf( fin, "%d %d %d %d %f %f %f %f %f %f\n", - &sen, &lh, &axis, &count, - &angle, &avglen, &stddevang, &stddevlen, - &max_outlier_length, &max_outlier_angle); - if (rt != 10) { break; } - - // If it's valid, store in the result - if (lh == LH_ID && sen < NUM_HMD) { - hmd_angle[sen][axis] = angle; - } - } - fclose(fin); -} - -#define PRINT_MAT(A,M,N) { \ - int m,n; \ - printf(#A "\n"); \ - for (m=0; m<M; m++) { \ - for (n=0; n<N; n++) { \ - printf("%f\t", A[m][n]); \ - } \ - printf("\n"); \ - } \ -} - -#define CrossProduct(ox,oy,oz,a,b,c,x,y,z) { \ - ox=(b)*(z)-(c)*(y); \ - oy=(c)*(x)-(a)*(z); \ - oz=(a)*(y)-(b)*(x); } - -void OrthoSolve( - float T[4][4], // OUTPUT: 4x4 transformation matrix - FLOAT S_out[2][MAX_POINTS], // OUTPUT: array of screenspace points - FLOAT S_in[2][MAX_POINTS], // INPUT: array of screenspace points - FLOAT X_in[3][MAX_POINTS], // INPUT: array of offsets - int nPoints) -{ - int i,j,k; - FLOAT R[3][3]; // OUTPUT: 3x3 rotation matrix - FLOAT trans[3]; // INPUT: x,y,z translation vector - - //-------------------- - // Remove the center of the HMD offsets, and the screen space - //-------------------- - FLOAT xbar[3] = {0.0, 0.0, 0.0}; - FLOAT sbar[2] = {0.0, 0.0}; - FLOAT S[2][MAX_POINTS]; - FLOAT X[3][MAX_POINTS]; - FLOAT inv_nPoints = 1.0 / nPoints; - for (i=0; i<nPoints; i++) { - xbar[0] += X_in[0][i]; - xbar[1] += X_in[1][i]; - xbar[2] += X_in[2][i]; - sbar[0] += S_in[0][i]; - sbar[1] += S_in[1][i]; - } - for (j=0; j<3; j++) { xbar[j] *= inv_nPoints; } - for (j=0; j<2; j++) { sbar[j] *= inv_nPoints; } - for (i=0; i<nPoints; i++) { - X[0][i] = X_in[0][i] - xbar[0]; - X[1][i] = X_in[1][i] - xbar[1]; - X[2][i] = X_in[2][i] - xbar[2]; - S[0][i] = S_in[0][i] - sbar[0]; - S[1][i] = S_in[1][i] - sbar[1]; - } - - //-------------------- - // Solve for the morph matrix - // S = M X - // thus - // (SX^t)(XX^t)^-1 = M - //-------------------- - FLOAT Xt[MAX_POINTS][3]; - FLOAT XXt[3][3]; - FLOAT invXXt[3][3]; - FLOAT SXt[2][3]; - FLOAT M[2][3]; // Morph matrix! (2 by 3) - TRANSP(X,Xt,3,nPoints); - MUL(X,Xt,XXt,3,nPoints,3); - MUL(S,Xt,SXt,2,nPoints,3); - INV(XXt,invXXt,3); - MUL(SXt,invXXt,M,2,3,3); -//PRINT(M,2,3); - -// Double checking work -FLOAT S_morph[2][MAX_POINTS]; -MUL(M,X,S_morph,2,3,nPoints); -for (i=0; i<nPoints; i++) { S_morph[0][i]+=sbar[0]; S_morph[1][i]+=sbar[1]; } - - //-------------------- - // Solve for the non-trivial vector - // uf -- vector that goes into the camera - //-------------------- - FLOAT uM[3][3] = { - { M[0][0], M[0][1], M[0][2] }, - { M[1][0], M[1][1], M[1][2] }, - { 3.14567, -1.2345, 4.32567 } }; // Morph matrix with appended row -//PRINT(uM,3,3); -// ToDo: Pick a number for the bottom that is NOT linearly separable with M[0] and M[1] - FLOAT B[3][1] = { {0.0}, {0.0}, {1.0} }; - FLOAT inv_uM[3][3]; - FLOAT uf[3][1]; - INV(uM,inv_uM,3); - MUL(inv_uM,B,uf,3,3,1); - - //-------------------- - // Solve for unit length vector - // f that goes into the camera - //-------------------- - FLOAT uf_len = sqrt( uf[0][0]*uf[0][0] + uf[1][0]*uf[1][0] + uf[2][0]*uf[2][0] ); - FLOAT f[3][1] = { {uf[0][0]/uf_len}, {uf[1][0]/uf_len}, {uf[2][0]/uf_len} }; - printf( "FFF: {%f %f %f}: %f\n", f[0][0], f[1][0], f[2][0], uf_len ); -//PRINT(uf,3,1); -//PRINT(f,3,1); - -//FLOAT check[3][1]; -//MUL(uM,uf,check,3,3,1); -//PRINT(check,3,1); - - //-------------------- - // take cross products to get vectors u,r - //-------------------- - FLOAT u[3][1], r[3][1]; - CrossProduct(u[0][0],u[1][0],u[2][0],f[0][0],f[1][0],f[2][0],1.0,0.0,0.0); - FLOAT inv_ulen = 1.0 / sqrt( u[0][0]*u[0][0] + u[1][0]*u[1][0] + u[2][0]*u[2][0] ); - u[0][0]*=inv_ulen; u[1][0]*=inv_ulen; u[2][0]*=inv_ulen; - CrossProduct(r[0][0],r[1][0],r[2][0],f[0][0],f[1][0],f[2][0],u[0][0],u[1][0],u[2][0]); -//PRINT(u,3,1); -//PRINT(r,3,1); - - //-------------------- - // Use morph matrix to get screen space - // uhat,rhat - //-------------------- - FLOAT uhat[2][1], rhat[2][1], fhat[2][1]; - MUL(M,f,fhat,2,3,1); - MUL(M,u,uhat,2,3,1); - MUL(M,r,rhat,2,3,1); - FLOAT fhat_len = sqrt( fhat[0][0]*fhat[0][0] + fhat[1][0]*fhat[1][0] ); - FLOAT uhat_len = sqrt( uhat[0][0]*uhat[0][0] + uhat[1][0]*uhat[1][0] ); - FLOAT rhat_len = sqrt( rhat[0][0]*rhat[0][0] + rhat[1][0]*rhat[1][0] ); - FLOAT urhat_len = 0.5 * (uhat_len + rhat_len); -/* -printf("fhat %f %f (len %f)\n", fhat[0][0], fhat[1][0], fhat_len); -printf("uhat %f %f (len %f)\n", uhat[0][0], uhat[1][0], uhat_len); -printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); -*/ - FLOAT ydist1 = 1.0 / uhat_len; //0.25*PI / uhat_len; - FLOAT ydist2 = 1.0 / rhat_len; //0.25*PI / rhat_len; - FLOAT ydist = 1.0 / urhat_len; - printf("ydist1 %f ydist2 %f ydist %f FH: %f\n", ydist1, ydist2, ydist, fhat_len); - - //-------------------- - // Rescale the axies to be of the proper length - //-------------------- - FLOAT x[3][1] = { {M[0][0]*ydist}, {0.0}, {M[1][0]*ydist} }; - FLOAT y[3][1] = { {M[0][1]*ydist}, {0.0}, {M[1][1]*ydist} }; - FLOAT z[3][1] = { {M[0][2]*ydist}, {0.0}, {M[1][2]*ydist} }; -printf( "YDIST: %f\n", ydist ); -printf( "{%f %f, %f %f, %f %f}\n", x[0][0], x[2][0], y[0][0], y[2][0], z[0][0], z[2][0] ); -printf( "{%f, %f, %f}\n", x[0][0]*x[0][0]+x[2][0]*x[2][0], y[0][0]*y[0][0]+y[2][0]*y[2][0], z[0][0]*z[0][0]+z[2][0]*z[2][0] ); - // we know the distance into (or out of) the camera for the z axis, - // but we don't know which direction . . . - FLOAT x_y = sqrt(1.0 - x[0][0]*x[0][0] - x[2][0]*x[2][0]); - FLOAT y_y = sqrt(1.0 - y[0][0]*y[0][0] - y[2][0]*y[2][0]); - FLOAT z_y = sqrt(1.0 - z[0][0]*z[0][0] - z[2][0]*z[2][0]); -printf( "{%f %f %f}\n", x_y, y_y, z_y ); - // Exhaustively flip the minus sign of the z axis until we find the right one . . . - FLOAT bestErr = 9999.0; - FLOAT xy_dot2 = x[0][0]*y[0][0] + x[2][0]*y[2][0]; - FLOAT yz_dot2 = y[0][0]*z[0][0] + y[2][0]*z[2][0]; - FLOAT zx_dot2 = z[0][0]*x[0][0] + z[2][0]*x[2][0]; - for (i=0;i<2;i++) { - for (j=0;j<2;j++) { - for(k=0;k<2;k++) { - - // Calculate the error term - FLOAT xy_dot = xy_dot2 + x_y*y_y; - FLOAT yz_dot = yz_dot2 + y_y*z_y; - FLOAT zx_dot = zx_dot2 + z_y*x_y; - FLOAT err = _ABS(xy_dot) + _ABS(yz_dot) + _ABS(zx_dot); - - // Calculate the handedness - FLOAT cx,cy,cz; - CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]); - FLOAT hand = cx*z[0][0] + cy*y_y + cz*z[2][0]; - printf("err %f hand %f\n", err, hand); - - // If we are the best right-handed frame so far - if (err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } - //if (i == 1 && j == 1 && k == 1) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } - z_y = -z_y; - } - y_y = -y_y; - } - x_y = -x_y; - } - printf("bestErr %f\n", bestErr); -/* - for (i=0; i<nPoints; i++) { - float x1 = x[0][0]*X[0][i] + y[0][0]*X[1][i] + z[0][0]*X[2][i]; - float y1 = x[1][0]*X[0][i] + y[1][0]*X[1][i] + z[1][0]*X[2][i]; - float z1 = x[2][0]*X[0][i] + y[2][0]*X[1][i] + z[2][0]*X[2][i]; - printf("x1z1 %f %f y1 %f\n", x1, z1, y1); - } -*/ -/* - //-------------------- - // Combine uhat and rhat to figure out the unit x-vector - //-------------------- - FLOAT xhat[2][1] = { {0.0}, {1.0} }; - FLOAT urhat[2][2] = { - {uhat[0][0], uhat[1][0]}, - {rhat[0][0], rhat[1][0]} }; - FLOAT inv_urhat[2][2]; - FLOAT ab[2][1]; - INV(urhat,inv_urhat,2); - MUL(inv_urhat,xhat,ab,2,2,1); -PRINT(ab,2,1); - FLOAT a = ab[0][0], b = ab[1][0]; - - //------------------- - // calculate the xyz coordinate system - //------------------- - FLOAT y[3][1] = { {f[0][0]}, {f[1][0]}, {f[2][0]} }; - FLOAT x[3][1] = { {a*u[0][0] + b*r[0][0]}, {a*u[1][0] + b*r[1][0]}, {a*u[2][0] + b*r[2][0]} }; - FLOAT inv_xlen = 1.0 / sqrt( x[0][0]*x[0][0] + x[1][0]*x[1][0] + x[2][0]*x[2][0] ); - x[0][0]*=inv_xlen; x[1][0]*=inv_xlen; x[2][0]*=inv_xlen; - FLOAT z[3][1]; - CrossProduct(z[0][0],z[1][0],z[2][0],x[0][0],x[1][0],x[2][0],y[0][0],y[1][0],y[2][0]); -*/ - // Store into the rotation matrix - for (i=0; i<3; i++) { R[i][0] = x[i][0]; R[i][1] = y[i][0]; R[i][2] = z[i][0]; } -//PRINT(R,3,3); - - //------------------- - // Calculate the translation of the centroid - //------------------- - trans[0]=tan(sbar[0]); trans[1]=1.0; trans[2]=tan(sbar[1]); - FLOAT inv_translen = ydist / sqrt( trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2] ); - trans[0]*=inv_translen; trans[1]*=inv_translen; trans[2]*=inv_translen; - - //------------------- - // Add in the centroid point - //------------------- - trans[0] -= xbar[0]*R[0][0] + xbar[1]*R[0][1] + xbar[2]*R[0][2]; - trans[1] -= xbar[0]*R[1][0] + xbar[1]*R[1][1] + xbar[2]*R[1][2]; - trans[2] -= xbar[0]*R[2][0] + xbar[1]*R[2][1] + xbar[2]*R[2][2]; - FLOAT transdist = sqrt( trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2] ); - - //------------------- - // Pack into the 4x4 transformation matrix - //------------------- - T[0][0]=R[0][0]; T[0][1]=R[0][1]; T[0][2]=R[0][2]; T[0][3]=trans[0]; - T[1][0]=R[1][0]; T[1][1]=R[1][1]; T[1][2]=R[1][2]; T[1][3]=trans[1]; - T[2][0]=R[2][0]; T[2][1]=R[2][1]; T[2][2]=R[2][2]; T[2][3]=trans[2]; - T[3][0]=0.0; T[3][1]=0.0; T[3][2]=0.0; T[3][3]=1.0; - - //------------------- - // Plot the output points - //------------------- - for (i=0; i<nPoints; i++) { - float Tx = T[0][0]*X_in[0][i] + T[0][1]*X_in[1][i] + T[0][2]*X_in[2][i] + T[0][3]; - float Ty = T[1][0]*X_in[0][i] + T[1][1]*X_in[1][i] + T[1][2]*X_in[2][i] + T[1][3]; - float Tz = T[2][0]*X_in[0][i] + T[2][1]*X_in[1][i] + T[2][2]*X_in[2][i] + T[2][3]; - S_out[0][i] = atan2(Tx, Ty); // horiz - S_out[1][i] = atan2(Tz, Ty); // vert - //S_out[0][i] = Tx; - //S_out[1][i] = Tz; - printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]); - } - -// printf("xbar %f %f %f\n", xbar[0], xbar[1], xbar[2]); -// printf("trans %f %f %f dist: %f\n", trans[0], trans[1], trans[2], transdist); -} - -void AffineSolve( - float T[4][4], // OUTPUT: transform - float O[MAX_POINTS][4], // INPUT: points, offsets - float N[MAX_POINTS][3], // INPUT: plane normals - float D[MAX_POINTS], // INPUT: plane offsets - int nPoints, int nIter, - float stepSizeRot, float stepSizePos, float falloff, int constrain) -{ - int i,j,k,iter; - //T[3][3] = 1.0f; - - printf("iter x y z error\n"); - - float gradDot = 1.0; - float prevGradDot = 1.0; - float de_dT[3][4]; // the gradient - float conj[3][4]; // the conjugate - float errorSq=0.0; - for (iter=0; iter<nIter; iter++) - { - //---------------------------------- - // Calculate the gradient direction - //---------------------------------- - errorSq = 0.0; - memset(de_dT, 0, 3*4*sizeof(float)); - for (i=0; i<nPoints; i++) - { - // What is the plane deviation error - float Ei = -D[i]; - for (j=0; j<3; j++) { - float Tj_oi = 0.0f; - for (k=0; k<4; k++) { - Tj_oi += T[j][k] * O[i][k]; - } - Ei += N[i][j] * Tj_oi; - } -// printf("E[%d] %f\n", i, Ei); - - // Figure out contribution to the error - for (j=0; j<3; j++) { - for (k=0; k<4; k++) { - de_dT[j][k] += N[i][j] * O[i][k] * Ei; - } - } - - errorSq += Ei*Ei; - } - -// printf("%d %f %f %f %f\n", iter, T[0][3], T[1][3], T[2][3], sqrt(errorSq)); -//exit(1); - // Constrain the gradient (such that dot products are zero) - if (constrain) - { - float T0T1 = 0.0, T1T2 = 0.0, T2T0 = 0.0; - for (k=0; k<3; k++) { - T0T1 += T[0][k] * T[1][k]; - T1T2 += T[1][k] * T[2][k]; - T2T0 += T[2][k] * T[0][k]; - } -// printf("T0T1 %f T1T2 %f T2T0 %f\n", T0T1, T1T2, T2T0); - for (k=0; k<3; k++) { - de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[1][k]; - de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[2][k]; - de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[2][k]; - de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[0][k]; - de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[1][k]; - de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[0][k]; - } - } - - // Calculate the gradient dot product - // (used by conjugate gradient method) - prevGradDot = gradDot; - gradDot = 0.0; - for (j=0; j<3; j++) { - for (k=0; k<4; k++) { - gradDot += de_dT[j][k] * de_dT[j][k]; - } - } - -// printf("Iter %d error %f gradDot %f prevGradDot %f\n", iter, sqrt(errorSq), gradDot, prevGradDot); - - //---------------------------------- - // Calculate the conjugate direction - //---------------------------------- -// if (iter==0) { - // First iteration, just use the gradient - for (j=0; j<3; j++) { - for (k=0; k<4; k++) { - conj[j][k] = -de_dT[j][k]; - } - } -/* } else { - // Calculate "beta" for Fletcher Reeves method - float beta = gradDot / prevGradDot; -//printf("gradDot %f prevGradDot %f beta %f\n", gradDot, prevGradDot, beta); - - // Update the conjugate - for (j=0; j<3; j++) { - for (k=0; k<4; k++) { - conj[j][k] = beta*conj[j][k] - de_dT[j][k]; - } - } - } -*/ - -// PRINT_MAT(de_dT,4,4); -// exit(1); - - //---------------------------------- - // How large is the gradient ? - //---------------------------------- - - double gradSizeRot = 0.0; - double gradSizePos = 0.0; - for (j=0; j<3; j++) { - for (k=0; k<3; k++) { - gradSizeRot += _ABS(conj[j][k]); - } - gradSizePos += _ABS(conj[j][k]); - } - if (gradSizeRot <= TOO_SMALL && gradSizePos <= TOO_SMALL) { break; } // Quit, we've totally converged - - //---------------------------------- - // Descend in the gradient direction - //---------------------------------- - if (gradSizeRot > TOO_SMALL) { - float scaleRot = stepSizeRot / gradSizeRot; - for (j=0; j<3; j++) { - for (k=0; k<3; k++) { - T[j][k] += scaleRot * conj[j][k]; - } - } - stepSizeRot *= falloff; - } - - if (gradSizePos > TOO_SMALL) { - float scalePos = stepSizePos / gradSizePos; - for (j=0; j<3; j++) { - T[j][3] += scalePos * conj[j][3]; - } - stepSizePos *= falloff; - } - - // Constrain the gradient (such that scaling is one) - if (constrain) - { - // Measure the scales - float len[3] = {0.0, 0.0, 0.0}; - for (j=0; j<3; j++) { - double lenSq = 0.0; - for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; } - len[j] = sqrt(lenSq); - } - - // How far off is the scale? - float xzLen = 0.5 * (len[0] + len[2]); - if (xzLen > TOO_SMALL) { - float inv_xzLen = 1.0 / xzLen; - for (j=0; j<3; j++) { - T[3][j] *= inv_xzLen; - } - } - - // Rescale the thing - for (j=0; j<3; j++) - { - if (len[j] > TOO_SMALL) { - float inv_len = 1.0 / len[j]; - for (k=0; k<3; k++) { T[j][k] *= inv_len; } - } - } - } - } - float dist = sqrt(T[0][3]*T[0][3] + T[1][3]*T[1][3] + T[2][3]*T[2][3]); - printf("AffineSolve: pos: %f %f %f dist: %f\n", T[0][3], T[1][3], T[2][3], dist); -} - -int main() -{ - int i,j,k,sen,axis; - - // Read the data files - printf( "...\n" ); - ReadHmdPoints(); - ReadPtinfo(); - - //------------------------- - // Package the lighthouse data for "AffineSolve" - //------------------------- - - // Data for the "iterative" affine solve formula - // float Tcalc[4][4]; - float O[MAX_POINTS][4]; - float N[MAX_POINTS][3]; - float D[MAX_POINTS]; - int nPlanes = 0; - - for (sen=0; sen<NUM_HMD; sen++) - { - for (axis=0; axis<2; axis++) - { - if (hmd_angle[sen][axis] != -9999.0) - { - // Set the offset - O[nPlanes][0] = hmd_pos[sen][0]; - O[nPlanes][1] = hmd_pos[sen][1]; - O[nPlanes][2] = hmd_pos[sen][2]; - O[nPlanes][3] = 1.0; - - // Calculate the plane equation - if (axis == 0) { // Horizontal - N[nPlanes][0] = -cos(hmd_angle[sen][axis]); - N[nPlanes][1] = -sin(hmd_angle[sen][axis]); - N[nPlanes][2] = 0.0; - D[nPlanes] = 0.0; - } else { // Vertical - N[nPlanes][0] = 0.0; - N[nPlanes][1] = -sin(hmd_angle[sen][axis]); - N[nPlanes][2] = cos(hmd_angle[sen][axis]); - D[nPlanes] = 0.0; - } - - printf("plane %d O %.3f %.3f %.3f %.3f N %.3f %.3f %.3f D %.3f\n", - nPlanes, - O[nPlanes][0], O[nPlanes][1], O[nPlanes][2], O[nPlanes][3], - N[nPlanes][0], N[nPlanes][1], N[nPlanes][2], - D[nPlanes]); - nPlanes++; - } - } - } - - - printf("nPlanes %d\n", nPlanes); - - //} - - //PRINT_MAT(Tcalc,4,4); - - - //-------------------------------------------------- - // Package the data for "OrthoSolve" - //-------------------------------------------------- - - // Data for the "fake" ortho solve formula - float Tortho[4][4]; // OUTPUT: 4x4 transformation matrix - FLOAT S_out[2][MAX_POINTS]; // INPUT: array of screenspace points - FLOAT S_in[2][MAX_POINTS]; // INPUT: array of screenspace points - FLOAT X_in[3][MAX_POINTS]; // INPUT: array of offsets - int nPoints=0; - - // Transform into the "OrthoSolve" format - for (sen=0; sen<NUM_HMD; sen++) - { - if (hmd_angle[sen][0] != -9999.0 && hmd_angle[sen][1] != -9999.0) - { - S_in[0][nPoints] = hmd_angle[sen][0]; - S_in[1][nPoints] = hmd_angle[sen][1]; - X_in[0][nPoints] = hmd_pos[sen][0]; - X_in[1][nPoints] = hmd_pos[sen][1]; - X_in[2][nPoints] = hmd_pos[sen][2]; - nPoints++; - } - } - printf("OrthoSolve nPoints %d\n", nPoints); - - //-------------------------------------------------- - // Run the "OrthoSolve" and then the "AffineSolve" - //-------------------------------------------------- - - int loop; - // for (loop=0; loop<1000000; loop++) - { - // Run OrthoSolve - OrthoSolve( - Tortho, // OUTPUT: 4x4 transformation matrix - S_out, // OUTPUT: array of output screenspace points - S_in, // INPUT: array of screenspace points - X_in, // INPUT: array of offsets - nPoints); - } - - // Run the calculation for Tcalc - //int run; - //for (run=0; run<100; run++) { -/* - // Initialize Tcalc to the identity matrix - memcpy(Tcalc, Tortho, 4*4*sizeof(float)); - //memset(Tcalc, 0, 4*4*sizeof(float)); - //for (i=0; i<4; i++) { Tcalc[i][i] = 1.0f; } - - // Solve it! - AffineSolve( - Tcalc, // OUTPUT: transform - O, // INPUT: points, offsets - N, // INPUT: plane normals - D, // INPUT: plane offsets - nPlanes, NITER, - STEP_SIZE_ROT, STEP_SIZE_POS, FALLOFF, - 1); -*/ - // insert code here... - return 0; -} |