ivp_constraint_local.cxx

hl2 source code. Do not use it illegal.

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

#include <ivp_physics.hxx>

#if defined(LINUX) || defined(SUN) || (defined(__MWERKS__) && defined(__POWERPC__))
#   include <alloca.h>
#endif

#if !defined(WIN32)
#     pragma implementation "ivp_constraint_local.hxx"
#endif


#include <ivp_great_matrix.hxx>
#include <ivp_core_macros.hxx>

#include <ivp_template_constraint.hxx>
#include "ivp_constraint_local.hxx"

// if set, constraints pushes at the same point in ws
//#define PUSH_SAMEPOINT
#define IVP_CONSTRAINT_BORDER_SOFTEN  /* if defined, a heuristic is activated that reduces the springy behavior of limit */

// !!! Attention !!!
//                          +----------+
// m_as_f_bs describes      | bs in as |
//                          +----------+
// m_as_f_bs.get_position() is the center of bs in as
// the first column is the x-axes of bs in as
// the first row is the x-axes of as in bs

// Debug-Colors:
// 0 black
// 1 red
// 2 green
// 3 blue
// 4 magenta
// 5 yellow
// 6 cyan
// 7 orange
// 8 white

IVP_Constraint_Local_Anchor::IVP_Constraint_Local_Anchor() {
    rot = NULL;
}

IVP_Constraint_Local_Anchor::~IVP_Constraint_Local_Anchor() {
    //if (rot) delete rot;
}

IVP_Constraint_Local::IVP_Constraint_Local() {
    is_enabled = IVP_FALSE;
    this->activate();
}

IVP_Constraint_Local::IVP_Constraint_Local(const IVP_Template_Constraint &tmpl) {
    init(tmpl);
    this->activate();
}

void IVP_Constraint_Local::sort_translation_mapping() {
    short pos = 0;
    {
	for (short i = 0; i < 3; i++) {
	    if (fixed[i] == IVP_CONSTRAINT_AXIS_FIXED) {
		mapping_uRfs_f_Rfs.k[pos++] = (IVP_COORDINATE_INDEX) i;
	    }
	}
    }
    {
	for (short i = 0; i < 3; i++) {
	    if (fixed[i] == IVP_CONSTRAINT_AXIS_LIMITED) {
		mapping_uRfs_f_Rfs.k[pos++] = (IVP_COORDINATE_INDEX) i;
	    }
	}
    }
    {
	for (short i = 0; i < 3; i++) {
	    if (fixed[i] == IVP_CONSTRAINT_AXIS_FREE) {
		mapping_uRfs_f_Rfs.k[pos++] = (IVP_COORDINATE_INDEX) i;
	    }
	}
    }
    fixedtrans_dim = limitedtrans_dim = 0;
    if (fixed[0] == IVP_CONSTRAINT_AXIS_FIXED) fixedtrans_dim++;
    if (fixed[0] & IVP_CONSTRAINT_AXIS_LIMITED) limitedtrans_dim++;
    if (fixed[1] == IVP_CONSTRAINT_AXIS_FIXED) fixedtrans_dim++;
    if (fixed[1] & IVP_CONSTRAINT_AXIS_LIMITED) limitedtrans_dim++;
    if (fixed[2] == IVP_CONSTRAINT_AXIS_FIXED) fixedtrans_dim++;
    if (fixed[2] & IVP_CONSTRAINT_AXIS_LIMITED) limitedtrans_dim++;
    matrix_size = fixedtrans_dim + fixedrot_dim + limitedtrans_dim + limitedrot_dim;
}

