ivp_core.cxx

hl2 source code. Do not use it illegal.

	p_calc_2d_distances_to_axis(core_point,direction,&distances_2d);
	distances_2d.set_pairwise_mult(&distances_2d, &distances_2d);		// use ^2 because speed = rot_speed * radius
	
	/* Now virtual push object by Ft = mv = 1.0f kg*m/sec
	 * and sum dv
	 * than resolve mdv = Ft to m */
	distances_2d.set_pairwise_mult(&distances_2d,pc->get_inv_rot_inertia());
	dv = pc->get_inv_mass() + distances_2d.real_length();
    }else{
	distances_2d.set(core_point);

	distances_2d.set_pairwise_mult(&distances_2d, &distances_2d);		// use ^2 because speed = rot_speed * radius
	
	/* Now virtual push object by Ft = mv = 1.0f kg*m/sec
	 * and sum dv
	 * than resolve mdv = Ft to m */
	distances_2d.set_pairwise_mult(&distances_2d,pc->get_inv_rot_inertia());
	dv = pc->get_inv_mass() + distances_2d.real_length();
    }
    return 1.0f/dv;
}

#if 0
IVP_DOUBLE IVP_Core::get_energy(){
    IVP_DOUBLE energy = speed.quad_length() * this->mass;
    IVP_U_Float_Point hp;
    hp.set_pairwise_mult( &rot_speed, &rot_speed);
    energy += hp.dot_product(&rot_inertia);
    return 0.5f * energy;
}
#endif

IVP_DOUBLE IVP_Core::get_energy_on_test(const IVP_U_Float_Point *speed_vec,const IVP_U_Float_Point *rot_speed_vec){
    IVP_DOUBLE energy = speed_vec->quad_length() * this->get_mass();
    IVP_U_Float_Point hp;
    hp.set_pairwise_mult( rot_speed_vec, rot_speed_vec);
    energy += hp.dot_product(get_rot_inertia());
    return 0.5f * energy;
}

void IVP_Core::set_radius(IVP_FLOAT upper_limit, IVP_FLOAT max_dev){
    upper_limit_radius = upper_limit;
    max_surface_deviation = max_dev;
}


void IVP_Core::calc_at_matrix( IVP_Time current_time, IVP_U_Matrix *m_world_f_core_out) const {
    while(IVP_MTIS_SIMULATED(movement_state)){
	IVP_DOUBLE d_time = current_time - time_of_last_psi;
	if (d_time == 0.0f) break;	// e.g. beginn of psi
	// quaternion based solution
	{
	  IVP_U_Quat res;
	  res.set_interpolate_smoothly( &q_world_f_core_last_psi, &q_world_f_core_next_psi, d_time * i_delta_time );
	  res.set_matrix(m_world_f_core_out);
	  m_world_f_core_out->get_position()->add_multiple( &pos_world_f_core_last_psi, &delta_world_f_core_psis, d_time );
	}
	return;
    }
    *m_world_f_core_out = m_world_f_core_last_psi; // no movement
}
    

void IVP_Core::abort_all_async_pushes()
{
    this->rot_speed_change.set_to_zero();
    this->speed_change.set_to_zero();
}


void IVP_Core::global_damp_core(IVP_DOUBLE d_time) {
    
	IVP_Core *my_core=this;

	if (my_core->movement_state>=IVP_MT_SLOW){
	    IVP_FLOAT s_add_on = 0.1f;
	    IVP_U_Float_Point add_on(s_add_on, s_add_on, s_add_on); // somewhat more dampening for slow objects
	    IVP_U_Float_Point rot_damp_factor;
	    rot_damp_factor.add(&my_core->rot_speed_damp_factor, &add_on);
	    
	    my_core->damp_object(d_time,
				 &rot_damp_factor,
				 my_core->speed_damp_factor + s_add_on); //60 1.5
	}else{
	    my_core->damp_object(d_time,
				 &my_core->rot_speed_damp_factor,
				 my_core->speed_damp_factor); //60 1.5
	}	
}

// object is affected after commit_all_async_pushes
void IVP_Core::center_push_core_multiple_ws(const IVP_U_Float_Point *delta_speed,IVP_DOUBLE factor){
    this->speed.add_multiple(delta_speed, factor * get_inv_mass());
}

void IVP_Core::async_center_push_core_multiple_ws(const IVP_U_Float_Point *delta_speed,IVP_DOUBLE factor){
    this->speed_change.add_multiple(delta_speed, factor * get_inv_mass());
}

