ivp_actuator.cxx

hl2 source code. Do not use it illegal.

IVP_Actuator_Torque_Active::~IVP_Actuator_Torque_Active(){
    if (active_float_max_rotation_speed) active_float_max_rotation_speed->remove_dependency(this);
    if (active_float_torque) active_float_torque->remove_dependency(this);
}


IVP_Actuator_Torque::~IVP_Actuator_Torque()
{
    if (active_float_rotation_speed_out) active_float_rotation_speed_out->remove_reference();
}

void IVP_Actuator_Torque::do_simulation_controller(IVP_Event_Sim *es,IVP_U_Vector<IVP_Core> * /*core_list*/) {
    // clear if not updated
	this->rot_speed_out = 0;
    if (this->torque == 0.0f){
	return;
    }

    IVP_Real_Object *obj = get_actuator_anchor(0)->l_anchor_object;
    IVP_Core *core = obj->get_core();

    if (!IVP_MTIS_SIMULATED(core->movement_state) || core->pinned){	// only rotate simulated cores
	return;
    }

    IVP_Core *orig_core = obj->get_original_core();

    IVP_U_Float_Point *axis;
    IVP_U_Float_Point h;
    if (core == orig_core){
		axis = &axis_in_core_coord_system;
    }else{
		h.subtract(&get_actuator_anchor(1)->core_pos,&get_actuator_anchor(0)->core_pos);
		h.normize();
		axis = &h;
    }

    IVP_DOUBLE domega = core->get_rot_speed_cs(axis);

    this->rot_speed_out = (IVP_FLOAT)domega; // memorize for sound e.g.
    if (active_float_rotation_speed_out){
	active_float_rotation_speed_out->set_double(rot_speed_out);
    }
    
    if (IVP_Inline_Math::fabsd(domega) > max_rotation_speed){
		return;	// too fast
    }
    IVP_DOUBLE rot_imp = torque * es->delta_time;
    core->async_rot_push_core_multiple_cs(axis, rot_imp);
}

///////////////////////


////////// ANCHOR //////////////


IVP_Anchor::~IVP_Anchor() {
    anchor_get_real_object()->remove_anchor(this);
}

void IVP_Anchor::object_is_going_to_be_deleted_event(IVP_Real_Object *obj){
    IVP_ASSERT( obj == anchor_get_real_object() );
    l_actuator->anchor_will_be_deleted_event(this);
}
////////// MANAGERS /////////////////



void IVP_Actuator_Extra::active_float_changed(IVP_U_Active_Float *)
{    
}

    
void IVP_Actuator_Force::do_simulation_controller(IVP_Event_Sim *es,IVP_U_Vector<IVP_Core> * /*core_list*/) {
	
	IVP_DOUBLE force_val = this->force;
	if(force_val==0.0f) return; // nothing to push

	IVP_Anchor *anch0,*anch1;
	anch0= this->get_actuator_anchor(0);
	anch1= this->get_actuator_anchor(1);

	IVP_Real_Object *r_obj0,*r_obj1;
	r_obj0=anch0->anchor_get_real_object();
	r_obj1=anch1->anchor_get_real_object();

	IVP_Core *pc0 = r_obj0->physical_core;
	IVP_Core *pc1 = r_obj1->physical_core;
	
        IVP_U_Point dir_ws;
	IVP_U_Point pos0_ws;
	IVP_U_Point pos1_ws;

	pc0->get_m_world_f_core_PSI()->vmult4( &anch0->core_pos, &pos0_ws);
	pc1->get_m_world_f_core_PSI()->vmult4( &anch1->core_pos, &pos1_ws);
	dir_ws.subtract(&pos0_ws,&pos1_ws); //force direction world coords

	dir_ws.fast_normize();
	
	IVP_U_Float_Point force_dir;
	force_dir.set(&dir_ws);
	force_dir.mult(force_val * es->delta_time);

	if(this->push_first_object){	
		if (IVP_MTIS_SIMULATED(pc0->movement_state) && !pc0->pinned){ 
		    pc0->async_push_core_ws(&pos0_ws,&force_dir);
		}
	}

	if(this->push_second_object){
	    // also push at other end (reversed)
		if (IVP_MTIS_SIMULATED(pc1->movement_state) && !pc1->pinned){ 
		    IVP_U_Float_Point force_dir_rev;
		    force_dir_rev.set_multiple(&force_dir, -1);
		    pc1->async_push_core_ws(&pos1_ws,&force_dir_rev);
		}
	}
}

