ivp_friction.cxx

hl2 source code. Do not use it illegal.

			//ivp_message( "handbrake factor %f\n", f);
		}
		else
		{
			// still handbrake
			IVP_DOUBLE impulse_factor = .5f;// reduce real values to avoid jitter effects
			impulses.k[0] *= body_impulse_factor * impulse_factor; 
			impulses.k[1] *= body_impulse_factor * impulse_factor;
			core_a->car_wheel->last_skid_value = 0.0f;
		}
	}
	else
	{
		square_impulse = imp0_sqrd + imp1_sqrd * body_impulse_factor * body_impulse_factor;
		if ( 1 && ( square_impulse > maximum_impulse_force * maximum_impulse_force ) ) 
		{
			// check for sliding with no body push
			IVP_DOUBLE square2 = imp0_sqrd + imp1_sqrd;
			if ( 1 && ( square2 > maximum_impulse_force*maximum_impulse_force ) )
			{
				// clip impulse on a circle
				IVP_DOUBLE isum_impulse = IVP_Inline_Math::isqrt_float( square2 ); // no body push
				IVP_DOUBLE f = maximum_impulse_force * isum_impulse;
				impulses.k[0] *= f;
				impulses.k[1] *= f;
				
				span_friction_s[0] *= f; // update reference point
				span_friction_s[1] *= f;
				core_a->car_wheel->last_skid_value = ( 1.0f - f ) * this->now_friction_pressure * solver->body_object->get_core()->get_inv_mass();
				//ivp_message(" sliding %f factor %f\n", impulses.k[1], f );
			}
			else
			{
				// reduce body push until no sliding (clipping impulse on a elipse)
				IVP_DOUBLE y = IVP_Inline_Math::sqrtd( maximum_impulse_force * maximum_impulse_force - imp0_sqrd );
				IVP_DOUBLE old1 = impulses.k[1];
				if ( impulses.k[1] > 0)
				{
					impulses.k[1] = y;
				}
				else
				{
					impulses.k[1] = -y;
				}
				
				if ( 1 ) // update reference point
				{
					//span_s_1 *= 0.95f;
					static IVP_DOUBLE extra_factor = .9f;
					IVP_DOUBLE s_factor = extra_factor * impulses.k[1] / ( old1 * body_impulse_factor );
					core_a->car_wheel->last_skid_value = 0.3f * ( 1.0f - s_factor ) * this->now_friction_pressure * solver->body_object->get_core()->get_inv_mass();
					if ( s_factor < 1.0f )
					{
						span_s_1 *= s_factor;
						IVP_DOUBLE s0 = span_s_0 * dot_old0_new0 + span_s_1 * dot_old0_new1;
						IVP_DOUBLE s1 = span_s_0 * dot_old1_new0 + span_s_1 * dot_old1_new1;
						span_friction_s[0] = s0;
						span_friction_s[1] = s1;
					}
					//ivp_message(" reduced body push %f  s_factor %f\n", impulses.k[1], s_factor );
				}
			}

			square_impulse = maximum_impulse_force * maximum_impulse_force;
		}
		else // no sliding
		{
			//impulses.k[1] *= body_impulse_factor;
			//ivp_message("normal %f\n", impulses.k[1] );
		}
	}

	tcb.exert_impulse_dim2( core_a, /*core_b*/NULL, impulses );

	flEnergy = 0.0f; // current_energy
	return true;
}

