ivp_actuator.cxx

hl2 source code. Do not use it illegal.

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

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

#include <ivp_cache_object.hxx>
#include <ivp_solver_core_reaction.hxx>

#include <ivu_active_value.hxx>
#include <ivp_actuator.hxx>
#include <ivp_hull_manager.hxx>

#include <ivp_radar.hxx>
#include <ivp_radar_appl.hxx>

IVP_Template_Rot_Mot::IVP_Template_Rot_Mot(){
    P_MEM_CLEAR(this);
}

IVP_Template_Torque::IVP_Template_Torque(){
    P_MEM_CLEAR(this);
}

IVP_Template_Stabilizer::IVP_Template_Stabilizer(){
    P_MEM_CLEAR(this);
}
IVP_Template_Check_Dist::IVP_Template_Check_Dist(){
    P_MEM_CLEAR(this);
}



//////////////////////////
IVP_Template_Force::IVP_Template_Force(){
    P_MEM_CLEAR(this);
    push_first_object = IVP_TRUE;
    push_second_object = IVP_FALSE;
}

IVP_Template_Two_Point::IVP_Template_Two_Point(){
    P_MEM_CLEAR(this);
}

IVP_Template_Four_Point::IVP_Template_Four_Point(){
    P_MEM_CLEAR(this);
}

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

IVP_Extra_Info::IVP_Extra_Info()
{
    P_MEM_CLEAR(this);    
    return;
}

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

void IVP_Template_Anchor::set_anchor_position_ws(IVP_Real_Object *obj, const IVP_U_Point *coords_ws)
{
    this->object = obj;
    this->coords_world = *coords_ws;
    return;
}


void IVP_Template_Anchor::set_anchor_position_ws(IVP_Real_Object *obj, const IVP_DOUBLE x, const IVP_DOUBLE y, const IVP_DOUBLE z)
{
    this->object = obj;
    this->coords_world.k[0] = x;
    this->coords_world.k[1] = y;
    this->coords_world.k[2] = z;
    return;
}


void IVP_Template_Anchor::set_anchor_position_os(IVP_Real_Object *obj, const IVP_U_Float_Point *coords_os)
{
    this->object = obj;

    IVP_Cache_Object *cache = obj->get_cache_object_no_lock();
    cache->transform_position_to_world_coords(coords_os, &this->coords_world);

	return;
}


void IVP_Template_Anchor::set_anchor_position_os(IVP_Real_Object *obj, const IVP_DOUBLE x, const IVP_DOUBLE y, const IVP_DOUBLE z)
{
    this->object = obj;
    IVP_U_Float_Point coords_os(x, y, z);

    IVP_Cache_Object *cache = obj->get_cache_object_no_lock();
    cache->transform_position_to_world_coords(&coords_os, &this->coords_world);
}

#if 1
void IVP_Template_Anchor::set_anchor_position_cs(IVP_Real_Object *obj, const IVP_U_Float_Point *coords_cs)
{
    this->object = obj;
    
    IVP_U_Matrix mat;
	obj->calc_m_core_f_object(&mat);
	IVP_U_Float_Point coords_os;
	mat.vimult4(coords_cs, &coords_os);

	this->set_anchor_position_os(obj, &coords_os);
	return;
}


void IVP_Template_Anchor::set_anchor_position_cs(IVP_Real_Object *obj, const IVP_DOUBLE x, const IVP_DOUBLE y, const IVP_DOUBLE z)
{
    this->object = obj;

    IVP_U_Matrix mat;
	obj->calc_m_core_f_object(&mat);
	IVP_U_Float_Point coords_os;
	IVP_U_Float_Point coords_cs(x, y, z);
	mat.vimult4(&coords_cs, &coords_os);

	this->set_anchor_position_os(obj, &coords_os);
	return;
}

#endif

