void controller(float32_t *input, ControllerParams params, float32_t *thetadot, float32_t *error, float32_t dt)
{
	float32_t ctheta, cphi;
	float32_t total;
	
	float32_t tmp_1[3], tmp_2[3], tmp_3[3];
	float32_t err[3];
	
	//Compute total thrust
	cphi = FastCos(params.integral[0]);
	ctheta = FastCos(params.integral[1]);
	total = params.m * params.g / params.k / (cphi * ctheta);
	
	//Compute error and inputs.
	Vector_Multiply_By_Scale(tmp_1, thetadot, params.Kd);
	Vector_Multiply_By_Scale(tmp_2, params.integral, params.Kp);
	Vector_Multiply_By_Scale(tmp_3, params.integral2rd, params.Ki);
	Vector_Add(err, tmp_1, tmp_2);
	Vector_Subtract(err, err, tmp_3);
	err2inputs(input, params, err, total);
	
	// Update controller state.
	Vector_Integral(params.integral, thetadot, dt);
	Vector_Integral(params.integral2rd, params.integral, dt);
	
   if ((thetadot[0] < 0.2f) && (thetadot[1] < 0.2f) && (thetadot[2] < 0.2f)){
		 //recalculate
		 cphi = FastCos(params.integral[0]);
		 ctheta = FastCos(params.integral[1]);
		 total = params.m * params.g / params.k / (cphi * ctheta);
		 //to do
	 }
}
void CWaterSurfaceSceneNode::animateWaterSurface()
{
	if (!Mesh)
		return;
	s32 meshBufferCount = Mesh->getMeshBufferCount();
	f32 time = os::Timer::getTime() / WaveSpeed;

	for (s32 b=0; b<meshBufferCount; ++b)
	{
		s32 vtxCnt = Mesh->getMeshBuffer(b)->getVertexCount();

		switch(Mesh->getMeshBuffer(b)->getVertexType())
		{
		case video::EVT_STANDARD:
		{
			video::S3DVertex* v =
			        (video::S3DVertex*)Mesh->getMeshBuffer(b)->getVertices();

			video::S3DVertex* v2 =
			        (video::S3DVertex*)OriginalMesh->getMeshBuffer(b)->getVertices();

			for (s32 i=0; i<vtxCnt; ++i)
			{
				v[i].Pos.Y = v2[i].Pos.Y +
				             ((f32)FastSin(((v2[i].Pos.X/WaveLength) + time)) * WaveHeight) +
				             ((f32)FastCos(((v2[i].Pos.Z/WaveLength) + time)) * WaveHeight);

			}
//printf("water2c %d %d\n", os::Timer::getRealTime() -d, vtxCnt);
		}
			break;
		case video::EVT_2TCOORDS:
		{
			video::S3DVertex2TCoords* v =
			        (video::S3DVertex2TCoords*)Mesh->getMeshBuffer(b)->getVertices();

			video::S3DVertex2TCoords* v2 =
			        (video::S3DVertex2TCoords*)OriginalMesh->getMeshBuffer(b)->getVertices();

			for (s32 i=0; i<vtxCnt; ++i)
			{
				v[i].Pos.Y = v2[i].Pos.Y +
				             ((f32)FastSin(((v2[i].Pos.X/WaveLength) + time)) * WaveHeight) +
				             ((f32)FastCos(((v2[i].Pos.Z/WaveLength) + time)) * WaveHeight);
			}
//printf("water2c %d %d\n", os::Timer::getRealTime() -d, vtxCnt);

		}
			break;
		} // end switch

	} // end for all mesh buffers

	SceneManager->getMeshManipulator()->recalculateNormals(Mesh);
}
Beispiel #3
0
//
//	EWaving::GetWave
//	returns wave attributes in a point
//
point_wave_s EWaving::GetWave( float x, float y, float depth, float time, uint cutoff )  const
{
	point_wave_s	pw;
	pw.offset	=	0;
	pw.pressure	=	depth * WATER_DENSITY * GRAVITY;

	if (still) {
		if (depth<0) { 
			pw.pressure = 0;
		}
		return pw;
	}
	
	//	compute vertical offset :	
	if (!sin_wave) {
		for (uint i=0; i<cutoff; i++) {
		
			register float	amp		=	wave.waves[i].amplitude;
			register float	freq	=	wave.waves[i].frequency;
			register float	phase	=	wave.waves[i].phase;
			register float	k		=	wave.waves[i].wave_num;

			float x2 = x + y * wave.waves[(i*7)&0xFF].phase / 8.0;

			float	fade	=	(depth<0) ? 1 : exp( - k * depth );

			pw.horizontal_offset	+=	fade * amp * FastCos(freq * time + k * x2 + phase + PI/2);
			pw.offset				+=	fade * amp * FastCos(freq * time + k * x2 + phase);
		}
	} else {
		float	w	=	sin_wave_w;
		float	k	=	w * w / GRAVITY;
		pw.offset	=	sin_wave_a * sin(w * time + k * x);
	}
	
	
	//pw.offset	*=	pw.offset;
	//pw.offset	/=	4.0f;
	
	//	compute pressure offset :	
	pw.pressure	 +=	pw.offset * GRAVITY * WATER_DENSITY;
	
	if ((-depth) > pw.offset) {
		pw.pressure = 0;
	}
	
	return pw;
}
Beispiel #4
0
inline float	EWaving::GetWaveFast( float x, float y, float time ) const
{
	float wave_h = 0;
	
	for (uint i=0; i<WAVE_BAND_NUM; i++) {
	
		float	amp		=	wave.waves[i].amplitude;
		float	freq	=	wave.waves[i].frequency;
		float	phase	=	wave.waves[i].phase;
		float	k		=	wave.waves[i].wave_num;

		float x2 = x + y * wave.waves[(i*7)&0xFF].phase / 8.0;

		wave_h	+=	amp * FastCos(freq * time + k * x2 + phase);
	}
	return wave_h;
}
void ExpSineEffectDistort::render(int time) {
	//update render target
	if(height==480) {
		glViewport(0,0,512,480);
	}
	else {
		glViewport(0,0,512,512);
	}
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);
	ef->render(time);
	render_target->backbuffer_copy(512);
	glViewport(0,0,width,height);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);

	int i,j;
	//update texcoords
	for(i=0;i<(size+1)*(size+1);i++) {
		vert_coords[3*i+2]=-0.2f-0.01f;
	}
	//distort texcoords
	for(i=0;i<(size+1)*(size+1);i++) {
		vert_coords[3*i+2]+=0.05f*(FastCos(vert_coords[3*i+1]*6.0f+0.001f*time)*
			FastCos(vert_coords[3*i]*5.28f+0.0012f*time)*
			FastCos(vert_coords[3*i+1]*9.21f+0.0016f*time));
	}
	if(is_optimized) {
		memcpy((void*)agp_vert_coords,(void*)vert_coords,3*(size+1)*(size+1)*sizeof(float));
	}
	c_state->want_disabled(GL_LIGHTING);
	c_state->want_disabled(GL_FOG);
	c_state->want_disabled(GL_DEPTH_TEST);
	c_state->want_disabled(GL_ALPHA_TEST);
	c_state->want_disabled(GL_BLEND);
	glColor4f(1.0f,1.0f,1.0f,1.0f);
	float x_size=0.266f;
	float y_size=0.2f;
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();
	glFrustum(-0.133,0.133,-0.1,0.1,0.1,2.0);
	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	glLoadIdentity();
	glScalef(0.8f,0.6f,1.0f);
	glTranslatef(-0.5f,-0.5f,0.0f);

	for(i=0;i<max_texture_units;i++) {
		c_state->textures[i].clear_tex_gen_flags();
		c_state->textures[i].enabled=0;
	}
	c_state->textures[0].enabled=1;
	c_state->textures[0].tex=render_target;
	c_state->textures[0].client_state_enabled=1;
	c_state->textures[0].set_texture_env(GL_MODULATE);
	if(is_optimized) {
		c_state->textures[0].coord_pointer=agp_tex_coords;
		glVertexPointer(3,GL_FLOAT,0,agp_vert_coords);
	}
	else {
		c_state->textures[0].coord_pointer=tex_coords;
		glVertexPointer(3,GL_FLOAT,0,vert_coords);
	}
	c_state->prerender_setup();
	if((!is_optimized) && hasVertexArrayRange) {
		glDisableClientState(GL_VERTEX_ARRAY_RANGE_NV);
	}
	glDisableClientState(GL_NORMAL_ARRAY);

	glDrawElements(GL_TRIANGLE_STRIP,2*(size+2)*size,GL_UNSIGNED_SHORT,indices);

	glEnableClientState(GL_NORMAL_ARRAY);
	if((!is_optimized) && hasVertexArrayRange) {
		glEnableClientState(GL_VERTEX_ARRAY_RANGE_NV);
	}
	c_state->postrender_setup();
	glPopMatrix();
	glMatrixMode(GL_PROJECTION);
	glPopMatrix();
	glMatrixMode(GL_MODELVIEW);

}