ivp_controller_buoyancy.cxx

hl2 source code. Do not use it illegal.

// Copyright (C) Ipion Software GmbH 1999-2000. All rights reserved.

#include <ivp_physics.hxx>

#if defined(LINUX) || defined(SUN) || (defined(__MWERKS__) && defined(__POWERPC__))
#   include <alloca.h>
#endif

#ifndef WIN32
#	pragma implementation "ivp_controller_buoyancy.hxx"
#endif

#include <ivp_cache_object.hxx>
#include <ivp_controller_buoyancy.hxx>
#include <ivp_liquid_surface_descript.hxx>
#include <ivp_buoyancy_solver.hxx>


#define DAMPENING_WITH_MOVEVECTOR               1
#define DAMPENING_WITH_PARTICLE_ACCELERATION    1

//#define WITH_DEBUG_OUTPUT                       1


/**************************************************************************
 * Name:        calculate_future_extrapolation(...)
 * Description: extrapolates a new input vector for the multidimensional
 *              interpolator from the current input and the previous input
 **************************************************************************/
IVP_RETURN_TYPE IVP_Controller_Buoyancy::calculate_future_extrapolation(const IVP_Controller_Buoyancy::Attacher_Interpolator::Last_IO *last_io_vectors,
							     const IVP_MI_Vector *new_input,
							     const IVP_MI_Vector *solution_values,
							     const IVP_DOUBLE d_time,
							     const IVP_Time current_time,
							     IVP_MI_Vector *future_input,
							     IVP_MI_Vector *future_solution) {
    
    IVP_DOUBLE future_time = current_time.get_time() + attacher_buoyancy->template_buoyancy.nr_future_psi_for_extrapolation*d_time;
    IVP_DOUBLE divisor = current_time - last_io_vectors->last_psi_time;
    if (divisor > P_DOUBLE_RES) {
	IVP_FLOAT time_quotient = (IVP_FLOAT) (future_time - last_io_vectors->last_psi_time) / divisor;
	
	future_input->set(new_input);
	future_input->subtract(last_io_vectors->last_input_vector);
	future_input->mult( time_quotient );
	future_input->add(last_io_vectors->last_input_vector);
	
	future_solution->set(solution_values);
	future_solution->subtract(last_io_vectors->last_solution_vector);
	future_solution->mult( time_quotient );
	future_solution->add(last_io_vectors->last_solution_vector);

	return(IVP_OK);
    }

    IVP_IF(1) {
	printf("No extrapolation possible!\n");
    }
    return(IVP_FAULT);
}





/*****************************************************************************
 * Name:        use_buoyancy_solver(...)
 * Description: calls the 'IVP_Buoyancy_Solver' and puts the results into
 *              'solution_values_out'
 *****************************************************************************/
IVP_BOOL IVP_Controller_Buoyancy::use_buoyancy_solver(const IVP_Buoyancy_Input *b_input,
						      const IVP_Template_Buoyancy *temp_buoyancy,
						      IVP_Buoyancy_Output *solution_values_out,
						      const IVP_U_Float_Point *resulting_speed_of_current_ws,
						      int index_attacher_interpolator) {

    IVP_Buoyancy_Solver bs( core, this, temp_buoyancy, resulting_speed_of_current_ws );

    IVP_U_Float_Point rel_speed_of_current_os_aligned; rel_speed_of_current_os_aligned.set(&b_input->rel_speed_of_current_os);
    //IVP_U_Float_Point surface_os_aligned; surface_os_aligned.set(&b_input->surface_os);
    IVP_BOOL in_water = bs.compute_forces(
	&rel_speed_of_current_os_aligned,
	&b_input->surface_os,
	attacher_interpolator[index_attacher_interpolator].object);

    if (in_water) {
		
	solution_values_out->volume_under = bs.volume_under;
	solution_values_out->volume_center_under.set(&bs.volume_center_under);
	solution_values_out->object_visible_surface_content_under= bs.object_visible_surface_content_under;
	solution_values_out->sum_impulse.set(&bs.sum_impulse);
	solution_values_out->sum_impulse_x_point.set(&bs.sum_impulse_x_point);
	solution_values_out->sum_impulse_x_movevector.set(&bs.sum_impulse_x_movevector);
	
#if 0
	printf("Buoyancy-Solver's results:\n");
	((IVP_MI_Vector*) solution_values_out)->print();
#endif
		
    }
    return(in_water);
}



/**********************************************************************************
 * Name:        apply_dampening(...)
 * Description: applies the dampening forces to the object provided as a parameter
 **********************************************************************************/