void IVP_Anchor::init_anchor(IVP_Actuator *ac, IVP_Template_Anchor *ta){
    this->l_anchor_object = ta->object;
    this->l_actuator = ac;
    IVP_U_Matrix m_core_f_object;
    l_anchor_object->calc_m_core_f_object(&m_core_f_object);

    IVP_Cache_Object *co = ta->object->get_cache_object_no_lock();
    IVP_U_Point obj_pos;
    co->transform_position_to_object_coords( &ta->coords_world, &obj_pos);
    this->object_pos.set(&obj_pos);
    m_core_f_object.vmult4(&object_pos,&core_pos);
    l_anchor_object->insert_anchor(this);
}


IVP_Actuator::IVP_Actuator(IVP_Environment *){
}

IVP_Actuator::~IVP_Actuator(){
}

void IVP_Actuator::anchor_will_be_deleted_event(IVP_Anchor *){
    // one end of actuator is broken -> remove actuator
    P_DELETE_THIS(this);
}

void IVP_Actuator::core_is_going_to_be_deleted_event(IVP_Core *) {
    P_DELETE_THIS(this);
}

///////////// TWO POINT //////////////

IVP_Actuator_Two_Point::IVP_Actuator_Two_Point(IVP_Environment *env,
					       IVP_Template_Two_Point *two_point_templ,
					       IVP_ACTUATOR_TYPE )
    : IVP_Actuator(env)
{
    this->client_data = two_point_templ->client_data;
    IVP_Anchor *anch0  = this->get_actuator_anchor(0);
    IVP_Anchor *anch1 =  this->get_actuator_anchor(1);
    anch0->init_anchor( this, two_point_templ->anchors[0] );
    anch1->init_anchor( this, two_point_templ->anchors[1] );

    //controller

    IVP_Core *core0,*core1;
    core0= anch0->l_anchor_object->get_core();
    core1= anch1->l_anchor_object->get_core();

    if(!core0->physical_unmoveable) {
        actuator_controlled_cores.add(core0);
    }
    if(!core1->physical_unmoveable) {
      if(core1!=core0) {
        actuator_controlled_cores.add(core1);
      }
    }
    core0->environment->get_controller_manager()->announce_controller_to_environment(this);
}



IVP_Actuator_Two_Point::~IVP_Actuator_Two_Point()
{
    IVP_Core *core0;
    core0 = get_actuator_anchor(0)->l_anchor_object->get_core();
    core0->environment->get_controller_manager()->remove_controller_from_environment(this,IVP_TRUE); //silently    
}

///////////// FOUR POINT //////////////

IVP_Actuator_Four_Point::IVP_Actuator_Four_Point(IVP_Environment *env,
					       IVP_Template_Four_Point *four_point_templ,
					       IVP_ACTUATOR_TYPE )
    : IVP_Actuator(env)
{
    this->client_data = four_point_templ->client_data;
  int i;
  for(i=0; i<4; i++){
    IVP_Anchor *anch = get_actuator_anchor(i);
    anch->init_anchor(this, four_point_templ->anchors[i]);

    IVP_Core *core = anch->l_anchor_object->get_core();
    if(core->physical_unmoveable) {
        actuator_controlled_cores.install(core);	
    }
  }
}



IVP_Actuator_Four_Point::~IVP_Actuator_Four_Point()
{
    IVP_Core *core0;
    core0 = get_actuator_anchor(0)->l_anchor_object->get_core();
    core0->environment->get_controller_manager()->remove_controller_from_environment(this,IVP_TRUE); //silently
}

///////////// FOUR POINT //////////////


/////////// CHECK DIST ///////////////

void IVP_Anchor_Check_Dist::init_anchor_check_dist(IVP_Real_Object *object, IVP_U_Point *position_world_space, IVP_Actuator_Check_Dist *i_act_check_dist){
    this->l_actuator_check_dist = i_act_check_dist;
    this->real_object = object;
    
    IVP_U_Matrix m_world_f_object;
    object->get_m_world_f_object_AT( &m_world_f_object);

    IVP_U_Point obj_pos;
    m_world_f_object.vimult4(position_world_space,&obj_pos);
    object_pos.set(&obj_pos);

}

void IVP_Anchor_Check_Dist::hull_limit_exceeded_event(IVP_Hull_Manager *, IVP_HTIME){
    IVP_Actuator_Check_Dist *act = this->l_actuator_check_dist;
    act->hull_limit_exceeded_event();
}

