/**
* \file solid_objects.cpp
* \author basile.graf@epfl.ch
* 
* This file relates to all graphical objects. SolidObject is the 3D desription of an object like a boat hull. 
* This file contains also functions to draw navigation instruments on the screen. 
*/

#include "solid_objects.h"




SolidObject hullStruct; ///< central hydroptere hull
SolidObject *hull = &hullStruct;

SolidObject rSwimmStruct; ///< right swimmer
SolidObject *rSwimm = &rSwimmStruct;

SolidObject lSwimmStruct; ///< left swimmer
SolidObject *lSwimm = &lSwimmStruct;

SolidObject bridgeMainStruct; ///< front bridge
SolidObject *bridgeMain = &bridgeMainStruct;

SolidObject bridge2Struct; ///<  bridge 2
SolidObject *bridge2 = &bridge2Struct;

SolidObject mastStruct; ///<  mast
SolidObject *mast = &mastStruct;

SolidObject foilRightStruct; ///<  right foil
SolidObject *foilr = &foilRightStruct;

SolidObject foilLeftStruct; ///<  left foil
SolidObject *foill = &foilLeftStruct;

SolidObject yawStruct; ///<  yaw
SolidObject *yaw = &yawStruct;

SolidObject pitchStruct; ///< pitch
SolidObject *pitch = &pitchStruct;



void SolidObject_create(SolidObject *so, int N, int M)
{// allocate 2D arrays
	int n;

	so->N = N;
	so->M = M;

	so->X = new float* [N];
	so->Y = new float* [N];
	so->Z = new float* [N];

	so->nX = new float* [N];
	so->nY = new float* [N];
	so->nZ = new float* [N];

	so->R = new float* [N];
	so->G = new float* [N];
	so->B = new float* [N];

	for (n=0; n<N; n++)
	{
		so->X[n] = new float[M];
		so->Y[n] = new float[M];
		so->Z[n] = new float[M];

		so->nX[n] = new float[M];
		so->nY[n] = new float[M];
		so->nZ[n] = new float[M];

		so->R[n] = new float[M];
		so->G[n] = new float[M];
		so->B[n] = new float[M];
	}
}


void SolidObject_delete(SolidObject *so)
{//delete 2D arrays
	int n, N;
	N = so->N;

	for (n=0; n<N; n++)
	{
		delete [] so->X[n];
		delete [] so->Y[n];
		delete [] so->Z[n];

		delete [] so->nX[n];
		delete [] so->nY[n];
		delete [] so->nZ[n];

		delete [] so->R[n];
		delete [] so->G[n];
		delete [] so->B[n];
	}
	delete [] so->X;
	delete [] so->Y;
	delete [] so->Z;

	delete [] so->nX;
	delete [] so->nY;
	delete [] so->nZ;

	delete [] so->R;
	delete [] so->G;
	delete [] so->B;
}





void cross_product(float *va, float *vb, float *vc)
{// computes     vc = va x vb
	vc[0] = va[1]*vb[2] - va[2]*vb[1];
	vc[1] = va[2]*vb[0] - va[0]*vb[2];
	vc[2] = va[0]*vb[1] - va[1]*vb[0];
}

void one_normal(float *v1, float *v2, float *v3, float *v4, float *vn)
{// computes     vn = c/|c|,  c = v1 x v2 + v2 x v3 + v3 x v4 + v4 x v1
	float c12[3]; //v1 x v2
	float c23[3]; //v2 x v3
	float c34[3]; //v3 x v4
	float c41[3]; //v4 x v1

	float vectorNorm, temp;

	cross_product(v1, v2, c12);
	cross_product(v2, v3, c23);
	cross_product(v3, v4, c34);
	cross_product(v4, v1, c41);


	vn[0] = (c12[0]) + (c23[0]) + (c34[0]) + (c41[0]);
	vn[1] = (c12[1]) + (c23[1]) + (c34[1]) + (c41[1]);
	vn[2] = (c12[2]) + (c23[2]) + (c34[2]) + (c41[2]);

	vectorNorm = sqrt(vn[0]*vn[0] + vn[1]*vn[1] + vn[2]*vn[2]);

	if (vectorNorm != (0.0f))
	{
		vn[0] /= vectorNorm;
		vn[1] /= vectorNorm;
		vn[2] /= vectorNorm;
	}
	//printf("normed: vn[0]=%f, vn[1]=%f, vn[2]=%f \n",vn[0],vn[1],vn[2]);
}