IVP_FLOAT IVP_Contact_Point::friction_force_local_constraint_2d(const IVP_Event_Sim *es) 
{
    IVP_Impact_Solver_Long_Term *info = this->tmp_contact_info;
    IVP_DOUBLE maximum_impulse_force = this->now_friction_pressure * this->real_friction_factor * es->delta_time;
    if ( maximum_impulse_force < P_FLOAT_RES )
		return 0.0f;
    
    IVP_Core *core_a = info->contact_core[0];
    IVP_Core *core_b = info->contact_core[1];	
	
    if ( 1 && core_a && core_a->car_wheel && !core_b && ( next_dist_in_friction == NULL || next_dist_in_friction->now_friction_pressure == 0.0f ) )
	{
		IVP_FLOAT flEnergy = 0.0f;
		if ( friction_force_local_constraint_2d_wheel( core_a, info, es, flEnergy ) )
			return flEnergy;		
    }

    IVP_Solver_Core_Reaction tcb;
	tcb.init_reaction_solver_translation_ws( core_a, core_b, info->contact_point_ws, &info->span_friction_v[0], &info->span_friction_v[1], NULL );
	
	IVP_DOUBLE a = span_friction_s[0] * es->i_delta_time - tcb.delta_velocity_ds.k[0];
	IVP_DOUBLE b = span_friction_s[1] * es->i_delta_time - tcb.delta_velocity_ds.k[1];
	
	IVP_U_Matrix3 &tpm = tcb.m_velocity_ds_f_impulse_ds;
	
	IVP_DOUBLE inv_mat2x2[4];
	
	const IVP_DOUBLE tpm00 = tpm.get_elem( 0, 0 );
	const IVP_DOUBLE tpm01 = tpm.get_elem( 0, 1 );
	const IVP_DOUBLE tpm10 = tpm.get_elem( 1, 0 );
	const IVP_DOUBLE tpm11 = tpm.get_elem( 1, 1 );
	
	IVP_RETURN_TYPE ret = IVP_Inline_Math::invert_2x2_matrix( tpm00, tpm01, tpm01, tpm11, &inv_mat2x2[0], &inv_mat2x2[1],&inv_mat2x2[2],&inv_mat2x2[3] );
	if( ret != IVP_OK ) 
		return 0.0f;
	
    IVP_U_Float_Point impulses;
	impulses.k[0] = inv_mat2x2[0] * a + inv_mat2x2[1] * b;
	impulses.k[1] = inv_mat2x2[2] * a + inv_mat2x2[3] * b;
	
    IVP_DOUBLE square_impulse;
	square_impulse = impulses.k[0] * impulses.k[0] + impulses.k[1] * impulses.k[1];
	
	if( square_impulse > maximum_impulse_force*maximum_impulse_force ) 
	{
		IVP_DOUBLE isum_impulse = IVP_Inline_Math::isqrt_float(square_impulse);
		IVP_DOUBLE f = maximum_impulse_force * isum_impulse;
		impulses.k[0] *= f;
		impulses.k[1] *= f;
	}

	tcb.exert_impulse_dim2( core_a, core_b, impulses );

	// calc  current energy:
	IVP_FLOAT e = square_impulse * es->delta_time * es->delta_time;
	e *= ( span_friction_s[0] * span_friction_s[0] + span_friction_s[1] * span_friction_s[1] );
	e = 0.5f * IVP_Inline_Math::ivp_sqrtf( e );  // #+# sqrt ???
	IVP_FLOAT diff_e= e - this->old_energy_dynamic_fr;
	this->old_energy_dynamic_fr=e;
	return diff_e;
}