void IVP_Constraint_Local::sort_rotation_mapping() {
    int pos = 0;
    {
	for (int i = 0; i < 3; i++) {
	    if (fixed[i + 3] == IVP_CONSTRAINT_AXIS_FIXED) {
		mapping_uRrs_f_Rrs.k[pos++] = (IVP_COORDINATE_INDEX) i;
	    }
	}
    }
    {
	for (int i = 0; i < 3; i++) {
	    if (fixed[i + 3] == IVP_CONSTRAINT_AXIS_LIMITED) {
		mapping_uRrs_f_Rrs.k[pos++] = (IVP_COORDINATE_INDEX) i;
	    }
	}
    }
    {
	for (int i = 0; i < 3; i++) {
	    if (fixed[i + 3] == IVP_CONSTRAINT_AXIS_FREE) {
		mapping_uRrs_f_Rrs.k[pos++] = (IVP_COORDINATE_INDEX) i;
	    }
	}
    }
    fixedrot_dim = limitedrot_dim = 0;
    if (fixed[3] == IVP_CONSTRAINT_AXIS_FIXED) fixedrot_dim++;
    if (fixed[3] & IVP_CONSTRAINT_AXIS_LIMITED) limitedrot_dim++;
    if (fixed[4] == IVP_CONSTRAINT_AXIS_FIXED) fixedrot_dim++;
    if (fixed[4] & IVP_CONSTRAINT_AXIS_LIMITED) limitedrot_dim++;
    if (fixed[5] == IVP_CONSTRAINT_AXIS_FIXED) fixedrot_dim++;
    if (fixed[5] & IVP_CONSTRAINT_AXIS_LIMITED) limitedrot_dim++;
    matrix_size = fixedtrans_dim + fixedrot_dim + limitedtrans_dim + limitedrot_dim;
}

void IVP_Constraint_Local::init(const IVP_Template_Constraint &tmpl) {
    norm = IVP_NORM_MAXIMUM;

    IVP_Real_Object *objectR = tmpl.objectR;
    IVP_Real_Object *objectA = tmpl.objectA;
    if (objectR && !objectR->get_core()->physical_unmoveable)
	cores_of_constraint_system.add(objectR->get_core());
    if (objectA && !objectA->get_core()->physical_unmoveable)
	cores_of_constraint_system.add(objectA->get_core());
    if (tmpl.m_Aos_f_Afs) (objectR ? objectR : objectA)->ensure_in_simulation();
    IVP_U_Matrix m_ws_f_Ros; if (objectR) objectR->get_m_world_f_object_AT(&m_ws_f_Ros); else m_ws_f_Ros.init();
    IVP_U_Matrix m_ws_f_Aos; if (objectA) objectA->get_m_world_f_object_AT(&m_ws_f_Aos); else m_ws_f_Aos.init();
    IVP_U_Matrix m_Ros_f_Aos; m_ws_f_Ros.mimult4(&m_ws_f_Aos, &m_Ros_f_Aos);
    IVP_U_Matrix m_Rcs_f_Ros; if (objectR) objectR->calc_m_core_f_object(&m_Rcs_f_Ros); else m_Rcs_f_Ros.init();
    IVP_U_Matrix m_Acs_f_Aos; if (objectA) objectA->calc_m_core_f_object(&m_Acs_f_Aos); else m_Acs_f_Aos.init();
    IVP_U_Matrix m_Rcs_f_Aos; m_Rcs_f_Ros.mmult4(&m_Ros_f_Aos, &m_Rcs_f_Aos);
    IVP_U_Matrix m_Rcs_f_Acs; m_Rcs_f_Aos.mi2mult4(&m_Acs_f_Aos, &m_Rcs_f_Acs);
    
    //m_Ros_f_Rfs->vimult4(&_centerT_Ros, &center_trans_Rfs);
    //m_Ros_f_Rfs->vimult3(&_centerR_Ros, &center_rot_Rfs);
    
    if (tmpl.m_Ros_f_Rfs) {
	m_Rcs_f_Ros.mmult4(tmpl.m_Ros_f_Rfs, &m_Rfs_f_Rcs); m_Rfs_f_Rcs.real_invert();
    } else {
	m_Rfs_f_Rcs.set_matrix(&m_Rcs_f_Ros);
	m_Rfs_f_Rcs.real_invert();
    }
    if (tmpl.m_Aos_f_Afs) {
	m_Acs_f_Aos.mmult4(tmpl.m_Aos_f_Afs, &m_Afs_f_Acs); m_Afs_f_Acs.real_invert();
    } else { // Afs is Rfs
	m_Rfs_f_Rcs.mmult4(&m_Rcs_f_Acs, &m_Afs_f_Acs);
    }
    if (tmpl.m_Ros_f_Rrs) {
	m_Rfs_f_Rcs.rot = new IVP_U_Matrix3;
	m_Rcs_f_Ros.mmult3(tmpl.m_Ros_f_Rrs, m_Rfs_f_Rcs.rot); m_Rfs_f_Rcs.rot->transpose3();
	IVP_U_Matrix3 m_fs_f_rs; m_Rfs_f_Rcs.mi2mult3(m_Rfs_f_Rcs.rot, &m_fs_f_rs);
	m_Afs_f_Acs.rot = new IVP_U_Matrix3;
        m_fs_f_rs.mimult3(&m_Afs_f_Acs, m_Afs_f_Acs.rot);
    } else {
	m_Rfs_f_Rcs.rot = NULL;
	m_Afs_f_Acs.rot = NULL;
    }
    
    m_Rfs_f_Rcs.object = objectR;
    m_Afs_f_Acs.object = objectA;

	
    fixed[0] = tmpl.axis_type[0];
    fixed[1] = tmpl.axis_type[1];
    fixed[2] = tmpl.axis_type[2];
    fixed[3] = tmpl.axis_type[3];
    fixed[4] = tmpl.axis_type[4];
    fixed[5] = tmpl.axis_type[5];

    fixedtrans_dim = fixedrot_dim = limitedtrans_dim = limitedrot_dim = 0;
    sort_translation_mapping();
    sort_rotation_mapping();
    
    force_factor = tmpl.force_factor; // Ausgleichsrate pro PSI-Zeiteinheit
    damp_factor_div_force = tmpl.damp_factor / force_factor; // Ausgleichsrate pro PSI-Zeiteinheit

    limited_axis_stiffness = tmpl.limited_axis_stiffness;
    IVP_BOOL use_force = IVP_FALSE;
    for (int i = 0; i < 6; i++){
	borderright_Rfs[i]      = tmpl.borderright_Rfs[i];
	borderleft_Rfs[i]       = tmpl.borderleft_Rfs[i];
	
	if (tmpl.maximpulse_type[i] != IVP_CFE_NONE){
	    use_force = IVP_TRUE;
	}
    }

    
    if (use_force) { // luke
	maxforce = new IVP_Constraint_Local_MaxImpulse();
	for (short i = 0; i < 6; i++) {
	    maxforce->halfimpulse[i] = tmpl.maximpulse[i];
	    maxforce->type[i] = tmpl.maximpulse_type[i];
	}
    } else {
	maxforce = NULL;
    }
    
    is_enabled = IVP_FALSE;
}