void SolidObject_compute_normals(SolidObject *so)
{
	int m, n;
	int *kx, *ky;
	int ix, iy;

	float v1[3]; //vectors between vetexes
	float v2[3];
	float v3[3];
	float v4[3];

	float vn[3]; // one normal vector (normalized)

	float **X, **Y, **Z;

	kx = new int [so->N+2];
	ky = new int [so->M+2];

	X = so->X; //solidObject vertexes coordiantes
	Y = so->Y;
	Z = so->Z;

	kx[0]=so->N-1;						 //
	for (n=1; n<so->N+1; n++) kx[n]=n-1; // make index vector kx = {N-1, 0, 1, 2, ..., N-2, N-1, 0}
	kx[so->N+1] = 0;					 // for easy wraping on Solidobject data

	ky[0]=so->M-1;						 //
	for (m=1; m<so->M+1; m++) ky[m]=m-1; // make index vector ky = {M-1, 0, 1, 2, ..., M-2, M-1, 0}
	ky[so->M+1] = 0;					 // for easy wraping on Solidobject data

	//       v2
	//       ¦
	//  v3 --p-- v1
	//       ¦
	//       v4
	//
	//normal at point p:  n = c/|c|,  c = v1 x v2 + v2 x v3 + v3 x v4 + v4 x v1

	for (n=1; n<so->N+1; n++)
	{
		for (m=1; m<so->M+1; m++)
		{
			//compute vectors v1 ... v4
			v1[0] = X[kx[n+1]][ky[m]]-X[kx[n]][ky[m]];
			v1[1] = Y[kx[n+1]][ky[m]]-Y[kx[n]][ky[m]];
			v1[2] = Z[kx[n+1]][ky[m]]-Z[kx[n]][ky[m]];

			v2[0] = X[kx[n]][ky[m+1]]-X[kx[n]][ky[m]];
			v2[1] = Y[kx[n]][ky[m+1]]-Y[kx[n]][ky[m]];
			v2[2] = Z[kx[n]][ky[m+1]]-Z[kx[n]][ky[m]];

			v3[0] = X[kx[n-1]][ky[m]]-X[kx[n]][ky[m]];
			v3[1] = Y[kx[n-1]][ky[m]]-Y[kx[n]][ky[m]];
			v3[2] = Z[kx[n-1]][ky[m]]-Z[kx[n]][ky[m]];

			v4[0] = X[kx[n-1]][ky[m-1]]-X[kx[n]][ky[m]];
			v4[1] = Y[kx[n-1]][ky[m-1]]-Y[kx[n]][ky[m]];
			v4[2] = Z[kx[n-1]][ky[m-1]]-Z[kx[n]][ky[m]];

			one_normal(v1, v2, v3, v4, vn);

			// copy normal vector to solidObject nX, nY and nZ arrays
			so->nX[kx[n]][ky[m]] = vn[0]; 
			so->nY[kx[n]][ky[m]] = vn[1];
			so->nZ[kx[n]][ky[m]] = vn[2];
		}
	}
	
	delete [] kx;
	delete [] ky;
}


void SolidObject_draw(SolidObject *so)
{
	int N, M, n, m;

	N = so->N;
	M = so->M;
	
	glBegin(GL_QUADS);	
	for (n=0; n<N-1; n++)
	{
		for (m=0; m<M-1; m++)
		{
			glColor3f(so->R[n][m], so->G[n][m], so->B[n][m]);
			glNormal3f(so->nX[n][m], so->nY[n][m], so->nZ[n][m]);
			glVertex3f(so->X[n][m], so->Y[n][m], so->Z[n][m]);

			glColor3f(so->R[n+1][m], so->G[n+1][m], so->B[n+1][m]);
			glNormal3f(so->nX[n+1][m], so->nY[n+1][m], so->nZ[n+1][m]);
			glVertex3f(so->X[n+1][m], so->Y[n+1][m], so->Z[n+1][m]);

			glColor3f(so->R[n+1][m+1], so->G[n+1][m+1], so->B[n+1][m+1]);
			glNormal3f(so->nX[n+1][m+1], so->nY[n+1][m+1], so->nZ[n+1][m+1]);
			glVertex3f(so->X[n+1][m+1], so->Y[n+1][m+1], so->Z[n+1][m+1]);

			glColor3f(so->R[n][m+1], so->G[n][m+1], so->B[n][m+1]);
			glNormal3f(so->nX[n][m+1], so->nY[n][m+1], so->nZ[n][m+1]);
			glVertex3f(so->X[n][m+1], so->Y[n][m+1], so->Z[n][m+1]);	
		}
	}
	glEnd();
}


void Sail_create_and_draw(datas *d)
{
	float v1[3], v2[3], vc[3], normv;

	//sail_E:
	v1[0] = d->x_sail - d->x_sail;
	v1[1] = d->y_sail - d->y_sail;
	v1[2] = d->z_foils+2.0f - d->z_foils+d->L_mast;

	v2[0] = d->x_sail - d->Baume/(sqrt(1.0 + d->tanAngle_sail*d->tanAngle_sail)) - d->x_sail;
	v2[1] = d->y_sail - d->Baume*d->tanAngle_sail/(sqrt(1.0 + d->tanAngle_sail*d->tanAngle_sail)) - d->y_sail;
	v2[2] = d->z_foils + 2.0f - d->z_foils+d->L_mast;

	cross_product(v1, v2 ,vc);

	normv = sqrt(vc[0]*vc[0] + vc[1]*vc[1] + vc[2]*vc[2]);

	vc[0] /= normv;
	vc[1] /= normv;
	vc[2] /= normv;

	glBegin(GL_TRIANGLES);
		glNormal3f(vc[0],vc[1],vc[2]);
		glColor3f(0.9f,0.9f,0.9f);
		glVertex3f(d->x_sail, d->y_sail, d->z_foils+2.0f);				
		glVertex3f(d->x_sail, d->y_sail, d->z_foils+d->L_mast);				
		glVertex3f(d->x_sail - d->Baume/(sqrt(1.0 + d->tanAngle_sail*d->tanAngle_sail)), d->y_sail - d->Baume*d->tanAngle_sail/(sqrt(1.0 + d->tanAngle_sail*d->tanAngle_sail)), d->z_foils + 2.0f);
	glEnd();
}