void IVP_Contact_Point::friction_force_local_constraint_1d(const IVP_Event_Sim *es) 
{
    IVP_U_Float_Point world_offset_contact;
    IVP_DOUBLE spring_len = get_and_set_real_friction_len(&world_offset_contact);

    IVP_Core *core[2];
    core[0]=this->get_synapse(0)->get_object()->friction_core;
    core[1]=this->get_synapse(1)->get_object()->friction_core;
    
    IVP_IF(0) {
      IVP_U_Float_Point debug_way_world;
      debug_way_world.set_multiple(&world_offset_contact,spring_len);
      //printf("lenofspring %f\n",spring_len);
      IVP_U_Point base_p;
      base_p.set(&tmp_contact_info->contact_point_ws);
      IVP_U_Float_Point vec_p;
      vec_p.set(&world_offset_contact);
      vec_p.mult(spring_len*10000.0f);
      char *out_text=p_make_string("f_vec");
      core[0]->environment->add_draw_vector(&base_p,&vec_p,out_text,3);
      P_FREE(out_text);
    }
    
    IVP_DOUBLE maximum_impulse_force = this->now_friction_pressure * this->real_friction_factor *
	es->delta_time;
    
    int i;
    IVP_U_Float_Point world_speed[2];
    for(i=0;i<2;i++) {
	IVP_Core *c = core[i];
	if(!c->physical_unmoveable){
	    c->get_surface_speed(&tmp_contact_info->contact_point_cs[i],&world_speed[i]);
	} else {
	    world_speed[i].set_to_zero();
	}
    }

    IVP_U_Float_Point rel_world_speed;
    rel_world_speed.subtract(&world_speed[0],&world_speed[1]);

    IVP_DOUBLE now_speed_direction=world_offset_contact.dot_product(&rel_world_speed);
    
    IVP_U_Float_Point translation_vec[2],rotation_vec[2],push_core_vec[2];
    IVP_DOUBLE speed_change_on_push=0.0f;
    IVP_DOUBLE sign=1.0f;
    
    for(i=0;i<2;i++) {
	IVP_Core *c = core[i];

	translation_vec[i].set_to_zero();
	rotation_vec[i].set_to_zero();
	
	if(!c->physical_unmoveable) {
	    IVP_U_Matrix *mat=&c->m_world_f_core_last_psi;
	   
	    IVP_U_Float_Point push_world_vec;
	    push_world_vec.set(&world_offset_contact);
	    push_world_vec.mult(sign);
	    mat->vimult3(&push_world_vec,&push_core_vec[i]);
	    IVP_U_Float_Point test_trans,test_rot;
	    c->test_push_core(&tmp_contact_info->contact_point_cs[i],&push_core_vec[i],&push_world_vec,&test_trans,&test_rot);
	    translation_vec[i].set(&test_trans);
	    rotation_vec[i].set(&test_rot);
	    IVP_U_Float_Point test_world;
	    c->get_surface_speed_on_test(&tmp_contact_info->contact_point_cs[i],&test_trans,&test_rot,&test_world);
	    speed_change_on_push+=test_world.dot_product(&push_world_vec);
	}	
	sign *= -1.0f;
    }

    IVP_DOUBLE desired_speed = spring_len * es->delta_time;
    IVP_DOUBLE diff_speed = desired_speed-now_speed_direction;

    IVP_DOUBLE push_val;
    if(speed_change_on_push>P_DOUBLE_EPS) {
	push_val=diff_speed/speed_change_on_push;
    } else {
	return;
    }
	
    if(IVP_Inline_Math::fabsd(push_val) > maximum_impulse_force) {
	push_val=maximum_impulse_force;
    }
					       
    for(i=0;i<2;i++) {
	IVP_Core *c = core[i];
	
	if(!c->physical_unmoveable) {
	    c->speed.add_multiple(&translation_vec[i],push_val);
	    c->rot_speed.add_multiple(&rotation_vec[i],push_val);
	}
    }

}


// get maximum (un)sharpness
IVP_FLOAT IVP_Contact_Point::get_possible_friction_slide_way(const IVP_Event_Sim *) {
    IVP_FLOAT way_passed_through_pressure;
    way_passed_through_pressure = now_friction_pressure * real_friction_factor * inv_virt_mass_mindist_no_dir;
    return way_passed_through_pressure;
}

IVP_FLOAT IVP_Friction_Core_Pair::get_sum_slide_way(const IVP_Event_Sim *es) {
    IVP_FLOAT sum=0.0f;
    for (int i = fr_dists.len()-1; i>=0; i--){
	IVP_Contact_Point *my_fr = fr_dists.element_at(i);
        sum += my_fr->get_possible_friction_slide_way(es);
    }
    return sum * es->delta_time * es->delta_time;
}


void IVP_Contact_Point::calc_pretension(IVP_FLOAT len)	{ // clip pretension
  //this->cp_status = IVP_CPBS_NEEDS_RECHECK;    
  IVP_DOUBLE qlen_of_spring = span_friction_s[0] * span_friction_s[0] + span_friction_s[1] * span_friction_s[1];
  if( qlen_of_spring > len * len + P_FLOAT_RES )    {
    IVP_DOUBLE ilen_of_spring = IVP_Inline_Math::isqrt_float(qlen_of_spring);
    this->integrated_destroyed_energy += (qlen_of_spring * ilen_of_spring - len) * real_friction_factor * now_friction_pressure;
    IVP_DOUBLE percent_of_old = len * ilen_of_spring;
    span_friction_s[0] *= percent_of_old;
    span_friction_s[1] *= percent_of_old;
    this->cp_status = IVP_CPBS_NEEDS_RECHECK;    
  }
}