void IVP_Core::damp_object(IVP_DOUBLE delta_time_, const IVP_U_Float_Point *rotation_factor, IVP_DOUBLE speed_factor){
    IVP_U_Float_Point rot;
    rot.set_multiple ( rotation_factor , delta_time_);

    IVP_DOUBLE qlen = rot.quad_length();

    IVP_U_Float_Point r_factor;
    if (qlen < 0.5f){  // small dampening
	r_factor.set( 1.0f - rot.k[0],  1.0f - rot.k[1],  1.0f - rot.k[2]);
    }else{
	r_factor.set( IVP_Inline_Math::ivp_expf(-rot.k[0]),
		      IVP_Inline_Math::ivp_expf(-rot.k[1]),
		      IVP_Inline_Math::ivp_expf(-rot.k[2]) );
    }
    
    IVP_DOUBLE sf = speed_factor * delta_time_;
    if (sf < 0.25f){
	speed_factor = 1.0f - sf;
    }else{
	speed_factor = IVP_Inline_Math::ivp_expf(-sf);
    }
    IVP_Core *pc = this;
    pc->rot_speed.set_pairwise_mult(&pc->rot_speed, &r_factor);
    pc->speed.mult(speed_factor);
}

void IVP_Core::set_rotation_inertia( const IVP_U_Float_Point *r){
    rot_inertia.set(r);
    this->calc_calc();
}


void IVP_Core::get_surface_speed(const IVP_U_Float_Point *point_core, IVP_U_Float_Point *speed_world_out) const{
    this->get_surface_speed_on_test(point_core,&this->speed,&this->rot_speed,speed_world_out);
}


// change rot_speed_change and speed_change, needs movement_transaction // #+# to be killed
// for (springs)
void IVP_Core::async_push_core(const IVP_U_Float_Point		*point_cs,
				     const IVP_U_Float_Point	*impulse_in_core,
				     const IVP_U_Float_Point	*impulse_in_world){
    IVP_U_Float_Point rot_change,center_change;
    this->test_push_core(point_cs,impulse_in_core,impulse_in_world,&center_change,&rot_change); //stores results in variables rot_change and center_change

    rot_speed_change.add( &rot_change);
    speed_change.add( & center_change );
}

// change rot_speed and speed, instantly // #+# to be killed
void IVP_Core::push_core(const IVP_U_Float_Point	*point_cs,
			 const IVP_U_Float_Point	*impulse_in_core,
			 const IVP_U_Float_Point	*impulse_in_world){
    
    IVP_U_Float_Point rot_change,center_change;
    this->test_push_core(point_cs,impulse_in_core,impulse_in_world,&center_change,&rot_change); //stores results in variables rot_change and center_change
    rot_speed.add( &rot_change);
    speed.add( & center_change );
}

// change rot_speed and speed, instantly
void IVP_Core::push_core_ws(const IVP_U_Point *world_point,
			    const IVP_U_Float_Point *impulse_in_world)
{
    const IVP_U_Matrix *m_world_f_core = get_m_world_f_core_PSI();
    IVP_U_Float_Point point_d_ws; point_d_ws.subtract(world_point, m_world_f_core->get_position());
    IVP_U_Float_Point cross_point_dir;
    cross_point_dir.calc_cross_product( &point_d_ws, impulse_in_world);
    m_world_f_core->inline_vimult3( &cross_point_dir, &cross_point_dir);
    cross_point_dir.set_pairwise_mult( &cross_point_dir, get_inv_rot_inertia());
    rot_speed.add( &cross_point_dir);
    speed.add_multiple( impulse_in_world, get_inv_mass() );
}

// push object to change 
void IVP_Core::async_push_core_ws(const IVP_U_Point *world_point,
				  const IVP_U_Float_Point *impulse_in_world)
{    
    const IVP_U_Matrix *m_world_f_core = get_m_world_f_core_PSI();
    IVP_U_Float_Point point_d_ws; point_d_ws.subtract(world_point, m_world_f_core->get_position());
    IVP_U_Float_Point cross_point_dir;
    cross_point_dir.calc_cross_product( &point_d_ws, impulse_in_world);
    m_world_f_core->inline_vimult3( &cross_point_dir, &cross_point_dir);
    cross_point_dir.set_pairwise_mult( &cross_point_dir, get_inv_rot_inertia());
    rot_speed_change.add( &cross_point_dir);
    speed_change.add_multiple( impulse_in_world, get_inv_mass() );
}

void IVP_Core::async_rot_push_core_multiple_ws( const IVP_U_Float_Point *angular_impulse_ws, IVP_DOUBLE factor){
    const IVP_U_Matrix *m_world_f_core = get_m_world_f_core_PSI();
    IVP_U_Float_Point angular_impulse_cs;
    m_world_f_core->vimult3 ( angular_impulse_ws, &angular_impulse_cs);

    IVP_U_Float_Point h;
    h.set_pairwise_mult( &angular_impulse_cs, get_inv_rot_inertia());
    rot_speed_change.add_multiple(&h,factor);
}

void IVP_Core::rot_push_core_multiple_ws( const IVP_U_Float_Point *angular_impulse_ws, IVP_DOUBLE factor){
    const IVP_U_Matrix *m_world_f_core = get_m_world_f_core_PSI();
    IVP_U_Float_Point angular_impulse_cs;
    m_world_f_core->vimult3 ( angular_impulse_ws, &angular_impulse_cs);

    IVP_U_Float_Point h;
    h.set_pairwise_mult( &angular_impulse_cs, get_inv_rot_inertia());
    rot_speed.add_multiple(&h,factor);
}