void IVP_Actuator_Extra::do_simulation_controller(IVP_Event_Sim *es,IVP_U_Vector<IVP_Core> * /*core_list*/) {
    IVP_Actuator_Extra *extra=this;
	if(extra->info.is_puck_force){
	    extra->do_puck_force(es->delta_time);
	    return;
	}
	if(extra->info.is_float_cam ){
	    extra->do_float_cam(es->delta_time);
	    return;
	}	
}


void IVP_Actuator_Extra::do_puck_force(IVP_DOUBLE dtime)
{
	IVP_Anchor *anch0,*anch1;
	anch0= this->get_actuator_anchor(0);
	anch1= this->get_actuator_anchor(1);
	IVP_Core *pc0 = anch0->l_anchor_object->friction_core;
	IVP_Core *pc1 = anch1->l_anchor_object->friction_core;

	IVP_Real_Object *r_obj0,*r_obj1;
	r_obj0=anch0->anchor_get_real_object();
	r_obj1=anch1->anchor_get_real_object();
		
        IVP_U_Float_Point dir_ws;
	IVP_U_Point pos0_ws;
	IVP_U_Point pos1_ws;

	pc0->get_m_world_f_core_PSI()->vmult4( &anch0->core_pos, &pos0_ws);
	pc1->get_m_world_f_core_PSI()->vmult4( &anch1->core_pos, &pos1_ws);
	dir_ws.subtract(&pos0_ws,&pos1_ws); //force direction world coords
	dir_ws.k[1] = 0;
	dir_ws.fast_normize();
	    
	{
	    IVP_DOUBLE force = 0; /* (dlen - this->info.spring_len) * this->info.spring_constant; */

	    IVP_U_Float_Point force_vector;	// the force 
	    force_vector.set_multiple(&dir_ws, force * dtime);

	    if (IVP_MTIS_SIMULATED(pc1->movement_state) && !pc1->pinned){ 
		pc1->async_push_core_ws(&pos1_ws,&force_vector);
	    }   
	    if (IVP_MTIS_SIMULATED(pc0->movement_state) && !pc0->pinned){ 
		force_vector.mult(-1);
		pc0->async_push_core_ws(&pos0_ws,&force_vector);
	    }
	}
}





void IVP_Actuator_Extra::calc_float_cam_matrix(IVP_U_Matrix *cam_matrix_out)
{
    // calc camera matrix from current fc values
    IVP_ASSERT(this->info.is_float_cam);

    IVP_U_Point cam_dir;
    cam_dir.subtract(&this->current_look_point, &this->current_float_cam_pos);
    cam_dir.normize();
    
    IVP_U_Point v0, v1;

    IVP_U_Point hp;
    hp.set(0.05f, -1.0f, 0.05f); // to zenit
    v0.calc_cross_product(&cam_dir, &hp);
    v0.normize();
    
    v1.calc_cross_product(&cam_dir, &v0);
    v1.normize();

    // export calculated cam matrix
    cam_matrix_out->init_columns4(&v0, &v1, &cam_dir, &this->current_float_cam_pos);
}

