From 3fa72f4b765457ce369d1aa1495dc36c90a94ebf Mon Sep 17 00:00:00 2001
From: ultramn <dchapm2@umbc.edu>
Date: Wed, 8 Mar 2017 21:03:05 -0800
Subject: Added OrthoSolve, it works great!

---
 dave/AffineSolve.c | 478 +++++++++++++++++++++++++++++++++++++++++++----------
 1 file changed, 395 insertions(+), 83 deletions(-)

(limited to 'dave')

diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c
index 36587e6..1eaf633 100644
--- a/dave/AffineSolve.c
+++ b/dave/AffineSolve.c
@@ -1,7 +1,6 @@
 //
 //  main.c
-//  AffineSolve
-//
+//  Aff
 //  Created by user on 3/2/17.
 //  Copyright © 2017 user. All rights reserved.
 //
@@ -10,10 +9,24 @@
 #include <string.h>
 #include <stdlib.h>
 #include <math.h>
+#include "dclapack.h"
 
 #define LH_ID    0
 #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()
 {
@@ -31,17 +44,35 @@ void ReadHmdPoints()
     fclose(fin);
 }
 
-#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 )
+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; }
 
-#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
+    // 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; \
@@ -54,6 +85,253 @@ void ReadHmdPoints()
     } \
 }
 
+#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} };
+//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\n", ydist1, ydist2, ydist);
+
+    //--------------------
+    // 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} };
+
+    // 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]);
+
+    // 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 (hand > 0 && err < bestErr) { 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] = asin(Tx / Ty);   // horiz
+        S_out[1][i] = asin(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
@@ -102,8 +380,8 @@ void AffineSolve(
             errorSq += Ei*Ei;
         }
 
-        printf("%d %f %f %f %f\n", iter, T[0][3], T[1][3], T[2][3], sqrt(errorSq));
-
+//        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)
         {
@@ -228,95 +506,129 @@ void AffineSolve(
             }
         }
     }
-
+    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;
+    int i,j,k,sen,axis;
+        
+    // Read the data files
+    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 nPoints = 0;
-    
-    // Read the hmd points
-    ReadHmdPoints();
-    
-    //-------------------------
-    // Read the lighthouse data
-    //-------------------------
-    FILE *fin = fopen("ptinfo.csv", "r");
-    if (fin==NULL) { printf("ERROR: could not open ptinfo.csv for reading\n"); exit(1); }
-    while (!feof(fin))
+    int nPlanes = 0;
+
+    for (sen=0; sen<NUM_HMD; sen++)
     {
-        // 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 (lh == LH_ID && sen < NUM_HMD) {
-            // Set the offset
-            O[nPoints][0] = hmd_pos[sen][0];
-            O[nPoints][1] = hmd_pos[sen][1];
-            O[nPoints][2] = hmd_pos[sen][2];
-            O[nPoints][3] = 1.0;
+        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 == 1) {   // Horizontal
-                N[nPoints][0] = -cos(angle);
-                N[nPoints][1] = -sin(angle);
-                N[nPoints][2] = 0.0;
-                D[nPoints]    = 0.0;
-            } else {           // Vertical
-                N[nPoints][0] = 0.0;
-                N[nPoints][1] = -sin(angle);
-                N[nPoints][2] =  cos(angle);
-                D[nPoints]    = 0.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("pt %d O %.3f %.3f %.3f %.3f  N %.3f %.3f %.3f  D %.3f\n",
-                nPoints,
-                O[nPoints][0], O[nPoints][1], O[nPoints][2], O[nPoints][3], 
-                N[nPoints][0], N[nPoints][1], N[nPoints][2], 
-                D[nPoints]);
-        
-            nPoints++;
         }
     }
-    fclose(fin);
+
     
-    printf("nPoints %d\n", nPoints);
+    printf("nPlanes %d\n", nPlanes);
         
-    // Run the calculation for Tcalc
-    int run;
-    //for (run=0; run<100; run++) {
-
-        // Initialize Tcalc to the identity matrix
-        //memcpy(Tcalc, Torig, 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
-            nPoints, NITER,
-            STEP_SIZE_ROT, STEP_SIZE_POS, FALLOFF,
-            1);
     //}
     
     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"
+    //--------------------------------------------------    
+    
+    // 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...
-    printf("Hello, World!\n");
     return 0;
 }
-- 
cgit v1.2.3