// don't store result of push directly in core variables but in temporary variables
void IVP_Core::test_push_core(const IVP_U_Float_Point *point_cs,
			      const IVP_U_Float_Point *impulse_in_core,const IVP_U_Float_Point *impulse_in_world,
			      IVP_U_Float_Point *speed_out,IVP_U_Float_Point *rot_out) const{
    IVP_U_Float_Point distances_2d;
    distances_2d.inline_calc_cross_product(point_cs,impulse_in_core);    
    rot_out->set_pairwise_mult(&distances_2d,this->get_inv_rot_inertia());
    speed_out->set_multiple(impulse_in_world, this->get_inv_mass());
}



// don't store result of rot push directly in core variables but in temporary variables
void IVP_Core::test_rot_push_core_multiple_cs( const IVP_U_Float_Point *normized_core_axis,
					       IVP_DOUBLE rot_impulse,
					       IVP_U_Float_Point *delta_rot_speed_out)
{
    IVP_U_Float_Point h;
    h.set_pairwise_mult( normized_core_axis, get_inv_rot_inertia());
    delta_rot_speed_out->set_multiple(&h, rot_impulse);
}


IVP_DOUBLE IVP_Core::get_rot_inertia_cs(const IVP_U_Float_Point *normized_core_axis){
    IVP_U_Float_Point h;
    h.set_pairwise_mult(normized_core_axis, get_rot_inertia());
    IVP_DOUBLE len = h.real_length();
    return len;
}

IVP_DOUBLE IVP_Core::get_rot_speed_cs(const IVP_U_Float_Point *normized_core_axis){
    return rot_speed.dot_product(normized_core_axis);
}

void IVP_Core::async_rot_push_core_multiple_cs( const IVP_U_Float_Point *normized_core_axis, IVP_DOUBLE rot_impulse){
    IVP_U_Float_Point h;
    h.set_pairwise_mult( normized_core_axis, get_inv_rot_inertia());
    rot_speed_change.add_multiple(&h,rot_impulse);
}

void IVP_Core::rot_push_core_multiple_cs( const IVP_U_Float_Point *normized_core_axis, IVP_DOUBLE rot_impulse){
    IVP_U_Float_Point h;
    h.set_pairwise_mult( normized_core_axis, get_inv_rot_inertia());
    rot_speed.add_multiple(&h,rot_impulse);
}

void IVP_Core::rot_push_core_cs(const IVP_U_Float_Point *rot_impulse_cs)
{
    IVP_U_Float_Point h;
    h.set_pairwise_mult(rot_impulse_cs, get_inv_rot_inertia());
    this->rot_speed.add(&h);
}


// #+# too many calls to this function
void IVP_Core::commit_all_async_pushes(){   

    this->rot_speed.add(&this->rot_speed_change);
    this->speed.add(&this->speed_change);
    this->speed_change.set_to_zero(); 
    this->rot_speed_change.set_to_zero();
    IVP_IF(1){
    	core_plausible_check();
    }
}

void IVP_Core::calc_next_PSI_matrix_zero_speed(IVP_Event_Sim *es){
    i_delta_time = es->i_delta_time;

    q_world_f_core_next_psi = q_world_f_core_last_psi;
    
    this->abs_omega = 0.0f;
    this->speed.set_to_zero();
    this->delta_world_f_core_psis.set_to_zero();
    this->current_speed = 0.0f;
    this->max_surface_rot_speed = 0.0f;
    this->rotation_axis_world_space.set(1.0f,0.0f,0.0f);

    //IVP_ASSERT( current_sim_man_slot <= 0 );
}

void IVP_Core::reset_time( IVP_Time offset){
    time_of_last_psi -= offset;
    for (int i = objects.len()-1; i>=0; i--){
	IVP_Real_Object *o = objects.element_at(i);
	o->reset_time( offset );
    }
}

void IVP_Core::core_add_link_to_obj(IVP_Real_Object *add_obj) {
	IVP_IF(1) {
	    for (int j = objects.len()-1; j>=0;j--){
		IVP_ASSERT(objects.element_at(j) != add_obj);
	    }
	}
	this->objects.add(add_obj);
}

void IVP_Core::unlink_obj_from_core_and_maybe_destroy(IVP_Real_Object *remove_obj)
{
  
    this->objects.remove(remove_obj);
    if(this->objects.len()==0) {
	// I'm not longer used
	if(IVP_MTIS_SIMULATED(movement_state)) {
	    this->stop_physical_movement();
	    //environment->core_sim_manager->remove_sim_core(this);
	}
   
	P_DELETE_THIS(this);
    }
}

void IVP_Core::stop_movement_without_collision_recheck() {
    //IVP_ASSERT(IVP_MTIS_SIMULATED(movement_state));