void RelativeWind_draw(datas *d)
{
	glLineWidth(1.0f);
	glColor3f(0.9, 0.2, 0.2);
	glPushMatrix();                              //stock current matrix
	glTranslatef(d->x_sail,d->y_sail,d->z_foils+d->L_mast); //translate it on top of mast
	glRotatef(-57.2958*d->psi,0.0f,0.0f,1.0f);	 //unrotate psi
	glRotatef(-57.2958*d->theta,0.0f,1.0f,0.0f); //unrotate theta
	glRotatef(-57.2958*d->phi,1.0f,0.0f,0.0f);   //unrotate phi
	glBegin(GL_LINES); //draw relative wind vector in boat coordinates (but with "grand coordinate system"-coordinates => wrong)
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(d->Wind_x - d->dx, d->Wind_y - d->dy, 0.0f);
	glEnd();
	glPopMatrix();                               //restore matrix
}


void Target_draw(float x, float y)
{
	int k, N;
	float a;
	glLineWidth(1.0f);
	glNormal3f(0.0,0.0,1.0);

	glColor3f(0.0, 0.0, 0.0);
	glBegin(GL_LINE_STRIP);
	for (a=0.0f; a<6.2834; a+=0.3141f)
	{
		glVertex3f(cos(a),sin(a),0.0f);
	}
	glEnd();

	glColor3f(0.0, 0.0, 0.0);
	glBegin(GL_LINES);
		glVertex3f(-1.1f, 0.0f, 0.0f);
		glVertex3f( 1.1f, 0.0f, 0.0f);
		glVertex3f( 0.0f,-1.1f, 0.0f);
		glVertex3f( 0.0f, 1.1f, 0.0f);
	glEnd();

	glLineWidth(2.0f);
	glColor3f(1.0, 0.0, 0.0);
	glBegin(GL_LINES);
		glVertex3f(-0.15f + x, 0.0f + y, 0.01f);
		glVertex3f( 0.15f + x, 0.0f + y, 0.01f);
		glVertex3f( 0.0f + x,-0.15f + y, 0.01f);
		glVertex3f( 0.0f + x, 0.15f + y, 0.01f);
	glEnd();
}


void VSlider_draw(float x)
{
	glLineWidth(1.0f);
	glColor3f(0.0,0.0,0.0);
	//scale:
	glBegin(GL_LINES);
		glVertex3f(0.0f, 0.5f, 0.0f);
		glVertex3f(0.0f,-0.5f, 0.0f);

		glVertex3f(-0.1f, 0.5f, 0.0f);
		glVertex3f( 0.1f, 0.5f, 0.0f);

		glVertex3f(-0.1f, 0.0f, 0.0f);
		glVertex3f( 0.1f, 0.0f, 0.0f);

		glVertex3f(-0.1f, -0.5f, 0.0f);
		glVertex3f( 0.1f, -0.5f, 0.0f);
	glEnd();

		glLineWidth(2.0f);
		glColor3f(1.0,0.0,0.0);

	//indicator:
	glBegin(GL_LINES);
		glVertex3f(-0.1f, 0.5f*x, 0.01f);
		glVertex3f( 0.1f, 0.5f*x, 0.01f);
	glEnd();
}

void HSlider_draw(float x)
{
	glLineWidth(1.0f);
	glColor3f(0.0,0.0,0.0);
	//scale:
	glBegin(GL_LINES);
		glVertex3f(-0.5f, 0.0f, 0.0f);
		glVertex3f( 0.5f,0.0f, 0.0f);

		glVertex3f( 0.5f,-0.1f, 0.0f);
		glVertex3f( 0.5f, 0.1f, 0.0f);

		glVertex3f( 0.0f,-0.1f, 0.0f);
		glVertex3f( 0.0f, 0.1f, 0.0f);

		glVertex3f(-0.5f,-0.1f, 0.0f);
		glVertex3f(-0.5f, 0.1f, 0.0f);
	glEnd();

		glLineWidth(2.0f);
		glColor3f(1.0,0.0,0.0);

	//indicator:
	glBegin(GL_LINES);
		glVertex3f( 0.5f*x,-0.1f, 0.0f);
		glVertex3f( 0.5f*x, 0.1f, 0.0f);
	glEnd();
}


