camera3d_t update_camera(camera3d_t camera) {
scalar *view,*X,*Y,*Z,*T,*V;
scalar d;
view=camera+VIEW;
X=camera+VEC_X;
Y=camera+VEC_Y;
Z=camera+VEC_Z;
T=camera+VEC_T;
d=camera[POS_DIST];
V=camera+VEC_TEMP;
//~ V[0]=-d*Z[0]-T[0]; V[1]=-d*Z[1]-T[1]; V[2]=-d*Z[2]-T[2];
OP3ABC(V,=,-d*Z,-,T)
// compute invert matrix
view[0]=X[0];
view[1]=Y[0];
view[2] =Z[0];
view[3]=0;
view[4]=X[1];
view[5]=Y[1];
view[6] =Z[1];
view[7]=0;
view[8]=X[2];
view[9]=Y[2];
view[10] =Z[2];
view[11]=0;
view[12]=dot3d(V,X);
view[13]=dot3d(V,Y);
view[14]=dot3d(V,Z);
view[15]=1;
return camera;
}
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 dpts = hmd->nr_locations;
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 < dpts; 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] );
FLT 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 < dpts; 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.
FLT errorsq = 0.0;
int count = 0;
for( p = 0; p < dpts; 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);
}
//WARNING: This function probably doesn't work AT ALL!
float * CalculateTangentSpace( int Triangles, int VertexCount, int * Indices, float * verts, float * normals, float * texs )
{
//Here is the place to calculate the Tangent values.
//It is a vector pointing in the direction of increasing u.
int i;
float * tans = 0;
if( normals && texs && verts )
{
//If we have both Texture coords and normals, we can calculate a tangent matrix.
tans = malloc( VertexCount * sizeof( float ) * 4 );
memset( tans, 0, VertexCount * sizeof( float ) * 4 );
//Process modeled after: http://www.terathon.com/code/tangent.html
float * tan1 = malloc( VertexCount * sizeof( float ) * 3 );
float * tan2 = malloc( VertexCount * sizeof( float ) * 3 );
memset( tan1, 0, VertexCount * sizeof( float ) * 3 );
memset( tan2, 0, VertexCount * sizeof( float ) * 3 );
for( i = 0; i < Triangles; i++ )
{
int v1 = Indices[i*3+0];
int v2 = Indices[i*3+1];
int v3 = Indices[i*3+2];
float * t1 = &texs[v1*3];
float * t2 = &texs[v2*3];
float * t3 = &texs[v3*3];
float * p1 = &verts[v1*3];
float * p2 = &verts[v2*3];
float * p3 = &verts[v3*3];
float vec1[3];
float vec2[3];
float tex1[3];
float tex2[3];
sub3d( vec1, p2, p1 ); //(x,y,z)
sub3d( vec2, p3, p1 );
sub3d( tex1, t2, t1 ); //(s,t,u)
sub3d( tex2, t3, t1 );
float r = 1.0f / ( tex1[0] * tex2[1] - tex2[0] * tex1[1] );
float sdir[3] = {
(tex2[1] * vec1[0] - tex1[1] * vec2[0]) * r,
(tex2[1] * vec1[1] - tex1[1] * vec2[1]) * r,
(tex2[1] * vec1[2] - tex1[1] * vec2[2]) * r };
float tdir[3] = {
(tex1[0] * vec2[0] - tex2[0] * vec1[0]) * r,
(tex1[0] * vec2[1] - tex2[0] * vec1[1]) * r,
(tex1[0] * vec2[2] - tex2[0] * vec1[2]) * r };
add3d( &tan1[v1*3], &tan1[v1*3], sdir );
add3d( &tan1[v2*3], &tan1[v2*3], sdir );
add3d( &tan1[v3*3], &tan1[v3*3], sdir );
add3d( &tan2[v1*3], &tan2[v1*3], sdir );
add3d( &tan2[v2*3], &tan2[v2*3], sdir );
add3d( &tan2[v3*3], &tan2[v3*3], sdir );
}
//Normalize and orthoganlize.
for (i = 0; i < VertexCount; i++)
{
const float * n = &normals[i*3];
const float * t = &tan1[i*3];
// Gram-Schmidt orthogonalize
float tmp[3];
float tdn[3];
float thisdot = dot3d( n, t );
scale3d( tdn, n, thisdot );
sub3d( tmp, t, tdn );
normalize3d( &tans[i*4], tmp );
cross3d( tmp, n, t );
tans[i*4+3] = (dot3d( tmp, &tan2[i*3] ) < 0)?-1:1; //set handedness
}
}
return tans;
}