void IVP_Controller_Buoyancy::apply_dampening( IVP_Real_Object *object,
					       IVP_FLOAT object_visible_surface_content_under,
					       IVP_DOUBLE delta_time,
					       IVP_U_Float_Point *sum_impulse_x_movevector,
					       IVP_U_Float_Point *sum_impulse_x_point,
					       IVP_U_Float_Point *sum_impulse) {	
  
#ifdef DAMPENING_WITH_MOVEVECTOR

    //mult the (user definable) torque_factor with the object_visible_surface_content_under, also turns around the move vector (multiply with -1.0f)
    IVP_FLOAT move_factor = -attacher_buoyancy->template_buoyancy.torque_factor * IVP_Inline_Math::ivp_sqrtf(object_visible_surface_content_under) * (0.1f);
	
    //mult sum_impulse_x_movevector with the modified move_factor and delta_PSI_time
    sum_impulse_x_movevector->mult(move_factor * delta_time);
#endif
	
    //mult the other global variables with delta_PSI_time
    sum_impulse_x_point->mult(delta_time);
    sum_impulse->mult(delta_time);
	
	
    //convert sum_impulse into world coordinate system
    IVP_U_Float_Point sum_impulse_ws;
    {
	IVP_Cache_Object *cache_object = object->get_cache_object_no_lock();
	IVP_U_Float_Point imp; imp.set(sum_impulse);
	cache_object->transform_vector_to_world_coords(&imp, &sum_impulse_ws);
    }
	
    IVP_U_Matrix m_core_f_object;
    object->calc_m_core_f_object(&m_core_f_object);
	
    //get the core center
    IVP_U_Point object_center_ws;
    {
	IVP_Cache_Object *co = object->get_cache_object_no_lock();
	object_center_ws.set( co->m_world_f_object.get_position());
    }
	
    //push the core center with the sum of impulses => translation (and a rotation component if the object center is not the same as the core system)
    IVP_U_Float_Point non_float_sum_impulse_ws(&sum_impulse_ws);
		
    core->async_push_core_ws( &object_center_ws, &non_float_sum_impulse_ws);
    IVP_IF(0) {
	printf("core->speed_change.real_length = %f\n", core->speed_change.real_length());
	IVP_ASSERT(core->speed_change.real_length() < 70000.0f);
	core->core_plausible_check();
    }
	
    //convert rotation related values to core system
    IVP_U_Float_Point sum_impulse_x_point_cs;
    IVP_U_Float_Point sum_impulse_x_movevector_cs;
    {
	m_core_f_object.vmult3(sum_impulse_x_point, &sum_impulse_x_point_cs);
	m_core_f_object.vmult3(sum_impulse_x_movevector, &sum_impulse_x_movevector_cs);
    }
	
    {
	//calc new rot_speed
	IVP_U_Float_Point delta_rot_component1_cs;
	delta_rot_component1_cs.set_pairwise_mult(&sum_impulse_x_point_cs, core->get_inv_rot_inertia());
#ifdef DAMPENING_WITH_MOVEVECTOR
	IVP_U_Float_Point delta_rot_component2_cs;
	delta_rot_component2_cs.set_pairwise_mult(&sum_impulse_x_movevector_cs, core->get_inv_rot_inertia());
	delta_rot_component1_cs.add(&delta_rot_component2_cs);
#endif
		
	//apply rotation to object
	core->rot_speed_change.add(&delta_rot_component1_cs);
	IVP_IF(1) {
	    core->core_plausible_check();
	}

	//if object is a ball then apply dampening of rotation which can result e.g. from the ball
	//falling not straightly into the medium
	if (object->get_type() == IVP_BALL) {
	    IVP_Ball *ball = (IVP_Ball *) object;
	    IVP_FLOAT radius = ball->get_radius();
	    IVP_U_Float_Point rotation_dampening;
	    rotation_dampening.set_multiple(&core->rot_speed,
					    attacher_buoyancy->template_buoyancy.ball_rot_dampening_factor *
					    object_visible_surface_content_under / (2*IVP_PI*radius*radius));
	    core->rot_speed_change.subtract(&rotation_dampening);
	    IVP_IF(1) {
		core->core_plausible_check();
	    }
	}
    }

#if 0
    printf("In apply_dampening:\n");
    sum_impulse_x_movevector->print("sum_impulse_x_movevector");
    sum_impulse_x_point->print("sum_impulse_x_point");
#endif
    
    //core->rot_speed_change.print("rot_speed_change");
}



/*********************************************************************************
 * Name:        apply_buoyancy_impulse(...)
 * Description: applies the buoyancy forces to the object provided as a parameter
 *********************************************************************************/