// some comments about two friction values:
//   At the moment the direction of the friction spring is adjusted in a way it is
//   shortened in direction of the lower second value.
//   For the old friction_force_local_constraint this has not the desired effect.
//   The contact points are pushed in a 2D manner (friction_span_vectors to span 2D)
//   to reach the desired positions. This means more force in direction of the lower
//   friction is applied.
//   Therefore now the two friction case is handled with a 1D local constraint.
//   That means that no unallowed force toward second friction direction is applied
//   However, the 1D local constraint is inaccurate and looses contact over the time :-(
//   The best solution would be to make a 2D local constraint for the two value case
//   and span the 2D with the vector of second direction and an orthogonal one. Then
//   we have to clip the force done on the second direction.

void IVP_Friction_Core_Pair::pair_calc_friction_forces(const IVP_Event_Sim *es) {
    IVP_FLOAT max_local_spring_len = get_sum_slide_way(es);

    IVP_FLOAT sum_delta_energy=0.0f;
    
    for (int k = fr_dists.len()-1; k>=0; k--){
	IVP_Contact_Point *my_fr = fr_dists.element_at(k);
	my_fr->calc_pretension( max_local_spring_len );
	
	IVP_ASSERT(my_fr->tmp_contact_info->coll_time_is_valid == IVP_FALSE);
	if(my_fr->two_friction_values!=IVP_TRUE) {
	    sum_delta_energy += my_fr->friction_force_local_constraint_2d(es);
	} else {
	    my_fr->friction_force_local_constraint_1d(es);
	}
    }

    if((sum_delta_energy>0.0f)) {
	integrated_anti_energy+=sum_delta_energy;
    }
}

void IVP_Friction_System::calc_friction_forces(const IVP_Event_Sim *es) {
    for (int i = fr_pairs_of_objs.len()-1; i>=0; i--){
	IVP_Friction_Core_Pair *my_pair = fr_pairs_of_objs.element_at(i);
    //printf("having_core_pairs  ");
	IVP_IF(my_pair->fr_dists.len() == 0) {
	    printf("\n\n\n warngin: empty core pair\n");
	}
	my_pair->pair_calc_friction_forces(es);
    }
    //printf("\n");
}

void IVP_Contact_Point::set_friction_to_neutral(){
    span_friction_s[0] = 0.0f;
    span_friction_s[1] = 0.0f;    
}

void IVP_Friction_Info_For_Core::set_all_dists_of_obj_neutral()
{
    for (int i = friction_springs.len()-1; i>=0; i--){
	IVP_Contact_Point *minfr = friction_springs.element_at(i);
	minfr->set_friction_to_neutral();
    }
}


void IVP_Contact_Point::ease_the_friction_force(IVP_U_Float_Point *ease_diff_vec){
    IVP_Impact_Solver_Long_Term *info = tmp_contact_info;
    this->span_friction_s[0] += ease_diff_vec->dot_product(&info->span_friction_v[0]);
    this->span_friction_s[1] += ease_diff_vec->dot_product(&info->span_friction_v[1]);
    return;
}

void IVP_Friction_Solver::ease_test_two_mindists(IVP_Contact_Point *dist0,IVP_Contact_Point *dist1,IVP_U_Float_Point *world_surf_normal)
{
    IVP_USE(dist1);
    IVP_USE(dist0);
    IVP_USE(world_surf_normal);
#ifdef NOEASING
    return;
#endif
#if 0   
    IVP_Core *rev_core; 
    rev_core=dist0->synapse[0]->l_obj->to_real()->physical_core; //forces seen relative to this core
    
    IVP_real_friction_data fr_data0,fr_data1;
    dist0->get_world_friction_forces_dist(&fr_data0,0);

    dist1->get_world_friction_forces_dist(&fr_data1,0);
    if(rev_core!=dist1->synapse[0]->l_obj->to_real()->physical_core) {