IVP_Constraint_Local::~IVP_Constraint_Local() {
    if (maxforce) P_DELETE( maxforce);
    if (m_Rfs_f_Rcs.rot) P_DELETE( m_Rfs_f_Rcs.rot);
    if (m_Afs_f_Acs.rot) P_DELETE( m_Afs_f_Acs.rot);
}

void IVP_Constraint_Local::core_is_going_to_be_deleted_event(IVP_Core *core){
    IVP_USE(core);
    P_DELETE_THIS(this);
}

IVP_Real_Object *IVP_Constraint_Local::get_objectR() {
    return m_Rfs_f_Rcs.object;
}

IVP_Real_Object *IVP_Constraint_Local::get_objectA() {
    return m_Afs_f_Acs.object;
}


void IVP_Constraint_Local::constraint_changed() {

}

// A 700MHz prozessor needs (10/7) ns per cycle
// A ballsocket constraint on a rope need
// 13860,44 ns on redbaron before optimization
// 9990,111 ns on redbaron before optimization
// 89,68650 ns on maul before optimization
// Calculates the impulses which are needed to relaxe the constraint completely till next PSI
void IVP_Constraint_Local::do_simulation_controller(IVP_Event_Sim *es,IVP_U_Vector<IVP_Core> * /*core_list*/){
    IVP_FLOAT d_time = es->delta_time;

    IVP_Core *coreR = m_Rfs_f_Rcs.object ? m_Rfs_f_Rcs.object->get_core() : NULL;
    IVP_Core *coreA = m_Afs_f_Acs.object ? m_Afs_f_Acs.object->get_core() : NULL;
    IVP_ASSERT(!coreR || coreR->physical_unmoveable || coreR->movement_state != IVP_MT_NOT_SIM);
    IVP_ASSERT(!coreA || coreA->physical_unmoveable || coreA->movement_state != IVP_MT_NOT_SIM);

    // fs Rfs Afs ws Rcs Acs
    IVP_U_Matrix m_identity; m_identity.init();
    // Note: Optimization: use  m_Rcs_f_Acs
    // can mmults be avoided ??

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