ivp_buoyancy_solver.cxx

hl2 source code. Do not use it illegal.

#include <ivp_physics.hxx>
#ifndef WIN32
#	pragma implementation "ivp_buoyancy_solver.hxx"
#endif

#include <ivp_compact_ledge.hxx>
#include <ivp_cache_object.hxx>
#include <ivp_buoyancy_solver.hxx>
#include <ivu_geometry.hxx>
#include <ivp_compact_ledge_solver.hxx>
#include <ivp_controller_buoyancy.hxx>

//define some decision variables
#define DAMPENING_WITH_MOVEVECTOR               1
#define DAMPENING_WITH_PARTICLE_ACCELERATION    1

#define BALL_DAMPENING_CALC_PUSHING_FORCES       1
#define BALL_DAMPENING_CALC_SUCKING_FORCES      -1

//return surface speed in object coordinate system
// #+# do everything in object space !!!!
void ivp_core_get_surface_speed_os(IVP_Core *pc,IVP_Real_Object *object, const IVP_U_Float_Point *point_os, IVP_U_Float_Point *speed_out_os) {
    const IVP_U_Float_Point *rot_speed_var = &pc->rot_speed;
    const IVP_U_Float_Point *center_speed_var_world = &pc->speed;
	
    //convert point_os into non_float version
    
    //convert non_float_point_os into core system
    IVP_U_Point point_ws;
    IVP_U_Float_Point point_core;

    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(point_os, &point_ws);

    const IVP_U_Matrix *m_world_f_core_PSI = object->get_core()->get_m_world_f_core_PSI();
    m_world_f_core_PSI->vimult4( &point_ws, &point_core );
    
    IVP_U_Float_Point speed_core;
    speed_core.inline_calc_cross_product(rot_speed_var,&point_core);
	
    IVP_U_Float_Point speed_out_ws;
    pc->m_world_f_core_last_psi.vmult3(&speed_core,&speed_out_ws); // transform to world coords
    speed_out_ws.add(center_speed_var_world);
	
    IVP_U_Float_Point float_speed_out_ws;    float_speed_out_ws.set(&speed_out_ws);
    cache_object->transform_vector_to_object_coords(&float_speed_out_ws, speed_out_os);
	
    IVP_IF(0) {
	//template_polygon_current_values->sum_impulse.print("");
	if ((IVP_Inline_Math::fabsd(speed_out_os->k[0]) > 1E1f) ||
	    (IVP_Inline_Math::fabsd(speed_out_os->k[1]) > 1E1f) ||
	    (IVP_Inline_Math::fabsd(speed_out_os->k[2]) > 1E1f)) {
	    CORE;
	}
    }
}

IVP_Buoyancy_Solver::IVP_Buoyancy_Solver( IVP_Core *core_, IVP_Controller_Buoyancy *cntrl, const class IVP_Template_Buoyancy *input, const IVP_U_Float_Point *resulting_speed_of_current_ws_ ){
    core = core_;
    environment = core->environment;

    simulate_wing_behavior = input->simulate_wing_behavior;
    
    buoyancy_eps = input->buoyancy_eps;
    medium_density = input->medium_density;
    pressure_damp_factor = input->pressure_damp_factor * 0.5f * medium_density;
    friction_damp_factor = input->friction_damp_factor * 0.5f * medium_density;
    torque_factor = input->torque_factor;
    controller_buoyancy = cntrl;
    
    ball_rot_dampening_factor = input->ball_rot_dampening_factor;
    viscosity_input_factor = input->viscosity_input_factor;
	
    object_visible_surface_content_under = 0.0f;
    sum_impulse_x_point.set(0.0f, 0.0f, 0.0f);
    sum_impulse_x_movevector.set(0.0f, 0.0f, 0.0f);
    sum_impulse.set(0.0f, 0.0f, 0.0f);
    viscosity_factor = input->viscosity_factor;
    resulting_speed_of_current_ws.set(resulting_speed_of_current_ws_);

	
    //initialize buoyancy class variables with zero values
    volume_under = 0;
    volume_center_under.set_to_zero();
}

