aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Makefile2
-rw-r--r--redist/lintest.c36
-rw-r--r--src/poser_charlesslow.c343
-rw-r--r--src/poser_daveortho.c9
-rw-r--r--src/survive_cal.c37
-rw-r--r--src/survive_config.c2
-rw-r--r--src/survive_data.c1
-rw-r--r--src/survive_vive.c4
8 files changed, 400 insertions, 34 deletions
diff --git a/Makefile b/Makefile
index 14d33bb..29b2164 100644
--- a/Makefile
+++ b/Makefile
@@ -5,7 +5,7 @@ CC:=gcc
CFLAGS:=-Iinclude/libsurvive -I. -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99
LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lm -flto -g
-POSERS:=src/poser_dummy.o src/poser_daveortho.o
+POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o
REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o
LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_vive.o src/survive_config.o src/survive_cal.o
LIBSURVIVE_O:=$(POSERS) $(REDISTS) $(LIBSURVIVE_CORE)
diff --git a/redist/lintest.c b/redist/lintest.c
index 933d2e5..fa5a9d7 100644
--- a/redist/lintest.c
+++ b/redist/lintest.c
@@ -3,15 +3,18 @@
int main()
{
+#if 1
+#define NONTRANSPOSED_DAVE
#ifdef NONTRANSPOSED_DAVE
- FLT pLH1[3] = {-0.275796, 3.105430, -0.868643};
- FLT qLH1[4] = {0.803743, -0.347009, 0.475276, 0.087718};
- FLT pNLH1[3] = { 0.122180, 2.772487, -1.511608 }; //1M +x
- FLT qNLH1[4] = { 0.796702, -0.389329, 0.457024, -0.069414 };
+ FLT pLH1[3] = {-0.396888, 3.182945, -0.568622};
+ FLT qLH1[4] = {0.668640, -0.576296, 0.103727, -0.458305};
+ FLT pNLH1[3] = { 0.113572, 2.791495, -1.495652 }; //1M +x
+ FLT qNLH1[4] = { 0.807419, 0.372818, -0.451339, 0.073308 };
+
- FLT pLH2[3] = {0.120546, 3.252532, -0.098940};
- FLT qLH2[4] = {-0.220026, 0.460836, 0.328534, 0.794534 };
+ FLT pLH2[3] = {0.195579, 3.193770, -0.424473};
+ FLT qLH2[4] = {0.401849, 0.104771, 0.580441, 0.700449};
FLT pNLH2[3] = {-0.183505, 3.356293, 0.695688, };
FLT qNLH2[4] = {-0.237438, 0.405213, 0.270438, 0.840410 };
#else
@@ -30,12 +33,23 @@ int main()
FLT pOut1[3];
FLT pOut2[3];
+ qLH1[0] *= -1;
+ qLH2[0] *= -1;
+
+ quatrotatevector( pOut1, qLH1, pLH1 );
+ quatrotatevector( pOut2, qLH2, pLH2 );
+
+ printf( "%f %f %f\n", PFTHREE( pOut1 ) );
+ printf( "%f %f %f\n", PFTHREE( pOut2 ) );
+
// qLH1[1]*=-1;
// qLH2[0]*=-1;
+/*
sub3d( pOut1, pLH1, pNLH1 );
sub3d( pOut2, pLH2, pNLH2 );
+
printf( "%f %f %f\n", PFTHREE( pOut1 ) );
printf( "%f %f %f\n", PFTHREE( pOut2 ) );
@@ -44,10 +58,13 @@ int main()
printf( "%f %f %f\n", PFTHREE( pOut1 ) );
printf( "%f %f %f\n", PFTHREE( pOut2 ) );
-
+*/
return -1;
-/*
+#endif
+
+#if 0
+
FLT e[3] = { 1,1,3.14 };
FLT q[4];
FLT m[16];
@@ -76,6 +93,7 @@ int main()
FLT pfromlh[3] = { 0, 1, 0 };
+ FLT p[3] = { 0, 1, 0 };
quatrotatevector( p, q, p );
printf( "%f %f %f\n", PFTHREE( p ) );
printf( "Flipping rotation\n" );
@@ -83,12 +101,12 @@ int main()
quatrotatevector( p, q, p );
printf( "%f %f %f\n", PFTHREE( p ) );
-*/
//Try setting up a pose.
// FLT mypose[7] = { 0, 0, 10, q[0], q[1], q[2], q[3] );
// ApplyPoseToPoint( FLT * pout, const FLT * pin, const FLT * pose );
//void InvertPose( FLT * poseout, const FLT * pose );
+#endif
}
diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c
new file mode 100644
index 0000000..3cc7d9d
--- /dev/null
+++ b/src/poser_charlesslow.c
@@ -0,0 +1,343 @@
+#include "survive_cal.h"
+#include <math.h>
+#include <string.h>
+#include "linmath.h"
+#include <survive.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <dclapack.h>
+#include <linmath.h>
+
+static int LH_ID;
+
+typedef struct
+{
+ int something;
+ //Stuff
+} DummyData;
+
+
+static FLT RunOpti( SurviveObject * so, PoserDataFullScene * fs, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat );
+
+int PoserCharlesSlow( SurviveObject * so, PoserData * pd )
+{
+ PoserType pt = pd->pt;
+ SurviveContext * ctx = so->ctx;
+ DummyData * dd = so->PoserData;
+
+ if( !dd ) so->PoserData = dd = malloc( sizeof( DummyData ) );
+
+ 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;
+ //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 );
+ break;
+ }
+ case POSERDATA_FULL_SCENE:
+ {
+ PoserDataFullScene * fs = (PoserDataFullScene*)pd;
+
+ int lh, cycle;
+ FLT dz, dy, dx;
+ for( lh = 0; lh < 2; lh++ )
+ {
+ FLT beste = 1e20;
+
+ FLT LighthousePos[3];
+ FLT LighthouseQuat[4];
+
+ LighthousePos[0] = 0;
+ LighthousePos[1] = 0;
+ LighthousePos[2] = 0;
+ LighthouseQuat[0] = 1;
+ LighthouseQuat[1] = 0;
+ LighthouseQuat[2] = 0;
+ LighthouseQuat[3] = 0;
+
+ FLT bestxyz[3];
+ memcpy( bestxyz, LighthousePos, sizeof( LighthousePos ) );
+
+ //STAGE1 1: Detemine vectoral position from lighthouse to target. Does not determine lighthouse-target distance.
+ //This also is constantly optimizing the lighthouse quaternion for optimal spotting.
+ FLT fullrange = 5; //Maximum search space for positions. (Relative to HMD)
+
+
+ //Sweep whole area 30 times
+ for( cycle = 0; cycle < 30; cycle ++ )
+ {
+
+ //Adjust position, one axis at a time, over and over until we zero in.
+ {
+ FLT bestxyzrunning[3];
+ beste = 1e20;
+
+ FILE * f;
+ if( cycle == 0 )
+ {
+ char filename[1024];
+ sprintf( filename, "calinfo/%d_lighthouse.dat", lh );
+ f = fopen( filename, "wb" );
+ }
+
+ //We split the space into this many groups (times 2) and
+ //if we're on the first cycle, we want to do a very linear
+ //search. As we refine our search we can then use a more
+ //binary search technique.
+ FLT splits = 4;
+ if( cycle == 0 ) splits = 32;
+ if( cycle == 1 ) splits = 13;
+ if( cycle == 2 ) splits = 10;
+ if( cycle == 3 ) splits = 8;
+ if( cycle == 4 ) splits = 5;
+
+ //Wwe search throug the whole space.
+ for( dz = 0; dz < fullrange; dz += fullrange/splits )
+ for( dy = -fullrange; dy < fullrange; dy += fullrange/splits )
+ for( dx = -fullrange; dx < fullrange; dx += fullrange/splits )
+ {
+ //Specificially adjust one axis at a time, searching for the best.
+ memcpy( LighthousePos, bestxyz, sizeof( LighthousePos ) );
+ LighthousePos[0] += dx; //These are adjustments to the "best" from last frame.
+ LighthousePos[1] += dy;
+ LighthousePos[2] += dz;
+
+ FLT ft;
+ //Try refining the search for the best orientation several times.
+ ft = RunOpti(so, fs, lh, 0, LighthousePos, LighthouseQuat);
+ if( cycle == 0 )
+ {
+ float sk = ft*10.;
+ if( sk > 1 ) sk = 1;
+ uint8_t cell = (1.0 - sk) * 255;
+ FLT epsilon = 0.1;
+
+ if( dz == 0 ) { /* Why is dz special? ? */
+ if ( dx > -epsilon && dx < epsilon )
+ cell = 255;
+ if ( dy > -epsilon && dy < epsilon )
+ cell = 128;
+ }
+
+ fprintf( f, "%c", cell );
+ }
+
+ if( ft < beste ) { beste = ft; memcpy( bestxyzrunning, LighthousePos, sizeof( LighthousePos ) ); }
+ }
+
+ if( cycle == 0 )
+ {
+ fclose( f );
+ }
+ memcpy( bestxyz, bestxyzrunning, sizeof( bestxyz ) );
+
+ //Print out the quality of the lock this time.
+ FLT dist = sqrt(bestxyz[0]*bestxyz[0] + bestxyz[1]*bestxyz[1] + bestxyz[2]*bestxyz[2]);
+ printf( "%f %f %f (%f) = %f\n", bestxyz[0], bestxyz[1], bestxyz[2], dist, beste );
+ }
+
+ //Every cycle, tighten up the search area.
+ fullrange *= 0.25;
+ }
+
+ if( beste > 0.1 )
+ {
+ //Error too high
+ SV_ERROR( "LH: %d / Best E %f Error too high\n", lh, beste );
+ return -1;
+ }
+
+ RunOpti(so, fs, lh, 1, LighthousePos, LighthouseQuat);
+
+ ctx->bsd[lh].PositionSet = 1;
+ copy3d( ctx->bsd[lh].Pose.Pos, LighthousePos );
+ quatcopy( ctx->bsd[lh].Pose.Rot, LighthouseQuat );
+#define ALT_COORDS
+#ifdef ALT_COORDS
+ so->FromLHPose[lh].Pos[0] = LighthousePos[0];
+ so->FromLHPose[lh].Pos[1] = LighthousePos[1];
+ so->FromLHPose[lh].Pos[2] = LighthousePos[2];
+ so->FromLHPose[lh].Rot[0] =-LighthouseQuat[0];
+ so->FromLHPose[lh].Rot[1] = LighthouseQuat[1];
+ so->FromLHPose[lh].Rot[2] = LighthouseQuat[2];
+ so->FromLHPose[lh].Rot[3] = LighthouseQuat[3];
+
+ quatrotatevector( so->FromLHPose[lh].Pos, so->FromLHPose[lh].Rot, so->FromLHPose[lh].Pos );
+#else
+ so->FromLHPose[lh].Pos[0] = LighthousePos[0];
+ so->FromLHPose[lh].Pos[1] = LighthousePos[1];
+ so->FromLHPose[lh].Pos[2] = LighthousePos[2];
+ so->FromLHPose[lh].Rot[0] = LighthouseQuat[0];
+ so->FromLHPose[lh].Rot[1] = LighthouseQuat[1];
+ so->FromLHPose[lh].Rot[2] = LighthouseQuat[2];
+ so->FromLHPose[lh].Rot[3] = LighthouseQuat[3];
+#endif
+ }
+
+ break;
+ }
+ case POSERDATA_DISASSOCIATE:
+ {
+ free( dd );
+ so->PoserData = 0;
+ //printf( "Need to disassociate.\n" );
+ break;
+ }
+ }
+
+}
+
+
+REGISTER_LINKTIME( PoserCharlesSlow );
+
+
+
+static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat )
+{
+ int i, p;
+ FLT UsToTarget[3];
+ FLT LastUsToTarget[3];
+ FLT mux = .9;
+ quatsetnone( LighthouseQuat );
+ FLT * hmd_points = hmd->sensor_locations;
+ FLT * hmd_normals = hmd->sensor_normals;
+
+ int first = 1, second = 0;
+
+ //First check to see if this is a valid viewpoint.
+ //If a sensor is pointed away from where we are testing a possible lighthouse position.
+ //BUT We get data from that light house, then we KNOW this is not a possible
+ //lighthouse position.
+ for( p = 0; p < 32; p++ )
+ {
+ int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2;
+ if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue;
+ FLT me_to_dot[3];
+ sub3d( me_to_dot, LighthousePos, &hmd_points[p*3] );
+ float dot = dot3d( &hmd_normals[p*3], me_to_dot );
+ if( dot < -.01 ) { return 1000; }
+ }
+ int iters = 6;
+
+ //Iterate over a refinement of the quaternion that constitutes the
+ //lighthouse.
+ for( i = 0; i < iters; i++ )
+ {
+ first = 1;
+ for( p = 0; p < 32; p++ )
+ {
+ int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2;
+ if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue;
+
+ //Find out where our ray shoots forth from.
+ FLT ax = fs->angles[p][lh][0];
+ FLT ay = fs->angles[p][lh][1];
+ //NOTE: Inputs may never be output with cross product.
+ //Create a fictitious normalized ray. Imagine the lighthouse is pointed
+ //straight in the +z direction, this is the lighthouse ray to the point.
+ FLT RayShootOut[3] = { sin(ax), sin(ay), 0 };
+ RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
+ FLT RayShootOutWorld[3];
+
+ quatnormalize( LighthouseQuat, LighthouseQuat );
+ //Rotate that ray by the current rotation estimation.
+ quatrotatevector( RayShootOutWorld, LighthouseQuat, RayShootOut );
+
+ //Find a ray from us to the target point.
+ sub3d( UsToTarget, &hmd_points[p*3], LighthousePos );
+ if( magnitude3d( UsToTarget ) < 0.0001 ) { continue; }
+ normalize3d( UsToTarget, UsToTarget );
+
+ FLT RotatedLastUs[3];
+ quatnormalize( LighthouseQuat, LighthouseQuat );
+ quatrotatevector( RotatedLastUs, LighthouseQuat, LastUsToTarget );
+
+ //Rotate the lighthouse around this axis to point at the HMD.
+ //If it's the first time, the axis is synthesized, if it's after that, use most recent point.
+ FLT ConcatQuat[4];
+ FLT AxisToRotate[3];
+ if( first )
+ {
+ cross3d( AxisToRotate, RayShootOutWorld, UsToTarget );
+ if( magnitude3d(AxisToRotate) < 0.0001 ) break;
+ normalize3d( AxisToRotate, AxisToRotate );
+ //Don't need to worry about being negative, cross product will fix it.
+ FLT RotateAmount = anglebetween3d( RayShootOutWorld, UsToTarget );
+ quatfromaxisangle( ConcatQuat, AxisToRotate, RotateAmount );
+ quatnormalize( ConcatQuat, ConcatQuat );
+ }
+ else
+ {
+ FLT Target[3];
+ FLT Actual[3];
+
+ copy3d( AxisToRotate, LastUsToTarget );
+ //Us to target = normalized ray from us to where we should be.
+ //RayShootOut = where we would be pointing.
+ sub3d( Target, UsToTarget, AxisToRotate ); //XXX XXX XXX WARNING THIS MESSES STUFF UP.
+ sub3d( Actual, RayShootOutWorld, AxisToRotate );
+ if( magnitude3d( Actual ) < 0.0001 || magnitude3d( Target ) < 0.0001 ) { continue; }
+ normalize3d( Target, Target );
+ normalize3d( Actual, Actual );
+
+ cross3d( AxisToRotate, Actual, Target ); //XXX Check: AxisToRotate should be equal to LastUsToTarget.
+ if( magnitude3d( AxisToRotate ) < 0.000001 ) { continue; }
+ normalize3d( AxisToRotate,AxisToRotate );
+
+ //printf( "%f %f %f === %f %f %f : ", PFTHREE( AxisToRotate ), PFTHREE( LastUsToTarget ) );
+ FLT RotateAmount = anglebetween3d( Actual, Target ) * mux;
+ //printf( "FA: %f (O:%f)\n", acos( dot3d( Actual, Target ) ), RotateAmount );
+ quatfromaxisangle( ConcatQuat, AxisToRotate, RotateAmount );
+ quatnormalize( ConcatQuat, ConcatQuat );
+ }
+
+
+ quatnormalize( ConcatQuat, ConcatQuat );
+ quatnormalize( LighthouseQuat, LighthouseQuat );
+ quatrotateabout( LighthouseQuat, ConcatQuat, LighthouseQuat ); //Checked. This appears to be
+
+ mux = mux * 0.94;
+ if( second ) { second = 0; }
+ if( first ) { first = 0; second = 1; }
+ copy3d( LastUsToTarget, RayShootOutWorld );
+ }
+ }
+
+ //Step 2: Determine error.
+ float errorsq = 0.0;
+ int count = 0;
+ for( p = 0; p < 32; p++ )
+ {
+ int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2;
+ if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue;
+
+ //Find out where our ray shoots forth from.
+ FLT ax = fs->angles[p][lh][0];
+ FLT ay = fs->angles[p][lh][1];
+ FLT RayShootOut[3] = { sin(ax), sin(ay), 0 };
+ RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
+
+ //Rotate that ray by the current rotation estimation.
+ quatrotatevector( RayShootOut, LighthouseQuat, RayShootOut );
+
+ //Point-line distance.
+ //Line defined by LighthousePos & Direction: RayShootOut
+
+ //Find a ray from us to the target point.
+ sub3d( UsToTarget, &hmd_points[p*3], LighthousePos );
+ FLT xproduct[3];
+ cross3d( xproduct, UsToTarget, RayShootOut );
+ FLT dist = magnitude3d( xproduct );
+ errorsq += dist*dist;
+ //if( print ) printf( "%f (%d(%d/%d))\n", dist, p, cd->ctsweeps[dataindex+0], cd->ctsweeps[dataindex+1] );
+ }
+ if( print ) printf( " = %f\n", sqrt( errorsq ) );
+ return sqrt(errorsq);
+}
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
index beba7ad..9f3b55a 100644
--- a/src/poser_daveortho.c
+++ b/src/poser_daveortho.c
@@ -79,10 +79,13 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
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] );
+
+ //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 );
@@ -319,7 +322,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
FLT ydist2 = ydist;
FLT bestBestErr = 9999.0;
FLT bestYdist = 0;
- //for (ydist2=ydist-0.1; ydist2<ydist+0.1; ydist2+=0.0001)
+ 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} };
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 59dd919..1376ee4 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -508,22 +508,24 @@ static void handle_calibration( struct SurviveCalData *cd )
SurvivePose * lhp = &ctx->bsd[lh].Pose;
- lhp->Pos[0] = objfromlh->Pos[0];
+/* lhp->Pos[0] = objfromlh->Pos[0];
lhp->Pos[1] = objfromlh->Pos[1];
- lhp->Pos[2] = objfromlh->Pos[2];
+ lhp->Pos[2] = objfromlh->Pos[2];*/
- lhp->Rot[0] = objfromlh->Rot[0];
+ lhp->Rot[0] =-objfromlh->Rot[0];
lhp->Rot[1] = objfromlh->Rot[1];
lhp->Rot[2] = objfromlh->Rot[2];
lhp->Rot[3] = objfromlh->Rot[3];
+ quatrotatevector( lhp->Pos, lhp->Rot, objfromlh->Pos );
+
//Write lhp from the inverse of objfromlh
//quatrotatevector( lhp->Pos, lhp->Rot, lhp->Pos );
- fprintf( stderr, "%f %f %f\n", objfromlh->Pos[0], objfromlh->Pos[1], objfromlh->Pos[2] );
- fprintf( stderr, "%f %f %f %f\n", objfromlh->Rot[0], objfromlh->Rot[1], objfromlh->Rot[2], objfromlh->Rot[3] );
- fprintf( stderr, "%f %f %f\n", lhp->Pos[0], lhp->Pos[1], lhp->Pos[2] );
+ fprintf( stderr, "%f, %f, %f\n", objfromlh->Pos[0], objfromlh->Pos[1], objfromlh->Pos[2] );
+ fprintf( stderr, "%f, %f, %f, %f\n", objfromlh->Rot[0], objfromlh->Rot[1], objfromlh->Rot[2], objfromlh->Rot[3] );
+
/*
-0.204066 3.238746 -0.856369
0.812203 -0.264897 0.505599 0.120520
@@ -554,23 +556,26 @@ static void handle_calibration( struct SurviveCalData *cd )
SurvivePose * objfromlh = &cd->hmd->FromLHPose[lh];
SurvivePose * lhp = &ctx->bsd[lh].Pose;
- FLT pos[3] = { objfromlh->Pos[0],
- objfromlh->Pos[1],
- objfromlh->Pos[2] };
+ FLT pos[3];
+ quatrotatevector( pos, lhp->Rot, objfromlh->Pos );
pos[0] -= lhp->Pos[0];
pos[1] -= lhp->Pos[1];
pos[2] -= lhp->Pos[2];
- FLT rot[4] = {
- lhp->Rot[0],
- lhp->Rot[1],
- lhp->Rot[2],
- lhp->Rot[3] };
+ //FLT rot[4] = {
+ // [0],
+ // lhp->Rot[1],
+ // lhp->Rot[2],
+ // lhp->Rot[3] };
+ //quatrotatevector( pos, lhp->Rot, pos );
- quatrotatevector( pos, rot, pos );
+ fprintf( stderr, "%f, %f, %f\n", objfromlh->Pos[0], objfromlh->Pos[1], objfromlh->Pos[2] );
+ fprintf( stderr, "%f, %f, %f, %f\n", objfromlh->Rot[0], objfromlh->Rot[1], objfromlh->Rot[2], objfromlh->Rot[3] );
+ fprintf( stderr, "%f, %f, %f\n", lhp->Pos[0], lhp->Pos[1], lhp->Pos[2] );
+ fprintf( stderr, "%f, %f, %f, %f\n", lhp->Rot[0], lhp->Rot[1], lhp->Rot[2], lhp->Rot[3] );
- fprintf( stderr, "====> %f %f %f ",
+ fprintf( stderr, "====> %f %f %f\n",
pos[0], pos[1], pos[2] );
}
diff --git a/src/survive_config.c b/src/survive_config.c
index 3f0f199..10da9a6 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -209,8 +209,6 @@ const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* valu
assert(ptr!=NULL);
cv->data = ptr;
- printf("float array\n");
-
memcpy(cv->data,values,sizeof(FLT)*count);
cv->type = CONFIG_FLOAT_ARRAY;
cv->elements = count;
diff --git a/src/survive_data.c b/src/survive_data.c
index e5f703a..6824b0f 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -12,7 +12,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
//if( so->codename[0] != 'H' )
-// printf( "*** %s %d %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp, le->timestamp-so->tsl );
if( le->sensor_id > SENSORS_PER_OBJECT )
{
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 7da2897..17c9dd5 100644
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -89,8 +89,7 @@ void survive_usb_close( SurviveContext * t );
int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1 );
int survive_usb_poll( SurviveContext * ctx );
int survive_get_config( char ** config, SurviveViveData * ctx, int devno, int interface, int send_extra_magic );
-
-
+int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen );
static void handle_transfer(struct libusb_transfer* transfer)
@@ -301,6 +300,7 @@ int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, s
SV_INFO( "All devices attached." );
+ survive_vive_send_magic(ctx, sv, 1, 0, 0 );
//libUSB initialized. Continue.
return 0;