void IVP_Controller_Buoyancy::apply_buoyancy_impulse( IVP_Real_Object *object,
						      IVP_Template_Buoyancy *temp_buoyancy,
						      IVP_DOUBLE delta_time,
						      IVP_FLOAT volume_under,
						      IVP_U_Float_Point *volume_center_under) {
	
    //vector indicating the direction the forces will push
    IVP_U_Float_Point dir_ws(core->environment->get_gravity());  //the buoyancy impulse works in the opposite direction of the gravity
    
    //compute the buoyancy of the part under the surface
    if (volume_under > temp_buoyancy->buoyancy_eps) {
		
	//convert the IVP_U_Float_Point volume_center_under to IVP_U_Point (ws)
	//to preserve precision in the world system
	IVP_U_Point volume_center_under_ws;
	{
	    IVP_Cache_Object *cache_object = object->get_cache_object_no_lock();  //needed to transform coordinates into other coord. systems
	    cache_object->transform_position_to_world_coords(volume_center_under, &volume_center_under_ws);
	}
		
	IVP_DOUBLE force_under = volume_under * (-temp_buoyancy->medium_density) * (delta_time);
	IVP_U_Float_Point impulse_ws;
	impulse_ws.set_multiple(&dir_ws, force_under);

	IVP_IF(0) {
	    impulse_ws.print("buoyancy-impulse");
	    volume_center_under_ws.print("volume_center_under_ws");
	    volume_center_under->print("volume_center_under_os");
	    printf("volume_under = %f\n\n", volume_under);
	}
		
	core->async_push_core_ws(&volume_center_under_ws, &impulse_ws);

	IVP_IF(1) {
	    core->core_plausible_check();
	}
    }
}



//prints the values calculated by the buoyancy solver to stdout
void ivp_debug_show_real_values(const IVP_Buoyancy_Input * /*b_input*/,
				IVP_Controller_Buoyancy *cntrl,
				const IVP_Template_Buoyancy *temp_buoyancy,
				IVP_Core *core,
				IVP_Real_Object *object,
				const IVP_U_Float_Point *resulting_speed_of_current_ws) {
  IVP_USE(object);
    IVP_Buoyancy_Output solution_values_out;
    IVP_Buoyancy_Solver bs( core, cntrl, temp_buoyancy, resulting_speed_of_current_ws );
    //    IVP_BOOL in_water = bs.compute_forces(&b_input->rel_speed_of_current_os, &b_input->surface_os, object);

	solution_values_out.volume_under = bs.volume_under;
	solution_values_out.object_visible_surface_content_under= bs.object_visible_surface_content_under;
	solution_values_out.sum_impulse.set(&bs.sum_impulse);
	solution_values_out.sum_impulse_x_point.set(&bs.sum_impulse_x_point);
	solution_values_out.sum_impulse_x_movevector.set(&bs.sum_impulse_x_movevector);
	
	printf("Buoyancy-Solver's results:\n");
	((IVP_MI_Vector*)&solution_values_out)->print();
		
}



/*************************************************************************************
     * Name:        provide_new_input_solution_combination(...)
     * Description: combines various ways of inserting new vectors of input and solution
     *              values into 'previous_inputs' and 'previous_solutions' of the current
     *              'IVP_Multidimensional_Interpolator' instance
     *************************************************************************************/
void IVP_Controller_Buoyancy::provide_new_input_solution_combination(Attacher_Interpolator *ai,
								     IVP_Template_Buoyancy *temp_buoyancy,
								     const IVP_MI_Vector *weighted_new_input,
								     const IVP_MI_Vector *solution_values,
								     const IVP_DOUBLE d_time,
								     const IVP_Time current_time) {

	float input_data[ INPUT_VECTOR_LENGTH ];
	float solution_data[ SOLUTION_VECTOR_LENGTH ];
	IVP_MI_Vector *future_input = (IVP_MI_Vector *)&input_data[0];
	IVP_MI_Vector *future_solution = (IVP_MI_Vector *)&solution_data[0];

	IVP_MI_VECTOR_CLEAR(future_input, INPUT_VECTOR_LENGTH);
	IVP_MI_VECTOR_CLEAR(future_solution, SOLUTION_VECTOR_LENGTH);

    switch(attacher_buoyancy->template_buoyancy.use_stochastic_insertion) {

    case IVP_TRUE: {

		    //use stochastic
		    if (ai->mi->get_nr_occupied() < ai->mi->get_nr_of_vectors()) {
			//weighted_new_input->set_time_stamp(current_time);
			//solution_values->set_time_stamp(current_time);
			if(!(temp_buoyancy->insert_extrapol_only)) {
			    ai->mi->add_new_input_solution_combination_conventional(weighted_new_input, solution_values);
			}

			//extrapolate into the future
			IVP_RETURN_TYPE success = calculate_future_extrapolation(&ai->last_io_vectors,
										 weighted_new_input,
										 solution_values,
										 d_time,
										 current_time,
										 future_input,
										 future_solution);
			if (success) {
			    future_input->set_time_stamp(current_time);
			    future_solution->set_time_stamp(current_time);
			    ai->mi->add_new_input_solution_combination_conventional(future_input, future_solution);
			}
		    } else {