IVP_BOOL IVP_Buoyancy_Solver::compute_forces( const IVP_U_Float_Point *rel_speed_of_current_os, const IVP_U_Float_Hesse *surface_os, IVP_Real_Object *object ) {
	
	
	// track object, but don't spend CPU on buoyancy solve
	if ( medium_density <= 0 )
		return IVP_FALSE;
    //initialize some global variables in case this solver should be used more than once
    object_visible_surface_content_under = 0.0f;
    sum_impulse_x_point.set(0.0f, 0.0f, 0.0f);
    sum_impulse_x_movevector.set(0.0f, 0.0f, 0.0f);
    sum_impulse.set(0.0f, 0.0f, 0.0f);

    {
	//translate resulting_speed_of_current_ws into object coordinate system
	IVP_Cache_Object *cache_object = object->get_cache_object_no_lock();
	cache_object->transform_vector_to_object_coords(&resulting_speed_of_current_ws, &resulting_speed_of_current_os);
    }

    
    switch (object->get_type()) {
    case IVP_BALL: {
			
	//ensure that surface_ws is normized
	IVP_IF(1){
	    IVP_ASSERT( IVP_Inline_Math::fabsd( surface_os->real_length() - 1.0f) < 0.01f);
	}
			
	//compute buoyancy and dampening values for this ball
	compute_values_for_one_ball(object, surface_os, rel_speed_of_current_os);  //compute the volume centers and the volumes under surface_ws for the pushing forces

    }
    break;
    case IVP_POLYGON: {
			
	compute_values_for_one_polygon(object, surface_os);

    }
    default: {
			
    }
    break;
    }
		
    
		
    
    //return value describes if the object is in the water
    if ((volume_under > buoyancy_eps) || (sum_impulse.quad_length() > buoyancy_eps)) {
	// //copy the solution values to the structure
// 	solution_values.volume_under = volume_under;
// 	solution_values.object_visible_surface_content_under = object_visible_surface_content_under;
// 	for (int i=0; i<3; i++) {
// 	    solution_values.volume_center_under[i] = volume_center_under.k[i];
// 	    solution_values.sum_impulse[i] = sum_impulse.k[i];
// 	    solution_values.sum_impulse_x_point[i] = sum_impulse_x_point.k[i];
// 	    solution_values.sum_impulse_x_movevector[i] = sum_impulse_x_movevector.k[i];
// 	}
	return(IVP_TRUE);
    } else {
	return(IVP_FALSE);
    }
}



void IVP_Buoyancy_Solver::compute_buoyancy_values_for_one_ball(const int &decision,
							       const IVP_FLOAT &distance,
							       const IVP_FLOAT &radius,
							       const IVP_U_Float_Hesse *surface_os,
							       const IVP_U_Float_Point *geom_center_os) {
	
    //initialize buoyancy class variables with zero values
    //volume_under = volume_above = 0;
    volume_under = 0;
    volume_center_under.set_to_zero();
    //volume_center_above.set(0,0,0);
    
	
    switch (decision) {
    case 0: { //speed_plane_os and/or ball is completely under surface
		
	volume_under = (4.0f/3.0f) * IVP_PI * (radius*radius*radius);
	volume_center_under.set(geom_center_os);
		
	break;
    }
    case 1:
    case 2:
    case 3:
    case 4: {
		
	IVP_FLOAT height_of_balls_part_under_water = radius + distance;
		
	volume_under = (IVP_PI * height_of_balls_part_under_water*height_of_balls_part_under_water * (3*radius - height_of_balls_part_under_water)) / 3.0f;
		
	IVP_FLOAT volume_center_under_factor = 0.75f * (4.0f *radius*radius - 4.0f * radius*height_of_balls_part_under_water + height_of_balls_part_under_water*height_of_balls_part_under_water) / (3.0f*radius - height_of_balls_part_under_water);
	volume_center_under.set(geom_center_os);
	volume_center_under.add_multiple(surface_os, volume_center_under_factor);  //surface_os has to be normized!
		
	break;
    }
    }
}


