aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--README.md2
-rw-r--r--include/libsurvive/poser.h1
-rw-r--r--src/poser_daveortho.c138
-rw-r--r--src/survive_process.c18
4 files changed, 131 insertions, 28 deletions
diff --git a/README.md b/README.md
index 6639946..ddd2c2d 100644
--- a/README.md
+++ b/README.md
@@ -319,7 +319,7 @@ Given an HMD:
* pointed FACE DOWN at the lighthouse
* 1M above the lighthouse
- Will produce a pose of [[0, 0, -1] [0, 0, 0, 1]* ]. NOTE: The quaternion is inverted. This is the pose of the OBJECT in WORLD space. For our example, the lighthouse is at 0,0,0 in world space. In practicality, the lighthouse will be at some other place in the scene.
+ Will produce a pose of [[0, 0, -1] [0, 1, 0, 0] ]. NOTE: The quaternion is inverted, so rotation axis is arbitrary, point is w is 0. This is the pose of the OBJECT in WORLD space. For our example, the lighthouse is at 0,0,0 in world space. In practicality, the lighthouse will be at some other place in the scene.
The idea is you should be able to take a coordinate local to the HMD and then use ```ApplyPoseToPoint``` to transform that point into world space, by applying the output of the poser.
diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h
index 6c74c52..ccd36ce 100644
--- a/include/libsurvive/poser.h
+++ b/include/libsurvive/poser.h
@@ -15,6 +15,7 @@ typedef enum PoserType_t
POSERDATA_LIGHT, //Single lighting event.
POSERDATA_FULL_SCENE, //Full, statified X, Y sweep data for both lighthouses.
POSERDATA_DISASSOCIATE, //If you get this, it doesn't contain data. It just tells you to please disassociate from the current SurviveObject and delete your poserdata.
+ POSERDATA_SYNC, //Sync pulse.
} PoserType;
typedef void (*poser_raw_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, void *user);
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
index 2bf57c6..0e88a4e 100644
--- a/src/poser_daveortho.c
+++ b/src/poser_daveortho.c
@@ -8,6 +8,8 @@
#include <dclapack.h>
#include <linmath.h>
+//Dave talks about this poser here: https://www.youtube.com/watch?v=nSbEltdH9vM&feature=youtu.be&t=2h29m47s
+
static int LH_ID;
void OrthoSolve(
@@ -20,7 +22,9 @@ void OrthoSolve(
typedef struct
{
- int something;
+ int nextaxis;
+ float lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];
+ float angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];
//Stuff
} DummyData;
@@ -30,7 +34,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
SurviveContext * ctx = so->ctx;
DummyData * dd = so->PoserData;
- if( !dd ) so->PoserData = dd = malloc( sizeof( DummyData ) );
+ if( !dd ) so->PoserData = dd = calloc( sizeof( DummyData ), 1 );
switch( pt )
{
@@ -43,7 +47,77 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
case POSERDATA_LIGHT:
{
PoserDataLight * l = (PoserDataLight*)pd;
- //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode );
+ 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 && l->lh == 0 && so->codename[0] == 'H' )
+ {
+ 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 )
+ {
+ 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] };
+ FLT MT[4][4];
+
+ //matrix44transpose( MT, &tOut[0][0] );
+ matrix44copy( &MT[0][0], &tOut[0][0] );
+
+ quatfrommatrix( quat, &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( quat, quat );
+ printf( "QUAT: %f %f %f %f = %f [%f %f %f]\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat), posoff[0], posoff[1], posoff[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, quat, pt );
+ add3d( pt, pt, posoff );
+ 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] ) );
+ }
+ }
+ }
+
+ //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:
@@ -148,7 +222,7 @@ REGISTER_LINKTIME( PoserDaveOrtho );
void OrthoSolve(
FLT T[4][4], // OUTPUT: 4x4 transformation matrix
- FLT S_out[2][SENSORS_PER_OBJECT], // OUTPUT: array of screenspace points
+ 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)
@@ -437,12 +511,26 @@ PRINT(ab,2,1);
FLT transdist = sqrt( trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2] );
//-------------------
- // Pack into the 4x4 transformation matrix
+ // 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
+
+ //XXX XXX XXX FOR ANYONE LOOKING TO CONTROL THE COORDINATE FRAME, THESE ARE THE LINES!!!
+ 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;
+
+#endif
+
+
FLT T2[4][4];
@@ -450,47 +538,44 @@ PRINT(ab,2,1);
//-------------------
// Orthogonalize the matrix
//-------------------
- FLT temp[4][4];
- FLT quat[4], quatNorm[4];
- FLT euler[3];
+ //FLT temp[4][4];
+ //FLT quat[4], quatNorm[4];
+ //FLT euler[3];
//-------------------
// Orthogonalize the matrix
//-------------------
- PRINT_MAT(T,4,4);
+ //PRINT_MAT(T,4,4);
#if 1
// matrix44transpose(T2, T); //Transpose so we are
matrix44copy((FLT*)T2,(FLT*)T);
- cross3d( &T2[1][0], &T2[0][0], &T2[2][0] );
cross3d( &T2[2][0], &T2[1][0], &T2[0][0] ); //Replace axes in-place.
- matrix44copy((FLT*)T,(FLT*)T2);
+ cross3d( &T2[1][0], &T2[0][0], &T2[2][0] );
+// matrix44copy((FLT*)T,(FLT*)T2);
// matrix44transpose(T, T2);
+ normalize3d( &T[0][0], &T2[0][0] );
+ normalize3d( &T[1][0], &T2[1][0] );
+ normalize3d( &T[2][0], &T2[2][0] );
-#endif
+#else
normalize3d( &T[0][0], &T[0][0] );
normalize3d( &T[1][0], &T[1][0] );
normalize3d( &T[2][0], &T[2][0] );
- //Change handedness
+#endif
- T[1][0]*=-1;
- T[1][1]*=-1;
- T[1][2]*=-1;
+ //Fix handedness for rotations...
+ T[0][0]*=-1;
+ T[0][1]*=-1;
+ T[0][2]*=-1;
+
+// PRINT_MAT(T,4,4);
-/*
- //Check Orthogonality. Yep. It's orthogonal.
- FLT tmp[3];
- cross3d( tmp, &T[0][0], &T[1][0] );
- printf( "M3: %f\n", magnitude3d( tmp ) );
- cross3d( tmp, &T[2][0], &T[1][0] );
- printf( "M3: %f\n", magnitude3d( tmp ) );
- cross3d( tmp, &T[2][0], &T[0][0] );
- printf( "M3: %f\n", magnitude3d( tmp ) );
-*/
// PRINT_MAT(T,4,4);
+#if 0
#if 1
@@ -554,6 +639,7 @@ PRINT(ab,2,1);
//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
}
diff --git a/src/survive_process.c b/src/survive_process.c
index b697f4a..f0928b4 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -19,7 +19,23 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode
}
//We don't use sync times, yet.
- if( acode < -1 ) return;
+ if( sensor_id <= -1 )
+ {
+ if( so->PoserFn )
+ {
+ PoserDataLight l = {
+ .hdr = { .pt = POSERDATA_SYNC, },
+ .sensor_id = sensor_id,
+ .acode = acode,
+ .timecode = timecode,
+ .length = length,
+ .angle = 0,
+ .lh = lh,
+ };
+ so->PoserFn( so, (PoserData *)&l );
+ }
+ return;
+ }
if( base_station > NUM_LIGHTHOUSES ) return;