IVP_Actuator_Check_Dist::IVP_Actuator_Check_Dist(IVP_Environment *, IVP_Template_Check_Dist *templ ) 
{
    this->client_data = templ->client_data;
    this->mod_is_outside = templ->mod_is_outside;
    this->range = templ->range;

    
    this->anchors[0].init_anchor_check_dist(templ->objects[0],&templ->position_world_space[0],this); 
    this->anchors[1].init_anchor_check_dist(templ->objects[1],&templ->position_world_space[1],this); 
    this->is_outside = IVP_FALSE;
    
    IVP_Anchor_Check_Dist *anchor_0 = &anchors[0];
    IVP_Anchor_Check_Dist *anchor_1 = &anchors[1];
    IVP_Hull_Manager *hull_man_0 = anchor_0->real_object->get_hull_manager();
    IVP_Hull_Manager *hull_man_1 = anchor_1->real_object->get_hull_manager();

    hull_man_0->insert_lazy_synapse(anchor_0, 0, 0);
    hull_man_1->insert_lazy_synapse(anchor_1, 0, 0);

    this->hull_limit_exceeded_event(); // does all we want
}

void IVP_Actuator_Check_Dist::set_range( IVP_DOUBLE new_range){
    this->range = new_range;
    this->hull_limit_exceeded_event();
}


void IVP_Actuator_Two_Point::ensure_actuator_in_simulation(){
    get_actuator_anchor(0)->l_anchor_object->get_environment()->get_controller_manager()->ensure_controller_in_simulation(this);
}




/************************************************************/
/************************************************************/
/************************************************************/
/************************************************************/

void IVP_Actuator_Check_Dist::fire_check_dist_event(IVP_BOOL distance_shorter){
    for (int i = listeners_check_dist_event.len()-1;i>=0;i--){
	listeners_check_dist_event.element_at(i)->check_dist_event(this,distance_shorter);
    }
}

void IVP_Actuator_Check_Dist::fire_check_dist_is_going_to_be_deleted_event(){
    for (int i = listeners_check_dist_event.len()-1;i>=0;i--){
	listeners_check_dist_event.element_at(i)->check_dist_is_going_to_be_deleted_event(this);
    }
}

void IVP_Actuator_Check_Dist::hull_limit_exceeded_event(){
    // e.g. called by IVP_Anchor_Check_Dist->hull_limit_exceeded_event()

    /** calc current len **/
    IVP_U_Point pos_ws[2];
    for (int i = 0;i<2;i++){
	IVP_U_Matrix m_world_f_object;
	anchors[i].real_object->get_m_world_f_object_AT(&m_world_f_object);
	m_world_f_object.vmult4( &anchors[i].object_pos, &pos_ws[i]);
    }
    
    IVP_DOUBLE qlen = pos_ws[0].quad_distance_to(&pos_ws[1]);
    IVP_DOUBLE len = IVP_Inline_Math::ivp_sqrtf(qlen);

    /** check len vs range **/
    if(len < this->range){
	if(this->is_outside){
	    this->is_outside = IVP_FALSE;
	    this->fire_check_dist_event(IVP_FALSE);
	}
    } else {
	if( !this->is_outside){
	    this->is_outside = IVP_TRUE;
	    this->fire_check_dist_event(IVP_TRUE);
	}	
    } // is_outside, else

    /** eventually set active_floats **/
    if(this->mod_is_outside)	mod_is_outside->set_int(this->is_outside, IVP_TRUE); // delayed!
        
    /** re-insert in hull managers **/
    IVP_DOUBLE new_delta_hull = IVP_Inline_Math::fabsd(len - this->range) * 0.5f; // @@@ maybe unequal hull vals, dependant on cur sur speed

    IVP_Anchor_Check_Dist *anchor_0 = &anchors[0];
    IVP_Anchor_Check_Dist *anchor_1 = &anchors[1];
    IVP_Hull_Manager *hull_man_0 = anchor_0->real_object->get_hull_manager();
    IVP_Hull_Manager *hull_man_1 = anchor_1->real_object->get_hull_manager();

    IVP_Time t_now = anchor_0->real_object->get_environment()->get_current_time();