void IVP_Buoyancy_Solver::compute_dampening_values_for_one_ball(const int &decision,
								const IVP_FLOAT &distance,
								const IVP_FLOAT &radius,
								const IVP_U_Float_Point *geom_center_os,
								const IVP_U_Float_Point *rel_speed_of_current_os_,
								const IVP_U_Float_Hesse *speed_plane_os_,
								const IVP_U_Float_Hesse *surface_os,
								const IVP_U_Float_Point *p1_os,
								const IVP_U_Float_Point *p2_os) {
	
    //initialize dampening variables with zero values
    IVP_U_Float_Point ball_point_of_impulse; ball_point_of_impulse.set_to_zero();
    IVP_U_Float_Point ball_impulse_vector;ball_impulse_vector.set_to_zero();

    IVP_U_Float_Point rel_speed_of_current_os;
    rel_speed_of_current_os.set(rel_speed_of_current_os_);
    IVP_U_Float_Hesse speed_plane_os;
    speed_plane_os.set(speed_plane_os_);
    speed_plane_os.hesse_val = speed_plane_os_->hesse_val;
	
    
    //will hold the content of the projected surface that磗 under the water surface
    IVP_DOUBLE ball_projected_surface_content_under = 0.0f;
    
    switch (decision) {
    case 0: { //speed_plane_os and/or ball is completely under surface
		
	ball_projected_surface_content_under = IVP_PI*radius*radius;  //surface content of speed_plane_os
	ball_point_of_impulse.set(geom_center_os);
    }
    break;
    case 1: { //center lies above the surface and speed_plane_os disects ball under and above the surface

	//check if surface_os and speed_plane_os are nearly parallel
	//if so, the we can switch to case 2
	IVP_U_Float_Point direction_down;
	speed_plane_os.proj_on_plane(surface_os, &direction_down);
	if (direction_down.quad_length() < buoyancy_eps) {
	    //if this is the case, then surface_os and speed_plane_os are nearly parallel, so we can switch to case 2
	    goto case_2;
	}
	direction_down.normize();
		
	IVP_U_Float_Point p1p2_os;
	p1p2_os.subtract(p2_os, p1_os);
		
	//calc point in the middle of the straight that results from the intersection between surface_os and speed_plane_os
	IVP_U_Float_Point Q_os;  //point between p1_os and p2_os
	Q_os.set(p1_os);
	Q_os.add_multiple(&p1p2_os, 0.5f);
		
	IVP_DOUBLE length_p1p2 = p1p2_os.real_length();
	if (length_p1p2 < buoyancy_eps) {  //check for safety reasons
	    ball_projected_surface_content_under = 0.0f;
	    break;
	}
	IVP_U_Float_Hesse p1p2_hesse_os;
	p1p2_hesse_os.set(&p1p2_os);
	p1p2_hesse_os.calc_hesse_val(&Q_os);
	p1p2_hesse_os.normize();
		
		
	/*** calc projected lowest point O_os on arc of ellipse ***/
		
	IVP_U_Float_Point tmp_O1_os;
	IVP_U_Float_Point tmp_O2_os;

    	//int erg =
	compute_disection_points_with_ball(surface_os, &p1p2_hesse_os, geom_center_os, radius, &tmp_O1_os, &tmp_O2_os);
	//IVP_ASSERT( erg == 1 );
	
	//decide which point is the right one and project it onto speed_plane_os => point O_os
	IVP_U_Float_Point tmp_O1O2_os;
	tmp_O1O2_os.subtract(&tmp_O2_os, &tmp_O1_os);
	IVP_U_Float_Point O_os;
	if (speed_plane_os.dot_product(&tmp_O1O2_os) >= 0.0f) {
	    speed_plane_os.proj_on_plane(&tmp_O1_os, &O_os);
	} else {
	    speed_plane_os.proj_on_plane(&tmp_O2_os, &O_os);
	}
		
		
	//check in which direction the ball is heading
	IVP_DOUBLE factor = surface_os->dot_product(&speed_plane_os);
	if (factor <= buoyancy_eps) {  //ball falls into the medium
			
	    /*** compute circle segment under surface ***/
	    IVP_U_Float_Point MQ_os;
	    MQ_os.subtract(&Q_os, geom_center_os);
	    IVP_DOUBLE height = radius - MQ_os.real_length();  //height of disected part of circle
	    IVP_DOUBLE circle_arc = 2*IVP_Inline_Math::fast_asin(length_p1p2 / (2.0f * radius)) * radius;  //length of arc which is cut off speed_plane_os by surface_os
			
	    IVP_DOUBLE surface_content_down = IVP_Inline_Math::fabsd(0.5f * (circle_arc*radius - length_p1p2*(radius - height)));

	    IVP_U_Float_Point center_of_segment_os;
	    if (surface_content_down < buoyancy_eps) { //safety check
		//if this is the case then the ball is nearly out of the water
		center_of_segment_os.set(0.0f, 0.0f, 0.0f);
	    } else {
		//check if speed_plane_os is orthogonal on surface_os; if yes we are almost finished
		if (factor > -buoyancy_eps) { //speed_plane_os is orthogonal on surface_os
		    //direction_down cannot be taken as direction vector
		    ball_point_of_impulse.set(geom_center_os);
		    ball_point_of_impulse.add_multiple(surface_os, length_p1p2*length_p1p2*length_p1p2 / (12 * surface_content_down));