void Girouette_draw(datas *d) // I don't know the english name for a "girouette" :-) ...
{
	float rx,lx,y, a, arrow_angle, hypo, baume_angle;
	float ax, ay;
	float Vrbx, Vrby, normVb, normVrb, num, denom;
	float va[3], vb[3], vc[3];
	a=0.5236; //Girouette half-opening angle (30 deg)
	y=-cos(a);
	rx=sin(a);
	lx=-rx;
	glLineWidth(1.0f);
	glNormal3f(0.0,0.0,1.0);

	glColor3f(0.0, 0.0, 0.0);
	//girouette arms
	glBegin(GL_LINES);
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(rx, y, 0.0f);

		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(lx, y, 0.0f);
	glEnd();

	//red girouette pads:
	glColor3f(0.85,0.05,0.05);
	glBegin(GL_QUADS);
		glVertex3f(rx+0.1f, y+0.1f, 0.0f);
		glVertex3f(rx-0.1f, y+0.1f, 0.0f);
		glVertex3f(rx-0.1f, y-0.1f, 0.0f);
		glVertex3f(rx+0.1f, y-0.1f, 0.0f);

		glVertex3f(lx+0.1f, y+0.1f, 0.0f);
		glVertex3f(lx-0.1f, y+0.1f, 0.0f);
		glVertex3f(lx-0.1f, y-0.1f, 0.0f);
		glVertex3f(lx+0.1f, y-0.1f, 0.0f);
	glEnd();

	//girouette arrow (wind)
	ax = - (d->Wind_x - d->dx);
	ay = - (d->Wind_y - d->dy);
	hypo = sqrt(ax*ax + ay*ay);
	if (hypo != 0.0)
	{
		Vrbx = d->Wind_x - d->dx;
		Vrby = d->Wind_y - d->dy;
		num = -d->dx*Vrbx - d->dy*Vrby;
		denom = sqrt(d->dx*d->dx + d->dy*d->dy)*sqrt(Vrbx*Vrbx + Vrby*Vrby);
		arrow_angle = 57.2958f*acos(num/denom);
		va[0]=Vrbx;
		va[1]=Vrby;
		va[2]=0.0f;
		vb[0]=-d->dx;
		vb[1]=-d->dy;
		vb[2]=0.0f;
		cross_product(va, vb, vc);
		if (vc[2]>0.0f) arrow_angle *= -1.0f;
		d->angle_girouette = arrow_angle/57.2958f;

	//	if (Vrby<0.0f) arrow_angle *= -1.0f;
		//arrow_angle = 57.2958f*acos(ax/hypo);
		//if (ay<0.0f) arrow_angle = - arrow_angle; //cosinus ambiguity
		//arrow_angle = 57.2958f*(atan(d->tanAngle_sail) - atan(d->Tan_AoA_sail));
		//arrow_angle += 57.2958f*d->psi;
		//printf("ax=%f, ay=%f, angle=%f , psi=%f \n",ax, ay, arrow_angle, 57.2958f*d->psi);
		glLineWidth(2.0f);
		glPushMatrix() ;
		glRotatef(arrow_angle, 0.0f, 0.0f, 1.0f);
		glBegin(GL_LINES);
			glVertex3f(0.0f, 0.0f, 0.0f);
			glVertex3f(0.0f, -0.8f, 0.0f);

			glVertex3f(0.0f, -0.8f, 0.0f);
			glVertex3f(0.1f, -0.7f, 0.0f);
	
			glVertex3f(0.0f, -0.8f, 0.0f);
			glVertex3f(-0.1f, -0.7f, 0.0f);
		glEnd();
		glPopMatrix() ;

		//baume
		baume_angle = atan(d->tanAngle_sail);
		glColor3f(0.9,0.9,0.05);
		glBegin(GL_LINES);
			glVertex3f(0.0f, 0.0f, -0.001f);
			glVertex3f(1.2f*sin(baume_angle), -1.2f*cos(baume_angle), -0.001f);
		glEnd();
	}
}




void Boat_draw()
{
	SolidObject_draw(hull);

	SolidObject_draw(rSwimm);
	SolidObject_draw(lSwimm);

	SolidObject_draw(bridgeMain);
	SolidObject_draw(bridge2);

	SolidObject_draw(mast);

	SolidObject_draw(foilr);
	SolidObject_draw(foill);

	SolidObject_draw(yaw);

	SolidObject_draw(pitch);
}

void Boat_delete()
{
	SolidObject_delete(hull);

	SolidObject_delete(rSwimm);
	SolidObject_delete(lSwimm);

	SolidObject_delete(bridgeMain);
	SolidObject_delete(bridge2);

	SolidObject_delete(mast);

	SolidObject_delete(foilr);
	SolidObject_delete(foill);

	SolidObject_delete(yaw);

	SolidObject_delete(pitch);
}





