// begin arcball rotation
void arcball_start(int mx, int my)
{
// saves a copy of the current rotation for comparison
quatcopy(ab_last,ab_quat);
if(ab_planar) ab_start = planar_coords((GLdouble)mx,(GLdouble)my);
else ab_start = sphere_coords((GLdouble)mx,(GLdouble)my);
}
// update current arcball rotation
void arcball_move(int mx, int my)
{
if(ab_planar)
{
ab_curr = planar_coords((GLdouble)mx,(GLdouble)my);
if(ab_curr.equals(ab_start)) return;
// d is motion since the last position
vec d = ab_curr - ab_start;
GLfloat angle = d.length() * 0.5;
GLfloat cosa = cos( angle );
GLfloat sina = sin( angle );
// p is perpendicular to d
vec p = ((ab_out*d.x)-(ab_up*d.y)).unit() * sina;
quaternion(ab_next,p.x,p.y,p.z,cosa);
quatnext(ab_quat,ab_last,ab_next);
// planar style only ever relates to the last point
quatcopy(ab_last,ab_quat);
ab_start = ab_curr;
} else {
ab_curr = sphere_coords((GLdouble)mx,(GLdouble)my);
if(ab_curr.equals(ab_start))
{ // avoid potential rare divide by tiny
quatcopy(ab_quat,ab_last);
return;
}
// use a dot product to get the angle between them
// use a cross product to get the vector to rotate around
GLfloat cos2a = ab_start*ab_curr;
GLfloat sina = sqrt((1.0 - cos2a)*0.5);
GLfloat cosa = sqrt((1.0 + cos2a)*0.5);
vec cross = (ab_start^ab_curr).unit() * sina;
quaternion(ab_next,cross.x,cross.y,cross.z,cosa);
// update the rotation matrix
quatnext(ab_quat,ab_last,ab_next);
}
}
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 p;
FLT * hmd_points = so->sensor_locations;
for( p = 0; p < so->nr_locations; p++ )
{
printf( "%f %f %f\n", hmd_points[p*3+0], hmd_points[p*3+1], hmd_points[p*3+2] );
}
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 = -fullrange; 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 )
{
FLT sk = ft*10.;
if( sk > 1 ) sk = 1;
uint8_t cell = (uint8_t)((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
}
return 0;
}
case POSERDATA_DISASSOCIATE:
{
free( dd );
so->PoserData = 0;
//printf( "Need to disassociate.\n" );
break;
}
}
return -1;
}
