#include "linmath.h"
#include "survive_cal.h"
#include <dclapack.h>
#include <linmath.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <survive.h>
#include <survive_reproject.h>
// Dave talks about this poser here: https://www.youtube.com/watch?v=nSbEltdH9vM&feature=youtu.be&t=2h29m47s
void OrthoSolve(
FLT T[4][4], // OUTPUT: 4x4 transformation matrix
FLT S_out[2][SENSORS_PER_OBJECT], // OUTPUT: array of screenspace points
FLT S_in[2][SENSORS_PER_OBJECT], // INPUT: array of screenspace points
FLT X_in[3][SENSORS_PER_OBJECT], // INPUT: array of offsets
int nPoints);
typedef struct
{
int nextaxis;
float lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];
float angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];
//Stuff
} DummyData;
int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
{
PoserType pt = pd->pt;
SurviveContext * ctx = so->ctx;
DummyData * dd = so->PoserData;
if (!dd)
so->PoserData = dd = calloc(sizeof(DummyData), 1);
switch( pt )
{
case POSERDATA_IMU:
{
PoserDataIMU * imu = (PoserDataIMU*)pd;
//printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] );
break;
}
case POSERDATA_LIGHT:
{
PoserDataLight * l = (PoserDataLight*)pd;
if (l->length > dd->lengths[l->sensor_id][l->lh][dd->nextaxis]) {
dd->lengths[l->sensor_id][l->lh][dd->nextaxis] = l->length;
dd->angles[l->sensor_id][l->lh][dd->nextaxis] = l->angle;
}
break;
}
case POSERDATA_SYNC: {
PoserDataLight *l = (PoserDataLight *)pd;
int lhid = l->lh;
dd->nextaxis = l->acode & 1;
if (dd->nextaxis == 0) {
int i;
int max_hits = 0;
FLT S_in[2][SENSORS_PER_OBJECT];
FLT X_in[3][SENSORS_PER_OBJECT];
for (i = 0; i < SENSORS_PER_OBJECT; i++) {
// Load all our valid points into something the LHFinder can use.
if (dd->lengths[i][lhid][0] > 0 && dd->lengths[i][lhid][1] > 0) {
S_in[0][max_hits] = dd->angles[i][lhid][0];
S_in[1][max_hits] = dd->angles[i][lhid][1];
dd->lengths[i][lhid][0] = -1;
dd->lengths[i][lhid][1] = -1;
X_in[0][max_hits] = so->sensor_locations[i * 3 + 0];
X_in[1][max_hits] = so->sensor_locations[i * 3 + 1];
X_in[2][max_hits] = so->sensor_locations[i * 3 + 2];
max_hits++;
}
}
if (max_hits > 2) {
// if( lhid == 0 ) { printf( "\033[H\033[2J" ); }
// printf( "%d\n", max_hits );
FLT tOut[4][4];
FLT S_out[2][SENSORS_PER_OBJECT];
OrthoSolve(tOut, S_out, S_in, X_in, max_hits);
// Now, we need to solve where we are as a function of where
// the lighthouses are.
SurvivePose objpose;
FLT MT[4][4];
objpose.Pos[0] = tOut[0][3];
objpose.Pos[1] = tOut[1][3];
objpose.Pos[2] = tOut[2][3];
// matrix44transpose( MT, &tOut[0][0] );
matrix44copy(&MT[0][0], &tOut[0][0]);
quatfrommatrix(objpose.Rot, &MT[0][0]);
// printf( "QUAT: %f %f %f %f = %f\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat) );
// quat[2] -= 0.005; //fixes up lh0 in test data set.
quatnormalize(objpose.Rot, objpose.Rot);
SurvivePose lh2world = so->ctx->bsd[lhid].Pose;
SurvivePose obj2world;
ApplyPoseToPose(&obj2world, &lh2world, &objpose);
PoserData_poser_pose_func(pd, so, &obj2world);
if (0) {
fprintf(stderr,"INQUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2],
objpose.Rot[3], quatmagnitude(objpose.Rot), objpose.Pos[0], objpose.Pos[1], objpose.Pos[2]);
// fprintf(stderr,"OUQUAT: %f %f %f %f = %f [%f %f %f]\n", poseout.Rot[0], poseout.Rot[1], poseout.Rot[2],
// poseout.Rot[3], quatmagnitude(poseout.Rot), poseout.Pos[0], poseout.Pos[1], poseout.Pos[2]);
}
}
}
// if( so->codename[0] == 'H' ) printf( "LIG:%s %d @ %d rad, %f s (AX %d) (TC %d)\n", so->codename,
// l->sensor_id, l->lh, l->length, dd->nextaxis, l->timecode );
break;
}
case POSERDATA_FULL_SCENE:
{
PoserDataFullScene * fs = (PoserDataFullScene*)pd;
int LH_ID;
SurvivePose alignLh0ToXAxis = {0};
for( LH_ID = 0; LH_ID < 2; LH_ID++ )
{
int i;
int max_hits = 0;
FLT S_in[2][SENSORS_PER_OBJECT];
FLT X_in[3][SENSORS_PER_OBJECT];
for( i = 0; i < SENSORS_PER_OBJECT; i++ )
{
//Load all our valid points into something the LHFinder can use.
if( fs->lengths[i][LH_ID][0] > 0 )
{
FLT out[2];
survive_apply_bsd_calibration(ctx, LH_ID, fs->angles[i][LH_ID], out);
S_in[0][max_hits] = out[0];
S_in[1][max_hits] = out[1];
X_in[0][max_hits] = so->sensor_locations[i*3+0];
X_in[1][max_hits] = so->sensor_locations[i*3+1];
X_in[2][max_hits] = so->sensor_locations[i*3+2];
max_hits++;
}
}
FLT tOut[4][4];
FLT S_out[2][SENSORS_PER_OBJECT];
OrthoSolve( tOut, S_out, S_in, X_in, max_hits );
//Now, we need to solve where we are as a function of where
//the lighthouses are.
// FLT quat[4];
// FLT posoff[3] = { tOut[0][3], tOut[1][3], tOut[2][3] };
SurvivePose objpose;
FLT MT[4][4];
objpose.Pos[0] = tOut[0][3];
objpose.Pos[1] = tOut[1][3];
objpose.Pos[2] = tOut[2][3];
//matrix44transpose( MT, &tOut[0][0] );
matrix44copy( &MT[0][0], &tOut[0][0] );
quatfrommatrix(objpose.Rot, &MT[0][0]);
//printf( "QUAT: %f %f %f %f = %f\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat) );
//quat[2] -= 0.005; //fixes up lh0 in test data set.
quatnormalize(objpose.Rot, objpose.Rot);
printf("QUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2],
objpose.Rot[3], quatmagnitude(objpose.Rot), objpose.Pos[0], objpose.Pos[1], objpose.Pos[2]);
if (0)
;
for (i = 0; i < max_hits; i++) {
FLT pt[3] = {X_in[0][i], X_in[1][i], X_in[2][i]};
quatrotatevector(pt, objpose.Rot, pt);
add3d(pt, pt, objpose.Pos);
printf("OUT %f %f %f ANGLE %f %f AOUT %f %f\n", pt[0], pt[1], pt[2], S_in[0][i], S_in[1][i],
atan2(pt[0], pt[1]), atan2(pt[2], pt[1]));
}
/*
so->FromLHPose[LH_ID].Pos[0] = objpose.Pos[0];
so->FromLHPose[LH_ID].Pos[1] = objpose.Pos[1];
so->FromLHPose[LH_ID].Pos[2] = objpose.Pos[2];
so->FromLHPose[LH_ID].Rot[0] = objpose.Rot[0];
so->FromLHPose[LH_ID].Rot[1] = objpose.Rot[1];
so->FromLHPose[LH_ID].Rot[2] = objpose.Rot[2];
so->FromLHPose[LH_ID].Rot[3] = objpose.Rot[3];
*/
SurvivePose poseout;
InvertPose(&poseout, &objpose);
printf("INQUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2],
objpose.Rot[3], quatmagnitude(objpose.Rot), objpose.Pos[0], objpose.Pos[1], objpose.Pos[2]);
PoserData_lighthouse_pose_func(&fs->hdr, so, LH_ID, &alignLh0ToXAxis, &poseout, 0);
printf("OUQUAT: %f %f %f %f = %f [%f %f %f]\n", poseout.Rot[0], poseout.Rot[1], poseout.Rot[2],
poseout.Rot[3], quatmagnitude(poseout.Rot), poseout.Pos[0], poseout.Pos[1], poseout.Pos[2]);
}
break;
}
case POSERDATA_DISASSOCIATE:
{
free( dd );
so->PoserData = 0;
//printf( "Need to disassociate.\n" );
break;
}
}
return 0;
}
REGISTER_LINKTIME( PoserDaveOrtho );
#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(FLT T[4][4], // OUTPUT: 4x4 transformation matrix
FLT S_out[2][SENSORS_PER_OBJECT], // OUTPUT: array of screenspace points (May not be populated!!!)
FLT S_in[2][SENSORS_PER_OBJECT], // INPUT: array of screenspace points
FLT X_in[3][SENSORS_PER_OBJECT], // INPUT: array of offsets
int nPoints) {
int i,j,k;
FLT R[3][3]; // OUTPUT: 3x3 rotation matrix
FLT trans[3]; // INPUT: x,y,z translation vector
//--------------------
// Remove the center of the HMD offsets, and the screen space
//--------------------
FLT xbar[3] = {0.0, 0.0, 0.0};
FLT sbar[2] = {0.0, 0.0};
FLT S[2][SENSORS_PER_OBJECT];
FLT X[3][SENSORS_PER_OBJECT];
FLT 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
//--------------------
FLT Xt[SENSORS_PER_OBJECT][3];
FLT XXt[3][3];
FLT invXXt[3][3];
FLT SXt[2][3];
FLT M[2][3]; // Morph matrix! (2 by 3)
TRANSP(Xt, X, 3, nPoints);
MUL(XXt, X, Xt, 3, nPoints, 3);
MUL(SXt, S, Xt, 2, nPoints, 3);
INV(invXXt, XXt, 3, 3);
MUL(M, SXt, invXXt, 2, 3, 3);
// PRINT(M,2,3);
// Double checking work
FLT S_morph[2][SENSORS_PER_OBJECT];
MUL(S_morph, M, X, 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
//--------------------
FLT 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]
FLT B[3][1] = { {0.0}, {0.0}, {1.0} };
FLT inv_uM[3][3];
FLT uf[3][1];
INV(inv_uM, uM, 3, 3);
MUL(uf, inv_uM, B, 3, 3, 1);
//--------------------
// Solve for unit length vector
// f that goes into the camera
//--------------------
FLT uf_len = sqrt( uf[0][0]*uf[0][0] + uf[1][0]*uf[1][0] + uf[2][0]*uf[2][0] );
FLT 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);
//FLT check[3][1];
//MUL(uM,uf,check,3,3,1);
//PRINT(check,3,1);
//--------------------
// take cross products to get vectors u,r
//--------------------
FLT 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);
FLT 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
//--------------------
FLT uhat[2][1], rhat[2][1], fhat[2][1];
MUL(fhat, M, f, 2, 3, 1);
MUL(uhat, M, u, 2, 3, 1);
MUL(rhat, M, r, 2, 3, 1);
FLT fhat_len = sqrt( fhat[0][0]*fhat[0][0] + fhat[1][0]*fhat[1][0] );
FLT uhat_len = sqrt( uhat[0][0]*uhat[0][0] + uhat[1][0]*uhat[1][0] );
FLT rhat_len = sqrt( rhat[0][0]*rhat[0][0] + rhat[1][0]*rhat[1][0] );
FLT 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);
*/
// FLT ydist1 = 1.0 / uhat_len; //0.25*PI / uhat_len;
// FLT ydist2 = 1.0 / rhat_len; //0.25*PI / rhat_len;
FLT ydist = 1.0 / urhat_len; // TRICKY XXX Dave operates with "y forward" for some reason...
// printf("ydist %f\n", ydist);
// printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
//--------------------
// Rescale the axies to be of the proper length
//--------------------
FLT x[3][1] = { {M[0][0]*ydist}, {0.0}, {M[1][0]*ydist} };
FLT y[3][1] = { {M[0][1]*ydist}, {0.0}, {M[1][1]*ydist} };
FLT 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 . . .
FLT x_y = sqrt(1.0 - x[0][0]*x[0][0] - x[2][0]*x[2][0]);
FLT y_y = sqrt(1.0 - y[0][0]*y[0][0] - y[2][0]*y[2][0]);
FLT z_y = sqrt(1.0 - z[0][0]*z[0][0] - z[2][0]*z[2][0]);
if( x_y != x_y ) x_y = 0;
if( y_y != y_y ) y_y = 0;
if( z_y != z_y ) z_y = 0;
// Exhaustively flip the minus sign of the z axis until we find the right one . . .
FLT bestErr = 9999.0;
FLT xy_dot2 = x[0][0]*y[0][0] + x[2][0]*y[2][0];
FLT yz_dot2 = y[0][0]*z[0][0] + y[2][0]*z[2][0];
FLT 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
FLT xy_dot = xy_dot2 + x_y*y_y;
FLT yz_dot = yz_dot2 + y_y*z_y;
FLT zx_dot = zx_dot2 + z_y*x_y;
FLT err = _ABS(xy_dot) + _ABS(yz_dot) + _ABS(zx_dot);
// Calculate the handedness
FLT cx,cy,cz;
CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]);
FLT hand = cx*z[0][0] + cy*z_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;
}
// if ( i == 0 && j == 0 && k == 0) { 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);
/*
//-------------------------
// A test version of the rescaling to the proper length
//-------------------------
FLT ydist2 = ydist;
FLT bestBestErr = 9999.0;
FLT bestYdist = 0;
for (ydist2=ydist-0.1; ydist2<ydist+0.1; ydist2+=0.0001)
{
FLT x2[3][1] = { {M[0][0]*ydist2}, {0.0}, {M[1][0]*ydist2} };
FLT y2[3][1] = { {M[0][1]*ydist2}, {0.0}, {M[1][1]*ydist2} };
FLT z2[3][1] = { {M[0][2]*ydist2}, {0.0}, {M[1][2]*ydist2} };
// we know the distance into (or out of) the camera for the z axis,
// but we don't know which direction . . .
FLT x_y = sqrt(1.0 - x2[0][0]*x2[0][0] - x2[2][0]*x2[2][0]);
FLT y_y = sqrt(1.0 - y2[0][0]*y2[0][0] - y2[2][0]*y2[2][0]);
FLT z_y = sqrt(1.0 - z2[0][0]*z2[0][0] - z2[2][0]*z2[2][0]);
if( x_y != x_y ) x_y = 0;
if( y_y != y_y ) y_y = 0;
if( z_y != z_y ) z_y = 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 . . .
FLT bestErr = 9999.0;
FLT xy_dot2 = x2[0][0]*y2[0][0] + x2[2][0]*y2[2][0];
FLT yz_dot2 = y2[0][0]*z2[0][0] + y2[2][0]*z2[2][0];
FLT zx_dot2 = z2[0][0]*x2[0][0] + z2[2][0]*x2[2][0];
for (i=0;i<2;i++) {
for (j=0;j<2;j++) {
for(k=0;k<2;k++) {
// Calculate the error term
FLT xy_dot = xy_dot2 + x_y*y_y;
FLT yz_dot = yz_dot2 + y_y*z_y;
FLT zx_dot = zx_dot2 + z_y*x_y;
FLT err = _ABS(xy_dot) + _ABS(yz_dot) + _ABS(zx_dot);
// Calculate the handedness
FLT cx,cy,cz;
CrossProduct(cx,cy,cz,x2[0][0],x_y,x2[2][0],y2[0][0],y_y,y2[2][0]);
FLT hand = cx*z2[0][0] + cy*z_y + cz*z2[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) { x2[1][0]=x_y; y2[1][0]=y_y; z2[1][0]=z_y; bestErr=err; }
z_y = -z_y;
}
y_y = -y_y;
}
x_y = -x_y;
}
printf("ydist2 %f bestErr %f\n",ydist2,bestErr);
if (bestErr < bestBestErr) {
memcpy(x,x2,3*sizeof(FLT));
memcpy(y,y2,3*sizeof(FLT));
memcpy(z,z2,3*sizeof(FLT));
bestBestErr = bestErr;
bestYdist = ydist2;
}
}
ydist = bestYdist;
*/
/*
for (i=0; i<nPoints; i++) {
FLT x1 = x[0][0]*X[0][i] + y[0][0]*X[1][i] + z[0][0]*X[2][i];
FLT y1 = x[1][0]*X[0][i] + y[1][0]*X[1][i] + z[1][0]*X[2][i];
FLT 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
//--------------------
FLT xhat[2][1] = { {0.0}, {1.0} };
FLT urhat[2][2] = {
{uhat[0][0], uhat[1][0]},
{rhat[0][0], rhat[1][0]} };
FLT inv_urhat[2][2];
FLT ab[2][1];
INV(urhat,inv_urhat,2);
MUL(inv_urhat,xhat,ab,2,2,1);
PRINT(ab,2,1);
FLT a = ab[0][0], b = ab[1][0];
//-------------------
// calculate the xyz coordinate system
//-------------------
FLT y[3][1] = { {f[0][0]}, {f[1][0]}, {f[2][0]} };
FLT 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]} };
FLT 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;
FLT 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]);
FLT 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];
FLT transdist = sqrt( trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2] );
//-------------------
// Pack into the 4x4 transformation matrix, and put into correct "world"-space. I.e. where is the object, relative to
// the lighthouse.
//-------------------
#if 0
//Don't do any transformation to the correct space.
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;
#else
/* WRONG: This transposes.
T[0][0]=-R[0][0]; T[0][1]=-R[0][1]; T[0][2]=-R[0][2]; T[0][3]=-trans[0];
T[2][0]= R[1][0]; T[2][1]= R[1][1]; T[2][2]= R[1][2]; T[2][3]=-trans[1];
T[1][0]= R[2][0]; T[1][1]= R[2][1]; T[1][2]= R[2][2]; T[1][3]=trans[2];
T[3][0]=0.0; T[3][1]=0.0; T[3][2]=0.0; T[3][3]=1.0;
*/
// XXX XXX XXX FOR ANYONE LOOKING TO CONTROL THE COORDINATE FRAME, THESE ARE THE LINES!!!
/** This is probably all wonky and backwards but "looks" right except with multiple transforms, perhaps it's
transposed.
T[0][0]=-R[0][0]; T[0][1]= R[0][1]; T[0][2]= R[0][2]; T[0][3]=-trans[0];
T[2][0]=-R[1][0]; T[2][1]=-R[1][1]; T[2][2]= R[1][2]; T[2][3]=-trans[1];//This Z axis has problems with getting
magnitudes. We can re=make it from the other two axes.
T[1][0]=-R[2][0]; T[1][1]= R[2][1]; T[1][2]=-R[2][2]; T[1][3]= trans[2];
T[3][0]=0.0; T[3][1]=0.0; T[3][2]=0.0; T[3][3]=1.0;
*/
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[2][0];
T[1][1] = R[2][1];
T[1][2] = R[2][2];
T[2][3] = -trans[1];
T[2][0] = R[1][0];
T[2][1] = R[1][1];
T[2][2] = R[1][2];
T[1][3] = trans[2];
T[3][0] = 0.0;
T[3][1] = 0.0;
T[3][2] = 0.0;
T[3][3] = 1.0;
#endif
//-------------------
// Orthogonalize the matrix
//-------------------
// FLT temp[4][4];
// FLT quat[4], quatNorm[4];
// FLT euler[3];
//-------------------
// Orthogonalize the matrix
//-------------------
cross3d(&T[2][0], &T[0][0], &T[1][0]); // Generate Z from X/Y because Z is kinda rekt.
///XXX TODO: Characterize how important these last two steps are. The matrix definitely is not normalized.
#if 1
cross3d(&T[1][0], &T[2][0], &T[0][0] ); //Renormalize rotations...
cross3d(&T[0][0], &T[1][0], &T[2][0] ); //Renormalize rotations...
normalize3d( &T[0][0], &T[0][0] );
normalize3d( &T[1][0], &T[1][0] );
normalize3d( &T[2][0], &T[2][0] );
#endif
// printf( " In Axis on headset \n" );
// printf( " x y z\n" );
// PRINT_MAT(T,4,4);
// PRINT_MAT(T,4,4);
// PRINT_MAT(T,4,4);
// Fix handedness for rotations...
// T[1][0]*=-1;
// T[1][1]*=-1;
// T[1][2]*=-1;
// PRINT_MAT(T,4,4);
#if 0
#if 1
//matrix44copy(T2,T);
matrix44transpose((FLT*)T2,(FLT*)T);
quatfrommatrix( quat, &T2[0][0] );
printf( "QM: %f\n", quatmagnitude( quat ) );
quatnormalize(quatNorm,quat);
quattoeuler(euler,quatNorm);
quattomatrix( &T2[0][0], quatNorm );
PRINT_MAT(T2,4,4);
printf("rot %f %f %f len %f\n", euler[0], euler[1], euler[2], quatmagnitude(quat));
// PRINT(T,4,4);
// matrix44copy(temp,T2);
matrix44transpose((FLT*)temp,(FLT*)T2);
// matrix44transpose(T2, temp);
// memcpy(T2,temp,16*sizeof(float));
for (i=0; i<3; i++) {
for (j=0; j<3; j++) {
T[i][j] = temp[i][j];
}
}
/* PRINT(T2,4,4); */
#endif
T[1][0]*=-1;
T[1][1]*=-1;
T[1][2]*=-1;
/*
CrossProduct(T[0][2],T[1][2],T[2][2], T[0][0],T[1][0],T[2][0], T[0][1],T[1][1],T[2][1]);
CrossProduct(T[0][0],T[1][0],T[2][0], T[0][1],T[1][1],T[2][1], T[0][2],T[1][2],T[2][2]);
CrossProduct(T[0][1],T[1][1],T[2][1], T[0][2],T[1][2],T[2][2], T[0][0],T[1][0],T[2][0]);
float xlen = sqrt(T[0][0]*T[0][0] + T[1][0]*T[1][0] + T[2][0]*T[2][0]);
float ylen = sqrt(T[0][1]*T[0][1] + T[1][1]*T[1][1] + T[2][1]*T[2][1]);
float zlen = sqrt(T[0][2]*T[0][2] + T[1][0]*T[1][2] + T[2][2]*T[2][2]);
T[0][0]/=xlen; T[1][0]/=xlen; T[2][0]/=xlen;
T[0][1]/=ylen; T[1][1]/=ylen; T[2][1]/=ylen;
T[0][2]/=zlen; T[1][2]/=zlen; T[2][2]/=zlen;
*/
// PRINT_MAT(T,4,4);
//-------------------
// Plot the output points
//-------------------
for (i=0; i<nPoints; i++) {
FLT 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];
FLT 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];
FLT 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]);
}
#endif
}