void IVP_Actuator_Extra::do_float_cam(IVP_DOUBLE d_time)
{
    // could be called per psi

    IVP_U_Vector<IVP_Core> *my_cores=get_associated_controlled_cores();
    int i;
    for(i=my_cores->len()-1;i>=0;i--) {
	IVP_Core *my_core=my_cores->element_at(i);
	if(!my_core->physical_unmoveable) {
	    my_core->objects.element_at(0)->ensure_in_simulation();
	}
    }
    
    IVP_ASSERT(this->info.is_float_cam);

    IVP_DOUBLE fc_height, fc_target_height, fc_dist, fc_speed;
    IVP_U_Point *fc_pos = this->get_float_cam_props(&fc_height, &fc_target_height, &fc_dist, &fc_speed);

    // what direction does actuator currently take ?
    IVP_Anchor *anch0,*anch1;
    anch0= this->get_actuator_anchor(0);
    anch1= this->get_actuator_anchor(1);
    IVP_Core *pc0 = anch0->l_anchor_object->physical_core;
    IVP_Core *pc1 = anch1->l_anchor_object->physical_core;

    IVP_Real_Object *r_obj0,*r_obj1;
    r_obj0=anch0->anchor_get_real_object();
    r_obj1=anch1->anchor_get_real_object();

    IVP_U_Point dir_ws;
    IVP_U_Point pos0_ws;
    IVP_U_Point pos1_ws;
    
    pc0->get_m_world_f_core_PSI()->vmult4( &anch0->core_pos, &pos0_ws);
    pc1->get_m_world_f_core_PSI()->vmult4( &anch1->core_pos, &pos1_ws);
    dir_ws.subtract(&pos0_ws,&pos1_ws); //force direction world coords

    dir_ws.normize(); 
    dir_ws.k[1] = 0.0f; // proj on y    @@@ proj on gravitation vec

    // where does current camera want to be ?
    IVP_U_Point cur_pos;
    cur_pos.add_multiple(&pos0_ws, &dir_ws, fc_dist);
    cur_pos.k[1] -= fc_height; // @@@ grav vec

    // what position shall camera take ?
    // determine real speed using d_time
    this->current_float_cam_pos.set_interpolate(fc_pos, &cur_pos, fc_speed * d_time);

    // what direction shall camera look along ?

    // smoothing is necessary to avoid 'jumps'
    IVP_U_Point look_point;
    look_point.add_multiple(&pos0_ws, &dir_ws, 0.0f); // 5m  should be adjustable
    look_point.k[1] -= fc_target_height;
    if(this->current_vals_are_set == IVP_FALSE){
	this->current_vals_are_set = IVP_TRUE;
	this->current_look_point.set(&look_point);
    } else {
	this->current_look_point.set_interpolate(&current_look_point, &look_point, fc_speed); // @@@ or other (fix) speed ?
    }
}





///////////// STABILIZER ////////////////
///////////// STABILIZER ////////////////
///////////// STABILIZER ////////////////

IVP_Actuator_Stabilizer::IVP_Actuator_Stabilizer(IVP_Environment *env,
				     IVP_Template_Stabilizer *templ
                                    ) : IVP_Actuator_Four_Point(
					env,templ,IVP_ACTUATOR_TYPE_STABILIZER )
{
    stabi_constant = templ->stabi_constant;
}

IVP_Actuator_Stabilizer::~IVP_Actuator_Stabilizer(){
}

void IVP_Actuator_Stabilizer::set_stabi_constant(IVP_DOUBLE val){
    if (val == stabi_constant) return;
    stabi_constant = val;
    
    get_actuator_anchor(0)->l_anchor_object->ensure_in_simulation();
}

void IVP_Actuator_Stabilizer::do_simulation_controller(IVP_Event_Sim *es,IVP_U_Vector<IVP_Core> * /*core_list*/) {

    IVP_Core *core[4];
    int i;
    for(i=0; i<4; i++){
      core[i] = get_actuator_anchor(i)->l_anchor_object->friction_core; // @@@OG friction_core???
    }
		
    IVP_U_Float_Point dir[2];
    IVP_U_Point pos0_ws[4];
    IVP_U_Point pos1_ws[4];
    IVP_DOUBLE dist[2];
    for(i=0; i<2; i++){
	IVP_Anchor *anch0,*anch1;
	anch0= this->get_actuator_anchor(i*2);
	anch1= this->get_actuator_anchor(i*2+1);

	IVP_Core *pc0 = anch0->l_anchor_object->friction_core;
	IVP_Core *pc1 = anch1->l_anchor_object->friction_core;

	pc0->get_m_world_f_core_PSI()->vmult4( &anch0->core_pos, &pos0_ws[i]);
	pc1->get_m_world_f_core_PSI()->vmult4( &anch1->core_pos, &pos1_ws[i]);
	dir[i].subtract(&pos0_ws[i],&pos1_ws[i]); //force direction world coords
	dist[i] = dir[i].real_length_plus_normize();
    }

    IVP_DOUBLE diff_dist = dist[1] - dist[0];

    IVP_DOUBLE force = diff_dist * this->stabi_constant * es->delta_time;
    
    for(i=0; i<2; i++){
      IVP_U_Float_Point force_vector; 
      force_vector.set_multiple(&dir[i], force);

      IVP_Core *pc0, *pc1;
      pc0 = core[i*2];
      pc1 = core[i*2+1];
      if (IVP_MTIS_SIMULATED(pc1->movement_state) && !pc1->pinned){ 
	pc1->async_push_core_ws(&pos1_ws[i], &force_vector);
      }   
      if (IVP_MTIS_SIMULATED(pc0->movement_state) && !pc0->pinned){ 
	force_vector.mult(-1);
	pc0->async_push_core_ws(&pos0_ws[i], &force_vector);
      }
      
      force *= -1; // reverted force direction for other anchor pair
    }
}

/////////////////