void myLookAt(GLdouble eyeX, GLdouble eyeY, GLdouble eyeZ,
GLdouble centerX, GLdouble centerY, GLdouble centerZ,
GLdouble upX, GLdouble upY, GLdouble upZ) {
// Change the coordinate system to camera coordinates.
// For that, we need the three vectors that represent the camera coordinates
// in world coordinates
// First the cz vector (pointing from Pcamera to Plookat)
GLdouble cz[3];
cz[0] = eyeX - centerX;
cz[1] = eyeY - centerY;
cz[2] = eyeZ - centerZ;
// Normalize cz
normalize3d(cz);
// Then the cy vector (points in the direction of the camera cy vector)
// Get a real orthogonal vector cx first, which is orthogonal to up and cz
GLdouble cx[3];
GLdouble up[3];
up[0] = upX;
up[1] = upY;
up[2] = upZ;
cross_product3d(up, cz, cx);
// Normalize cx
normalize3d(cx);
// Get cy, which is orthogonal to cx and cz
GLdouble cy[3];
cross_product3d(cz, cx, cy);
// Normalize cy
normalize3d(cy);
// Get the rotation matrix R and apply it
GLfloat R[16] = {
cx[0], cy[0], cz[0], 0,
cx[1], cy[1], cz[1], 0,
cx[2], cy[2], cz[2], 0,
0, 0, 0, 1
};
glMultMatrixf(R);
}
/* Similaire à la précédente avec des doubles */
static void set_average_normal3d(half_edge e, gl_vertex* v) {
point3d A,B,C;
vecteur3d_cell AB, AC, V;
A = v->coord.a3d;
GLnormal3d(v,0.0,0.0,0.0);
half_edge e0 = e;
while(e0->prev && e0->prev != e) e0 = e0->prev;
half_edge e1 = e0, e2 = e1 -> next;
unsigned char first = 1;
while(e1 && e2 && (first || e1 != e0)) {
B = e1->opp->vertex->coord.a3d;
C = e2->opp->vertex->coord.a3d;
vec3d(&AB,A,B);
vec3d(&AC,A,C);
vec_prod3d(&V,&AB,&AC);
add3d(e->vertex->normal.a3d,&V);
first = 0;
e1 = e2;
e2 = e1 -> next;
}
normalize3d(e->vertex->normal.a3d);
}
/*Returns the tangent of a point along a spline. Outputs to a 3x1 array of doubles that is assumed to be initiliazed. Takes in pointer to array of splines, interger in that spline, and the u*/
void pTangent3d(double u, spline *in, int x, GLdouble *output) {
point outP;
GLdouble uMatrix[16];
uMatrix[0] = 3*u*u; uMatrix[4] = 2*u; uMatrix[8] = 1; uMatrix[12] = 0;
uMatrix[1] = 1; uMatrix[5] = 1; uMatrix[9] = 1; uMatrix[13] = 1;
uMatrix[2] = 1; uMatrix[6] = 1; uMatrix[10] = 1; uMatrix[14] = 1;
uMatrix[3] = 1; uMatrix[7] = 1; uMatrix[11] = 1; uMatrix[15] = 1;
GLdouble basisMatrix[16];
basisMatrix[0] = -1.0*splineS; basisMatrix[4] = 2.0-splineS; basisMatrix[8] = splineS-2.0; basisMatrix[12] = splineS;
basisMatrix[1] = 2.0*splineS; basisMatrix[5] = splineS-3.0; basisMatrix[9] = 3.0-2.0*splineS; basisMatrix[13] = -1.0*splineS;
basisMatrix[2] = -1.0*splineS; basisMatrix[6] = 0.0; basisMatrix[10] = splineS; basisMatrix[14] = 0.0;
basisMatrix[3] = 0.0; basisMatrix[7] = 1.0; basisMatrix[11] = 0.0; basisMatrix[15] = 0.0;
GLdouble controlM[16];
controlM[0] = (*in).points[x].x; controlM[4] = (*in).points[x].y; controlM[8] =(*in).points[x].z; controlM[12] = 1;
controlM[1] = (*in).points[x+1].x; controlM[5] = (*in).points[x+1].y; controlM[9] =(*in).points[x+1].z; controlM[13] = 1;
controlM[2] = (*in).points[x+2].x; controlM[6] = (*in).points[x+2].y; controlM[10]=(*in).points[x+2].z; controlM[14] = 1;
controlM[3] = (*in).points[x+3].x; controlM[7] = (*in).points[x+3].y; controlM[11]=(*in).points[x+3].z; controlM[15] = 1;
GLdouble halfway[16];
GLdouble tempoutput[16];
matrix4mult(uMatrix, basisMatrix, halfway);
matrix4mult(halfway, controlM, tempoutput);
output[0] = tempoutput[0];
output[1] = tempoutput[4];
output[2] = tempoutput[8];
normalize3d(output);
}
void repere_route(route r, double t, point3d centre, vecteur3d devant, vecteur3d cote, vecteur3d vertical) {
point3d_cell centre_suivant;
calcule3d(centre, r->milieu, t);
calcule3d(¢re_suivant, r->milieu, t+1e-6);
double inclinaison = calcule1d(r->inclinaison, t);
vec3d(devant, centre, ¢re_suivant);
normalize3d(devant);
*vertical = vert0;
vec_prod3d(cote, vertical, devant);
normalize3d(cote);
scal_prod3d(cote, inclinaison);
add3d(vertical, cote);
normalize3d(vertical);
vec_prod3d(cote, devant, vertical);
normalize3d(cote);
}
camera3d_t set_camera_direction(camera3d_t camera, scalar x, scalar y, scalar z,scalar upx,scalar upy,scalar upz) {
scalar* X,*Y,*Z;
X=camera+VEC_X;
Y=camera+VEC_Y;
Z=camera+VEC_Z;
Z[0]=-x;
Z[1]=-y;
Z[2]=-z;
Y[0]=upx;
Y[1]=upy;
Y[2]=upz;
cross3d(Y,Z,X);
cross3d(Z,X,Y);
normalize3d(X);
normalize3d(Y);
normalize3d(Z);
return camera;
}
scalar* camera3d_xy2ray(camera3d_t camera, scalar x, scalar y,scalar* ray) { /* compute a ray <org,dir> from (x,y) in viewport, where org is stored at camera+VEC_ORG, and dir is stored at camera+VEC_DIR*/
scalar d,*projection,*X,*Y,*Z,*T,*org,*dir;
ray=ray?ray:alloc_data(6);
org=ray;
dir=ray+3;
X=camera+VEC_X;
Y=camera+VEC_Y;
Z=camera+VEC_Z;
T=camera+VEC_T;
d=camera[POS_DIST];
projection=camera+PROJECTION;
OP3ABC(org,=,T,+,d*Z);
x/=projection[0];
y/=projection[5];
OP3ABC(dir,=,x*X,+,y*Y);
OP3ABC(dir,=,dir,-,Z);
normalize3d(dir);
return ray;
}
/*Returns a normalized tangent as a point from a given spline input*/
point pTangent(double u, spline *in, int x) {
point outP;
GLdouble uMatrix[16];
uMatrix[0] = 3*u*u; uMatrix[4] = 2*u; uMatrix[8] = 1; uMatrix[12] = 0;
uMatrix[1] = 1; uMatrix[5] = 1; uMatrix[9] = 1; uMatrix[13] = 1;
uMatrix[2] = 1; uMatrix[6] = 1; uMatrix[10] = 1; uMatrix[14] = 1;
uMatrix[3] = 1; uMatrix[7] = 1; uMatrix[11] = 1; uMatrix[15] = 1;
GLdouble basisMatrix[16];
basisMatrix[0] = -1.0*splineS; basisMatrix[4] = 2.0-splineS; basisMatrix[8] = splineS-2.0; basisMatrix[12] = splineS;
basisMatrix[1] = 2.0*splineS; basisMatrix[5] = splineS-3.0; basisMatrix[9] = 3.0-2.0*splineS; basisMatrix[13] = -1.0*splineS;
basisMatrix[2] = -1.0*splineS; basisMatrix[6] = 0.0; basisMatrix[10] = splineS; basisMatrix[14] = 0.0;
basisMatrix[3] = 0.0; basisMatrix[7] = 1.0; basisMatrix[11] = 0.0; basisMatrix[15] = 0.0;
GLdouble controlM[16];
controlM[0] = (*in).points[x].x; controlM[4] = (*in).points[x].y; controlM[8] =(*in).points[x].z; controlM[12] = 1;
controlM[1] = (*in).points[x+1].x; controlM[5] = (*in).points[x+1].y; controlM[9] =(*in).points[x+1].z; controlM[13] = 1;
controlM[2] = (*in).points[x+2].x; controlM[6] = (*in).points[x+2].y; controlM[10]=(*in).points[x+2].z; controlM[14] = 1;
controlM[3] = (*in).points[x+3].x; controlM[7] = (*in).points[x+3].y; controlM[11]=(*in).points[x+3].z; controlM[15] = 1;
GLdouble halfway[16];
GLdouble output[16];
matrix4mult(uMatrix, basisMatrix, halfway);
matrix4mult(halfway, controlM, output);
GLdouble temp[] = {output[0], output[4], output[8]};
normalize3d(temp);
outP.x =temp[0];
outP.y =temp[1];
outP.z =temp[2];
return outP;
}
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);
}
/*Renders the rails as boxes. Draws quads defining the outside of a shape from the previous to current u value. Iterates through all splines*/
void renderRails() {
glBegin(GL_QUADS);
glColor3b(64,16,16);
/*Modifier to make the rail cross-section smaller or larger*/
double w = .05;
double h = .05;
/*Initialize all variables*/
point temp;
point tangentdebug;
GLdouble tangent[] = {0.,0.,0.};
GLdouble arbitrary[] = {1.,0.,1.};
GLdouble prevPoint[] = {0.,0.,0.};
GLdouble curPoint[] = {0.,0.,0.};
GLdouble prevN[] = {0.,0.,0.};
GLdouble curN[] = {0.,0.,0.};
GLdouble prevB[] = {0.,0.,0.};
GLdouble curB[] = {0.,0.,0.};
for ( int x = 0; x < g_iNumOfSplines; x++ ) // For all tracks
for ( int y = 0; y < g_Splines[x].numControlPoints-3; y++) {//For all splines in a track
//Get the previous for the very first spline
temp = p((double)0.0,&(g_Splines[x]), y);
prevPoint[0] = temp.x; prevPoint[1] = temp.y; prevPoint[2] = temp.z;
pTangent3d((double)0.0,&(g_Splines[x]), y, tangent);
tangentdebug = pTangent((double)0.0,&(g_Splines[x]), y);
crossproduct3d(tangent, arbitrary, prevN);
normalize3d(prevN);
crossproduct3d(tangent, prevN, prevB);
normalize3d(prevB);
for (int u = 0; u <= 100; u++) { //For all u's along the track
/*Calculate Tangent, point, N, and B*/
temp = p((double)u/100.0,&(g_Splines[x]), y);
curPoint[0] = temp.x; curPoint[1] = temp.y; curPoint[2] = temp.z;
pTangent3d((double)u/100.0,&(g_Splines[x]), y, tangent);
crossproduct3d(tangent, arbitrary, curN);
normalize3d(curN);
crossproduct3d(tangent, curN, curB);
normalize3d(curB);
/*Define all of the corners of the "box"*/
GLdouble v0[] = {prevPoint[0] + prevN[0]*w - prevB[0]*h ,prevPoint[1] + prevN[1]*w - prevB[1]*h ,prevPoint[2] + prevN[2]*w - prevB[2]*h };
GLdouble v1[] = {prevPoint[0] + prevN[0]*w + prevB[0]*h ,prevPoint[1] + prevN[1]*w + prevB[1]*h ,prevPoint[2] + prevN[2]*w + prevB[2]*h };
GLdouble v2[] = {prevPoint[0] - prevN[0]*w + prevB[0]*h ,prevPoint[1] - prevN[1]*w + prevB[1]*h ,prevPoint[2] - prevN[2]*w + prevB[2]*h };
GLdouble v3[] = {prevPoint[0] - prevN[0]*w - prevB[0]*h ,prevPoint[1] - prevN[1]*w - prevB[1]*h ,prevPoint[2] - prevN[2]*w - prevB[2]*h };
GLdouble v4[] = {curPoint[0] + curN[0]*w - curB[0]*h ,curPoint[1] + curN[1]*w - curB[1]*h ,curPoint[2] + curN[2]*w - curB[2]*h };
GLdouble v5[] = {curPoint[0] + curN[0]*w + curB[0]*h ,curPoint[1] + curN[1]*w + curB[1]*h ,curPoint[2] + curN[2]*w + curB[2]*h };
GLdouble v6[] = {curPoint[0] - curN[0]*w + curB[0]*h ,curPoint[1] - curN[1]*w + curB[1]*h ,curPoint[2] - curN[2]*w + curB[2]*h };
GLdouble v7[] = {curPoint[0] - curN[0]*w - curB[0]*h ,curPoint[1] - curN[1]*w - curB[1]*h ,curPoint[2] - curN[2]*w - curB[2]*h };
/*Draw each poly*/
glVertex3dv(v0); glVertex3dv(v1); glVertex3dv(v5); glVertex3dv(v4);
glVertex3dv(v1); glVertex3dv(v2); glVertex3dv(v6); glVertex3dv(v5);
glVertex3dv(v2); glVertex3dv(v3); glVertex3dv(v7); glVertex3dv(v6);
glVertex3dv(v0); glVertex3dv(v3); glVertex3dv(v7); glVertex3dv(v4);
/*Set current to previous*/
prevPoint[0] = curPoint[0]; prevPoint[1] = curPoint[1]; prevPoint[2] = curPoint[2];
prevN[0] = curN[0]; prevN[1] = curN[1]; prevN[2] = curN[2];
prevB[0] = curB[0]; prevB[1] = curB[1]; prevB[2] = curB[2];
}
}
glEnd();
}
//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;
}
void update_position(route r, position p, double cur_time) {
point3d_cell centre;
vecteur3d_cell devant, cote, dessus;
// calcul du repère à l'index t = p->index
repere_route(r, p->index, ¢re, &devant, &cote, &dessus);
point3d_cell centre_d;
vecteur3d_cell devant_d, cote_d, dessus_d;
// et ç l'index t + dt.
repere_route(r, p->index+1e-6, ¢re_d, &devant_d, &cote_d, &dessus_d);
if (debug) fprintf(stderr, "centre = (%lf, %lf, %lf)\n", centre.x, centre.y , centre.z);
if (debug) fprintf(stderr, "devant = (%lf, %lf, %lf)\n", devant.x, devant.y , devant.z);
if (debug) fprintf(stderr, "cote = (%lf, %lf, %lf)\n", cote.x, cote.y , cote.z);
if (debug) fprintf(stderr, "dessus = (%lf, %lf, %lf)\n", dessus.x, dessus.y , dessus.z);
// cacule du gradient de P(t) = P'(t)
vecteur3d_cell d_centre;
vec3d(&d_centre,¢re,¢re_d);
scal_prod3d(&d_centre, 1e6);
// intervalle de temps écoulé et mise à jour de p->lastime.
double delta_t = cur_time - p->last_update;
p->last_update = cur_time;
if (debug) fprintf(stderr,"delta = %lf\n", delta_t);
/* Rotation : on a directement la vitesse de rotation dans la variable globale rotation */
double alpha = rotation * delta_t;
// on fait cette rotation en modifiant les vecteur p->rel_devant et p->rel_dessus
// on aurait pu faire avec p->devant et p->dessus
vecteur3d_cell rel_cote;
vec_prod3d(&rel_cote, &(p->rel_dessus), &(p->rel_devant));
scal_prod3d(&p->rel_devant, cos(alpha));
translate3d((point3d) &(p->rel_devant), sin(alpha), &rel_cote);
normalize3d(&(p->rel_devant));
if (debug) fprintf(stderr, "p->rel_devant = (%lf, %lf, %lf)\n", p->rel_devant.x, p->rel_devant.y , p->rel_devant.z);
// on calcule les coordonnées absolue de p->devant et p->dessus à partir
// des coordonnées relatives à la route. Elle ne sont pas mie à jour plus tôt.
zero3d(&(p->devant));
translate3d((point3d) &(p->devant), p->rel_devant.x, &devant);
translate3d((point3d) &(p->devant), p->rel_devant.y, &cote);
translate3d((point3d) &(p->devant), p->rel_devant.z, &dessus);
normalize3d(&(p->devant));
if (debug) fprintf(stderr, "p->devant = (%lf, %lf, %lf)\n", p->devant.x, p->devant.y , p->devant.z);
zero3d(&(p->dessus));
translate3d((point3d) &(p->dessus), p->rel_dessus.x, &devant);
translate3d((point3d) &(p->dessus), p->rel_dessus.y, &cote);
translate3d((point3d) &(p->dessus), p->rel_dessus.z, &dessus);
normalize3d(&(p->dessus));
if (debug) fprintf(stderr, "p->dessus = (%lf, %lf, %lf)\n", p->dessus.x, p->dessus.y , p->dessus.z);
/* Mise à jour vitesse, après calcul de l'accélération */
// initialization avec le force de gravité.
vecteur3d_cell accel = { .x = 0.0, .y = 0.0, .z = -9.81 };
if (debug) fprintf(stderr,"acceleration = %lf\n", acceleration);
if (acceleration > 0.0) if (debug) fprintf(stderr, " non zero\n");
// acceleration du à l'accelerateur (dans la variable globale accelerateur
translate3d((point3d) &accel, p->coef_accel*acceleration, &(p->devant));
if (p->z <= 0.0) {
//si on touche la route
//calcul des forttements
double coef2 = fabs(p->y)>r->largeur?r->largeur-fabs(p->y)-p->frot_quad:-p->frot_quad;
translate3d((point3d) &accel, coef2*norm3d(&(p->vitesse))-p->frot_lin, &(p->vitesse));
//projecttion de la vitesse et de l'acceleration pour qu'elles soient parallèle à la route.
double t = dot_prod3d(&accel, &dessus);
translate3d((point3d) &accel, -t, &dessus);
t = dot_prod3d(&(p->vitesse), &dessus);
translate3d((point3d) &accel, -t, &dessus);
}
// on peut enfin mettre à jour la vitesse
if (debug) fprintf(stderr, "accel = (%lf, %lf, %lf)\n", accel.x, accel.y , accel.z);
translate3d((point3d) &(p->vitesse), delta_t, &accel);
p->z = 0.0;
if (debug) fprintf(stderr, "p->vitesse = (%lf, %lf, %lf)\n", p->vitesse.x, p->vitesse.y , p->vitesse.z);
// on met maintenant à jour les coordonnées relative. Attention pour l'index,
// cela dépend de la norme de P'(t) = d_centre.
double d_index = dot_prod3d(&d_centre, &(p->vitesse)) / norm2_3d(&d_centre);
double d_y = dot_prod3d(&cote, &(p->vitesse));
double d_z = dot_prod3d(&dessus, &(p->vitesse));
p->index += d_index * delta_t;
// l'index doit rester entre 0 et 1 !
while (p->index >= 1.0) p->index -= 1.0;
while (p->index < 0.0) p->index += 1.0;
p->y += d_y * delta_t;
p->z += d_z * delta_t;
if (debug) fprintf(stderr, "d_index = %lf, d_y = %lf, d_z = %lf\n", d_index, d_y, d_z);
/* calcul des nouvelles coordonnées absolues */
p->centre = centre;
translate3d(&(p->centre), p->y, &cote);
translate3d(&(p->centre), p->z, &dessus);
if (debug) fprintf(stderr, "p->centre = (%lf, %lf, %lf)\n", p->centre.x, p->centre.y , p->centre.z);
/* calcule des coordonnées relative dans le nouveau reprère, on vient de bouger */
repere_route(r, p->index, ¢re, &devant, &cote, &dessus);
// on calcule d'abord le déplacement.
vecteur3d_cell delta_centre;
vec3d(&delta_centre, ¢re, &(p->centre));
// x ci dessous doit rester petit, mais on fait une simple approximation linéaire et il ne reste pas
// si petit que ça.
double x = dot_prod3d(&delta_centre, &devant);
p->y = dot_prod3d(&delta_centre, &cote);
p->z = dot_prod3d(&delta_centre, &dessus);
if (debug) fprintf(stderr, "x = %lf, p->y = %lf; p->z = %lf\n", x, p->y, p->z);
// mise à jour du vecteur p->rel_devant en fonction du nouveau repère.
p->rel_devant.x = dot_prod3d(&devant, &(p->devant));
p->rel_devant.y = dot_prod3d(&cote, &(p->devant));
p->rel_devant.z = dot_prod3d(&dessus, &(p->devant));
normalize3d(&(p->rel_devant));
/* on garde la caméra parallèle à la route,
sinon ça saute. Dommage, ce n'est pas très realiste
en cas de saut !, mais il faudrait sinon mettre des suspensions et
tenir compte des points de contact, et du moment d'intertie de la voiture */
p->rel_dessus.x = 0.0;
p->rel_dessus.y = 0.0;
p->rel_dessus.z = 1.0;
p->rel_devant.z *= 0.0;
normalize3d(&(p->rel_devant));
if (debug) fprintf(stderr, "p->rel_devant = (%lf, %lf, %lf)\n", p->rel_devant.x, p->rel_devant.y , p->rel_devant.z);
if (debug) fprintf(stderr, "p->rel_dessus = (%lf, %lf, %lf)\n\n\n", p->rel_dessus.x, p->rel_dessus.y , p->rel_dessus.z);
}
void initialize_position(route r, position p, double curtime) {
p->index = 0.0;
p->y = 0.0;
p->z = 0.0;
p->last_update = curtime;
zero3d(&(p->rel_devant));
p->rel_devant.x = 1.0;
zero3d(&(p->rel_dessus));
p->rel_dessus.z = 1.0;
p->coef_accel = 30.0;
p->frot_quad = 0.03;
p->frot_lin = 0.5;
/* calcule des autres champs, on utilise le fait que acceleration = rotation = 0.0
au départ */
update_position(r, p, curtime);
}