void Boat_create(datas *d)
{
	int n,m,k,q;
	float temp1, temp2;

	SolidObject_create(hull, 8, 9);
	SolidObject_create(rSwimm, 8, 9);
	SolidObject_create(lSwimm, 8, 9);
	SolidObject_create(bridgeMain, 4, 5);
	SolidObject_create(bridge2, 5, 5);
	SolidObject_create(mast, 2, 5);
	SolidObject_create(foilr, 2, 2);
	SolidObject_create(foill, 2, 2);
	SolidObject_create(yaw, 2, 2);
	SolidObject_create(pitch, 2, 2);
	

//hull coordinates (column by collumn)

	for (k=0; k<9; k++) hull->X[0][k] = d->x_yaw-0.001;

	hull->X[1][0]=d->x_yaw-0.001;
	hull->X[1][8]=d->x_yaw-0.001;
	hull->X[1][6]=d->x_yaw-0.001;
	hull->X[1][1]=d->x_yaw-0.15f-0.001;
	hull->X[1][5]=d->x_yaw-0.15f-0.001;
	hull->X[1][2]=d->x_yaw-0.25f-0.001;
	hull->X[1][4]=d->x_yaw-0.25f-0.001;
	hull->X[1][3]=d->x_yaw-0.3f-0.001;
	hull->X[1][7]=d->x_yaw-0.1f-0.001;

	hull->X[2][0]=d->x_yaw;
	hull->X[2][8]=d->x_yaw;
	hull->X[2][6]=d->x_yaw;
	hull->X[2][1]=d->x_yaw-0.15f;
	hull->X[2][5]=d->x_yaw-0.15f;
	hull->X[2][2]=d->x_yaw-0.25f;
	hull->X[2][4]=d->x_yaw-0.25f;
	hull->X[2][3]=d->x_yaw-0.3f;
	hull->X[2][7]=d->x_yaw-0.1f;

	for (k=0; k<9; k++) hull->X[3][k] = d->x_yaw+(d->Long/3.0);
	for (k=0; k<9; k++) hull->X[4][k] = d->x_yaw+2.0*(d->Long/4.0);
	for (k=0; k<9; k++) hull->X[5][k] = d->x_yaw+3.0*(d->Long/4.0);
	for (k=0; k<9; k++) hull->X[6][k] = d->x_yaw+4.0*(d->Long/4.0);
	for (k=0; k<9; k++) hull->X[7][k] = d->x_yaw+4.0*(d->Long/4.0)-0.01;


	for (k=0; k<9; k++) hull->Y[0][k] = d->y_yaw;

	hull->Y[1][0] = d->y_yaw - 0.3f;
	hull->Y[1][8] = d->y_yaw - 0.3f;
	hull->Y[1][6] = d->y_yaw + 0.3f;
	hull->Y[1][1] = d->y_yaw - 0.3f;
	hull->Y[1][5] = d->y_yaw + 0.3f;
	hull->Y[1][2] = d->y_yaw - 0.2f;
	hull->Y[1][4] = d->y_yaw + 0.2f;
	hull->Y[1][3] = d->y_yaw;
	hull->Y[1][7] = d->y_yaw;
	
	hull->Y[2][0] = d->y_yaw - 0.3f;
	hull->Y[2][8] = d->y_yaw - 0.3f;
	hull->Y[2][6] = d->y_yaw + 0.3f;
	hull->Y[2][1] = d->y_yaw - 0.3f;
	hull->Y[2][5] = d->y_yaw + 0.3f;
	hull->Y[2][2] = d->y_yaw - 0.2f;
	hull->Y[2][4] = d->y_yaw + 0.2f;
	hull->Y[2][3] = d->y_yaw;
	hull->Y[2][7] = d->y_yaw;

	hull->Y[3][0] = d->y_yaw - 0.65f;
	hull->Y[3][8] = d->y_yaw - 0.65f;
	hull->Y[3][6] = d->y_yaw + 0.65f;
	hull->Y[3][1] = d->y_yaw - 0.65f;
	hull->Y[3][5] = d->y_yaw + 0.65f;
	hull->Y[3][2] = d->y_yaw - 0.4f;
	hull->Y[3][4] = d->y_yaw + 0.4f;
	hull->Y[3][3] = d->y_yaw;
	hull->Y[3][7] = d->y_yaw;

	hull->Y[4][0] = d->y_yaw - 0.8f;
	hull->Y[4][8] = d->y_yaw - 0.8f;
	hull->Y[4][6] = d->y_yaw + 0.8f;
	hull->Y[4][1] = d->y_yaw - 0.8f;
	hull->Y[4][5] = d->y_yaw + 0.8f;
	hull->Y[4][2] = d->y_yaw - 0.6f;
	hull->Y[4][4] = d->y_yaw + 0.6f;
	hull->Y[4][3] = d->y_yaw;
	hull->Y[4][7] = d->y_yaw;

	hull->Y[5][0] = d->y_yaw - 0.5f;
	hull->Y[5][8] = d->y_yaw - 0.5f;
	hull->Y[5][6] = d->y_yaw + 0.5f;
	hull->Y[5][1] = d->y_yaw - 0.5f;
	hull->Y[5][5] = d->y_yaw + 0.5f;
	hull->Y[5][2] = d->y_yaw - 0.3f;
	hull->Y[5][4] = d->y_yaw + 0.3f;
	hull->Y[5][3] = d->y_yaw;
	hull->Y[5][7] = d->y_yaw;

	hull->Y[6][0] = d->y_yaw - 0.01f;
	hull->Y[6][8] = d->y_yaw - 0.01f;
	hull->Y[6][6] = d->y_yaw + 0.01f;
	hull->Y[6][1] = d->y_yaw - 0.01f;
	hull->Y[6][5] = d->y_yaw + 0.01f;
	hull->Y[6][2] = d->y_yaw - 0.005f;
	hull->Y[6][4] = d->y_yaw + 0.005f;
	hull->Y[6][3] = d->y_yaw;
	hull->Y[6][7] = d->y_yaw;

	hull->Y[7][0] = d->y_yaw - 0.0f;
	hull->Y[7][8] = d->y_yaw - 0.0f;
	hull->Y[7][6] = d->y_yaw + 0.0f;
	hull->Y[7][1] = d->y_yaw - 0.0f;
	hull->Y[7][5] = d->y_yaw + 0.0f;
	hull->Y[7][2] = d->y_yaw - 0.0f;
	hull->Y[7][4] = d->y_yaw + 0.0f;
	hull->Y[7][3] = d->y_yaw;
	hull->Y[7][7] = d->y_yaw;


	for (k=0; k<9; k++) hull->Z[0][k] = d->z_foils;

	hull->Z[1][7] = d->z_foils + 0.03f;
	hull->Z[1][0] = d->z_foils - 0.0f;
	hull->Z[1][8] = d->z_foils - 0.0f;
	hull->Z[1][6] = d->z_foils - 0.0f;
	hull->Z[1][1] = d->z_foils - 0.3f;
	hull->Z[1][5] = d->z_foils - 0.3f;
	hull->Z[1][2] = d->z_foils - 0.5f;
	hull->Z[1][4] = d->z_foils - 0.5f;
	hull->Z[1][3] = d->z_foils - 0.6f;

	hull->Z[2][7] = d->z_foils + 0.03f;
	hull->Z[2][0] = d->z_foils - 0.0f;
	hull->Z[2][8] = d->z_foils - 0.0f;
	hull->Z[2][6] = d->z_foils - 0.0f;
	hull->Z[2][1] = d->z_foils - 0.3f;
	hull->Z[2][5] = d->z_foils - 0.3f;
	hull->Z[2][2] = d->z_foils - 0.5f;
	hull->Z[2][4] = d->z_foils - 0.5f;
	hull->Z[2][3] = d->z_foils - 0.6f;

	hull->Z[3][7] = d->z_foils + 0.07f;
	hull->Z[3][0] = d->z_foils - 0.0f;
	hull->Z[3][8] = d->z_foils - 0.0f;
	hull->Z[3][6] = d->z_foils - 0.0f;
	hull->Z[3][1] = d->z_foils - 0.4f;
	hull->Z[3][5] = d->z_foils - 0.4f;
	hull->Z[3][2] = d->z_foils - 0.8f;
	hull->Z[3][4] = d->z_foils - 0.8f;
	hull->Z[3][3] = d->z_foils - 1.1f;

	hull->Z[4][7] = d->z_foils + 0.1f;
	hull->Z[4][0] = d->z_foils - 0.0f;
	hull->Z[4][8] = d->z_foils - 0.0f;
	hull->Z[4][6] = d->z_foils - 0.0f;
	hull->Z[4][1] = d->z_foils - 0.4f;
	hull->Z[4][5] = d->z_foils - 0.4f;
	hull->Z[4][2] = d->z_foils - 0.9f;
	hull->Z[4][4] = d->z_foils - 0.9f;
	hull->Z[4][3] = d->z_foils - 1.2f;

	hull->Z[5][7] = d->z_foils + 0.06f;
	hull->Z[5][0] = d->z_foils - 0.0f;
	hull->Z[5][8] = d->z_foils - 0.0f;
	hull->Z[5][6] = d->z_foils - 0.0f;
	hull->Z[5][1] = d->z_foils - 0.3f;
	hull->Z[5][5] = d->z_foils - 0.3f;
	hull->Z[5][2] = d->z_foils - 0.7f;
	hull->Z[5][4] = d->z_foils - 0.7f;
	hull->Z[5][3] = d->z_foils - 1.0f;

	hull->Z[6][7] = d->z_foils + 0.0f;
	hull->Z[6][0] = d->z_foils - 0.0f;
	hull->Z[6][8] = d->z_foils - 0.0f;
	hull->Z[6][6] = d->z_foils - 0.0f;
	hull->Z[6][1] = d->z_foils - 0.2f;
	hull->Z[6][5] = d->z_foils - 0.2f;
	hull->Z[6][2] = d->z_foils - 0.35f;
	hull->Z[6][4] = d->z_foils - 0.35f;
	hull->Z[6][3] = d->z_foils - 0.5f;

	hull->Z[7][7] = d->z_foils + 0.0f;
	hull->Z[7][0] = d->z_foils - 0.0f;
	hull->Z[7][8] = d->z_foils - 0.0f;
	hull->Z[7][6] = d->z_foils - 0.0f;
	hull->Z[7][1] = d->z_foils - 0.2f;
	hull->Z[7][5] = d->z_foils - 0.2f;
	hull->Z[7][2] = d->z_foils - 0.35f;
	hull->Z[7][4] = d->z_foils - 0.35f;
	hull->Z[7][3] = d->z_foils - 0.5f;

	for (q=0; q<8; q++)
	{
		for(k=0; k<9; k++)
		{
			hull->R[q][k]=0.95;
			hull->G[q][k]=0.95;
			hull->B[q][k]=0.95;
		}
	}

	///////////////////////////////////////////////////////////////////

	//swimmers (same as hull but smaller)
	//right swimmer:
	for (q=0; q<8; q++)
	{
		for(k=0; k<9; k++)
		{
			rSwimm->X[q][k] = hull->X[q][k];
			rSwimm->Y[q][k] = hull->Y[q][k] - d->y_yaw;
			rSwimm->Z[q][k] = hull->Z[q][k] - d->z_foils;

			rSwimm->R[q][k] = hull->R[q][k];
			rSwimm->G[q][k] = hull->G[q][k];
			rSwimm->B[q][k] = hull->B[q][k];

			//resize:
			rSwimm->X[q][k] *=0.45;
			rSwimm->Y[q][k] *=0.45;
			rSwimm->Z[q][k] *=0.45;

			//translate:
			rSwimm->X[q][k] += d->x_foils -1.0;
			rSwimm->Y[q][k] += d->y_foil_right;
			rSwimm->Z[q][k] += d->z_foils;			
		}
	}

	//left swimmer:
	for (q=0; q<8; q++)
	{
		for(k=0; k<9; k++)
		{
			lSwimm->X[q][k] = hull->X[q][k];
			lSwimm->Y[q][k] = hull->Y[q][k] - d->y_yaw;
			lSwimm->Z[q][k] = hull->Z[q][k] - d->z_foils;

			lSwimm->R[q][k] = hull->R[q][k];
			lSwimm->G[q][k] = hull->G[q][k];
			lSwimm->B[q][k] = hull->B[q][k];

			//resize:
			lSwimm->X[q][k] *=0.45;
			lSwimm->Y[q][k] *=0.45;
			lSwimm->Z[q][k] *=0.45;

			//translate:
			lSwimm->X[q][k] += d->x_foils -1.0;
			lSwimm->Y[q][k] += d->y_foil_left;
			lSwimm->Z[q][k] += d->z_foils;			
		}
	}


	//hull is too "flat"...:
	for (q=0; q<8; q++)
	{
		for(k=0; k<9; k++)
		{
			hull->Z[q][k]*=1.5;
		}
	}

	/////////////////////////////////////////////////////////////////////

	//Bridge main:
	temp1 = 0.7;
	temp2 = 0.2;

	for (k=0; k<4; k++)
	{
		bridgeMain->X[k][0] = d->x_foils + temp2 - temp1;
		bridgeMain->X[k][1] = d->x_foils + temp2 - temp1;
		bridgeMain->X[k][2] = d->x_foils - temp2 - temp1;
		bridgeMain->X[k][3] = d->x_foils - temp2 - temp1;
		bridgeMain->X[k][4] = d->x_foils + temp2 - temp1;
	}

	for (q=0; q<5; q++)
	{
		bridgeMain->Y[0][q] = d->y_foil_left;
		bridgeMain->Y[1][q] = d->y_foil_left + 0.001*d->y_foil_right;
		bridgeMain->Y[2][q] = d->y_foil_right + 0.001*d->y_foil_left;
		bridgeMain->Y[3][q] = d->y_foil_right;
	}

	for (k=0; k<4; k++)
	{
		bridgeMain->Z[k][0] = d->z_foils + temp2;
		bridgeMain->Z[k][1] = d->z_foils - temp2;
		bridgeMain->Z[k][2] = d->z_foils - temp2;
		bridgeMain->Z[k][3] = d->z_foils + temp2;
		bridgeMain->Z[k][4] = d->z_foils + temp2;
	}

	for (q=0; q<4; q++)
	{
		for(k=0; k<5; k++)
		{
			bridgeMain->R[q][k] = hull->R[q][k];
			bridgeMain->G[q][k] = hull->G[q][k];
			bridgeMain->B[q][k] = hull->B[q][k]; 
		}
	}


	/////////////////////////////////////////////////////////
	// bridge2:

	for (k=0; k<5;k++)
	{
		bridge2->X[k][0] = d->x_foils + temp2 - d->Chord_max + temp1;
		bridge2->X[k][1] = d->x_foils + temp2 - d->Chord_max + temp1;
		bridge2->X[k][2] = d->x_foils - temp2 - d->Chord_max + temp1;
		bridge2->X[k][3] = d->x_foils - temp2 - d->Chord_max + temp1;
		bridge2->X[k][4] = d->x_foils + temp2 - d->Chord_max + temp1;
	}

	for (k=0; k<5;k++)
	{
		bridge2->X[2][k] -= 1.5;
	}


	for (q=0; q<5; q++)
	{
		bridge2->Y[0][q] = d->y_foil_left;
		bridge2->Y[1][q] = d->y_foil_left + 0.001*d->y_foil_right;
		bridge2->Y[2][q] = 0.5*(d->y_foil_right+d->y_foil_left);
		bridge2->Y[3][q] = d->y_foil_right + 0.001*d->y_foil_left;
		bridge2->Y[4][q] = d->y_foil_right;
	}


	for (q=0; q<5; q++)
	{
		bridge2->Z[q][0] = d->z_foils + temp2;
		bridge2->Z[q][1] = d->z_foils - temp2;
		bridge2->Z[q][2] = d->z_foils - temp2;
		bridge2->Z[q][3] = d->z_foils + temp2;
		bridge2->Z[q][4] = d->z_foils + temp2;
	}


	for (q=0; q<5; q++)
	{
		for(k=0; k<5; k++)
		{
			bridge2->R[q][k] = hull->R[q][k];
			bridge2->G[q][k] = hull->G[q][k];
			bridge2->B[q][k] = hull->B[q][k]; 
		}
	}

	////////////////////////////////////////////////////////////
	// mast
	for (k=0; k<2; k++)
	{
		mast->X[k][0] = d->x_sail - 0.18;
		mast->X[k][1] = d->x_sail + 0.18;
		mast->X[k][2] = d->x_sail + 0.18;
		mast->X[k][3] = d->x_sail - 0.18;
		mast->X[k][4] = d->x_sail - 0.18;

		mast->Y[k][0] = d->y_sail - 0.18;
		mast->Y[k][1] = d->y_sail - 0.18;
		mast->Y[k][2] = d->y_sail + 0.18;
		mast->Y[k][3] = d->y_sail + 0.18;
		mast->Y[k][4] = d->y_sail - 0.18;

		mast->nX[k][0] = - sqrt(2.0);
		mast->nX[k][1] = + sqrt(2.0);
		mast->nX[k][2] = + sqrt(2.0);
		mast->nX[k][3] = - sqrt(2.0);
		mast->nX[k][4] = - sqrt(2.0);

		mast->nY[k][0] = - sqrt(2.0);
		mast->nY[k][1] = - sqrt(2.0);
		mast->nY[k][2] = + sqrt(2.0);
		mast->nY[k][3] = + sqrt(2.0);
		mast->nY[k][4] = - sqrt(2.0);
	}
	for (k=0; k<5; k++)
	{
		mast->Z[0][k] = d->z_foils+d->L_mast;
		mast->Z[1][k] = d->z_foils;
		
		for (q=0; q<2; q++)
		{
			mast->nZ[q][k] = 0.0;

			mast->R[q][k] = 0.85;
			mast->G[q][k] = 0.85;
			mast->B[q][k] = 0.95;
		}
	}


	//////////////////////////////////////////////////////////////////
	//right foil:

	foilr->X[0][0] = d->x_foils-d->Chord_min;
	foilr->X[0][1] = d->x_foils;
	foilr->X[1][0] = d->x_foils-d->Chord_max;
	foilr->X[1][1] = d->x_foils;

	foilr->Y[0][0] = d->y_foil_right+cos(d->alpha_right)*d->E_foils;
	foilr->Y[0][1] = d->y_foil_right+cos(d->alpha_right)*d->E_foils;
	foilr->Y[1][0] = d->y_foil_right;
	foilr->Y[1][1] = d->y_foil_right;

	foilr->Z[0][0] = d->z_foils-sin(d->alpha_right)*d->E_foils;
	foilr->Z[0][1] = d->z_foils-sin(d->alpha_right)*d->E_foils;
	foilr->Z[1][0] = d->z_foils;
	foilr->Z[1][1] = d->z_foils;

	for (k=0; k<2; k++)
	{
		for (q=0; q<2; q++)
		{
			foilr->nX[k][q] = 0.0;
			foilr->nY[k][q] = sin(d->alpha_right);
			foilr->nZ[k][q] = cos(d->alpha_right);

			foilr->R[k][q] = hull->R[0][0];
			foilr->G[k][q] = hull->G[0][0];
			foilr->B[k][q] = hull->B[0][0];
		}
	}

	//left foil:

	foill->X[1][0] = d->x_foils-d->Chord_min;
	foill->X[1][1] = d->x_foils;
	foill->X[0][0] = d->x_foils-d->Chord_max;
	foill->X[0][1] = d->x_foils;

	foill->Y[1][0] = d->y_foil_left-cos(d->alpha_left)*d->E_foils;
	foill->Y[1][1] = d->y_foil_left-cos(d->alpha_left)*d->E_foils;
	foill->Y[0][0] = d->y_foil_left;
	foill->Y[0][1] = d->y_foil_left;

	foill->Z[1][0] = d->z_foils-sin(d->alpha_left)*d->E_foils;
	foill->Z[1][1] = d->z_foils-sin(d->alpha_left)*d->E_foils;
	foill->Z[0][0] = d->z_foils;
	foill->Z[0][1] = d->z_foils;

	for (k=0; k<2; k++)
	{
		for (q=0; q<2; q++)
		{
			foill->nX[k][q] = 0.0;
			foill->nY[k][q] = -sin(d->alpha_right);
			foill->nZ[k][q] = cos(d->alpha_right);

			foill->R[k][q] = hull->R[0][0];
			foill->G[k][q] = hull->G[0][0];
			foill->B[k][q] = hull->B[0][0];
		}
	}


	/////////////////////////////////////////////////////////////////////////
	//yaw
	for (q=0; q<2; q++)
	{
		yaw->X[0][q] = d->x_yaw;
		yaw->X[1][q] = d->x_yaw+d->Chord_yaw;

		yaw->Z[q][0] = d->z_foils;
		yaw->Z[q][1] = d->z_yaw-d->E_yaw;
		
		for (k=0; k<2; k++)
		{
			yaw->Y[k][q] = d->y_yaw;

			yaw->nX[k][q] = 0.0;
			yaw->nY[k][q] = 1.0;
			yaw->nZ[k][q] = 0.0;

			yaw->R[k][q] = hull->R[0][0];
			yaw->G[k][q] = hull->G[0][0];
			yaw->B[k][q] = hull->B[0][0];
		}
	}


	/////////////////////////////////////////////////////////////////////////
	//pitch
	for (q=0; q<2; q++)
	{
		pitch->X[q][0] = d->x_pitch;
		pitch->X[q][1] = d->x_pitch+d->Chord_pitch;

		pitch->Y[0][q] = d->y_pitch+0.5*d->Env_pitch;
		pitch->Y[1][q] = d->y_pitch-0.5*d->Env_pitch;

		for (k=0; k<2; k++)
		{
			pitch->Z[k][q] = d->z_pitch;

			pitch->nX[k][q] = 0.0;
			pitch->nY[k][q] = 0.0;
			pitch->nZ[k][q] = 1.0;

			pitch->R[k][q] = hull->R[0][0];
			pitch->G[k][q] = hull->G[0][0];
			pitch->B[k][q] = hull->B[0][0];
		}
	}


	SolidObject_compute_normals(hull);
	SolidObject_compute_normals(rSwimm);
	SolidObject_compute_normals(lSwimm);
	SolidObject_compute_normals(bridgeMain);
	SolidObject_compute_normals(bridge2);

	for (k=0;k<8;k++)//normals errors at junction...:
	{
		hull->nX[k][0] = hull->nX[k][6];
		hull->nX[k][8] = hull->nX[k][6];
		hull->nY[k][0] =-hull->nY[k][6];
		hull->nY[k][8] =-hull->nY[k][6];
		hull->nZ[k][0] = hull->nZ[k][6];
		hull->nZ[k][8] = hull->nZ[k][6];

		rSwimm->nX[k][0] = rSwimm->nX[k][6];
		rSwimm->nX[k][8] = rSwimm->nX[k][6];
		rSwimm->nY[k][0] =-rSwimm->nY[k][6];
		rSwimm->nY[k][8] =-rSwimm->nY[k][6];
		rSwimm->nZ[k][0] = rSwimm->nZ[k][6];
		rSwimm->nZ[k][8] = rSwimm->nZ[k][6];

		lSwimm->nX[k][0] = lSwimm->nX[k][6];
		lSwimm->nX[k][8] = lSwimm->nX[k][6];
		lSwimm->nY[k][0] =-lSwimm->nY[k][6];
		lSwimm->nY[k][8] =-lSwimm->nY[k][6];
		lSwimm->nZ[k][0] = lSwimm->nZ[k][6];
		lSwimm->nZ[k][8] = lSwimm->nZ[k][6];
	}
	// mast normals entered by hand
	// foils normals entered by hand
}

 
