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