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; }