Switch from pos,quat to pose. Also change initialization order.

1 files changed, 197 insertions, 0 deletions

diff --git a/simple_pose_test.c b/simple_pose_test.c
new file mode 100644
index 0000000..4b392b9
--- /dev/null
+++ b/simple_pose_test.c
@@ -0,0 +1,197 @@
+#ifdef __linux__
+#include <unistd.h>
+#endif
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <survive.h>
+#include "src/survive_cal.h"
+#include <CNFG3D.h>
+#include <os_generic.h>
+#include <CNFGFunctions.h>
+#include <string.h>
+#include <linmath.h>
+struct SurviveContext * ctx;
+
+void HandleKey( int keycode, int bDown )
+{
+	if( !bDown ) return;
+
+	if( keycode == 'O' || keycode == 'o' )
+	{
+		survive_send_magic(ctx,1,0,0);
+	}
+	if( keycode == 'F' || keycode == 'f' )
+	{
+		survive_send_magic(ctx,0,0,0);
+	}
+}
+
+void HandleButton( int x, int y, int button, int bDown )
+{
+}
+
+void HandleMotion( int x, int y, int mask )
+{
+}
+
+void HandleDestroy()
+{
+}
+
+FLT hpos[3];
+FLT hpos2[3];
+
+void testprog_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pose)
+{
+	survive_default_raw_pose_process(so, lighthouse, pose );
+
+	if( lighthouse != 0 || strcmp( so->codename, "HMD" ) != 0 )
+		return;
+
+	// print the pose;
+/*	double qw = quat[0];
+	double qx = quat[1];
+	double qy = quat[2];
+	double qz = quat[3];
+
+	hra = 2 * acos(qw);
+	hrx = qx / sqrt(1-qw*qw);
+	hry = qy / sqrt(1-qw*qw);
+	hrz = qz / sqrt(1-qw*qw);
+	hra *=  180/3.14159;
+
+	hx = pos[0];
+	hy = pos[1];
+	hz = pos[2];*/
+
+	printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [ang:%08.2f %08.2f %08.2f %08.2f]\n", lighthouse, so->codename, pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], pose[6] );
+
+	hpos[0] = pose[0];
+	hpos[1] = pose[1];
+	hpos[2] = pose[2];
+	FLT hposin[3] = { 0, 0, 1 };
+	ApplyPoseToPoint( hpos2, hposin, pose );
+
+	fflush(stdout);
+}
+
+
+void testprog_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh )
+{
+//	printf( "%d: %d %d\n", sensor_id, acode, timecode );
+}
+
+void DrawLineSegment( float x, float y, float z, float x2, float y2, float z2 )
+{
+	//float pti[6] = { 1, sin(TimeSinceStart), cos(TimeSinceStart ), 1, 0, 0};
+	float pa[3] = { x, y, z };
+	float pb[3] = { x2, y2, z2 };
+
+	tdFinalPoint( pa, pa );
+	tdFinalPoint( pb, pb );
+
+	if( pa[2] >= 1.0 ) return;
+	if( pb[2] >= 1.0 ) return;
+	if( pa[2] < 0 ) return;
+	if( pb[2] < 0 ) return;
+
+	int dx, dy;
+	for( dx = 0; dx < 2; dx++ )
+	for( dy = 0; dy < 2; dy++ )
+	{
+		CNFGTackPixel( pa[0]+dx, pa[1]+dy );
+		CNFGTackSegment( pa[0]+dx, pa[1]+dy, pb[0]+dx, pb[1]+dy );
+	}
+}
+
+
+void *GUIThread(void*v)
+{
+	CNFGSetup( "Orientation test", 640, 480 );
+	double fLast = 0;
+	short screenx, screeny;
+	double TimeSinceStart = 0;
+	while(1)
+	{
+		double Now = OGGetAbsoluteTime();
+		double dt = Now - fLast;
+		fLast = Now;
+		TimeSinceStart += dt;
+
+		CNFGClearFrame();
+		CNFGHandleInput();
+
+		CNFGColor( 0xFFFFFF );
+		CNFGGetDimensions( &screenx, &screeny );
+
+		int x, y;
+		float eye[3] = { 8, 8, 8};
+		float at[3] = { 0,0, 0 };
+		float up[3] = { 0,0, 1 };
+
+		tdSetViewport( -1, -1, 1, 1, screenx, screeny );
+
+		tdMode( tdPROJECTION );
+		tdIdentity( gSMatrix );
+		tdPerspective( 40, ((float)screenx)/((float)screeny), .1, 200., gSMatrix );
+
+		tdMode( tdMODELVIEW );
+		tdIdentity( gSMatrix );
+		tdLookAt( gSMatrix, eye, at, up );
+
+		CNFGColor( 0x00ffff ); DrawLineSegment( hpos[0], hpos[1], hpos[2], hpos2[0], hpos2[1], hpos2[2] );
+		CNFGColor( 0x0000ff ); DrawLineSegment( 0, 0, 0, 1, 0, 0  );
+		CNFGColor( 0xff0000 ); DrawLineSegment( 0, 0, 0, 0, 1, 0  );
+		CNFGColor( 0x00ff00 ); DrawLineSegment( 0, 0, 0, 0, 0, 1  );
+		CNFGColor( 0xffffff ); DrawLineSegment( 0, 0, 0, 0, 0, 0  );
+		CNFGSwapBuffers();
+		OGUSleep(10000);
+	}
+}
+
+
+int main()
+{
+	int magicon = 0;
+	double Start = OGGetAbsoluteTime();
+
+	ctx = survive_init( 0 );
+
+	//survive_install_button_fn(ctx, testprog_button_process);
+	survive_install_raw_pose_fn(ctx, testprog_raw_pose_process);
+	//survive_install_imu_fn(ctx, testprog_imu_process);
+	survive_install_raw_pose_fn(ctx, testprog_raw_pose_process);
+	//survive_install_angle_fn(ctx, testprog_angle_process );
+
+	ctx->bsd[0].PositionSet = ctx->bsd[1].PositionSet = 1;
+	int i;
+	for( i = 0; i < 2; i++ )
+	{
+		SurvivePose * p = &ctx->bsd[i].Pose;
+		p->Pos[0] = 0;
+		p->Pos[1] = 0;
+		p->Pos[2] = 0;
+		p->Rot[0] = 1;
+		p->Rot[1] = 0;
+		p->Rot[2] = 0;
+		p->Rot[3] = 0;
+	}
+
+	OGCreateThread( GUIThread, 0 );
+
+	if( !ctx )
+	{
+		fprintf( stderr, "Fatal. Could not start\n" );
+		return 1;
+	}
+
+	//survive_cal_install(ctx);
+	
+
+	while(survive_poll(ctx) == 0)
+	{
+		//Do Stuff
+	}
+}
+