ivp_constraint_local.cxx

hl2 source code. Do not use it illegal.

     *	Description:	calculate the translation system 
     *  Output:         m_fs_f_Acs and m_fs_f_Rcs
     ********************************************************************************/
    const IVP_U_Matrix &m_ws_f_Rcs = coreR ? *coreR->get_m_world_f_core_PSI() : m_identity;
    const IVP_U_Matrix &m_ws_f_Acs = coreA ? *coreA->get_m_world_f_core_PSI() : m_identity;

    IVP_U_Matrix3 &m_fs_f_Rcs = m_Rfs_f_Rcs;
    IVP_U_Matrix3 m_fs_f_ws; m_fs_f_Rcs.mi2mult3(&m_ws_f_Rcs, &m_fs_f_ws); // #+# wahrscheinlich nur temporaer verwendet
    IVP_U_Matrix3 m_fs_f_Acs; m_fs_f_ws.mmult3(&m_ws_f_Acs, &m_fs_f_Acs);
    

    /********************************************************************************
     *	Description:	calculate the rotation system and pretension
     *  Output:         m_rs_f_Acs and m_rs_f_Rcs
     *                  drRA_rs
     ********************************************************************************/
    const IVP_U_Matrix3 &m_Rrs_f_Rcs = (m_Rfs_f_Rcs.rot) ? *(m_Rfs_f_Rcs.rot) : (IVP_U_Matrix3) m_Rfs_f_Rcs; //TL: cast (IVP_U_Matrix) needed for sun compiler
    const IVP_U_Matrix3 &m_Ars_f_Acs = (m_Afs_f_Acs.rot) ? *m_Afs_f_Acs.rot   : (IVP_U_Matrix3) m_Afs_f_Acs; //
    IVP_U_Matrix3 m_rs_f_Rcs_buffer;
    const IVP_U_Matrix3 *pm_rs_f_Rcs = &m_Rrs_f_Rcs;
    
    IVP_U_Matrix3 m_rs_f_ws, m_rs_f_Acs;
    
    IVP_U_Point drRA_now_rs; // orientation difference between both ?rs
    if (fixedrot_dim + limitedrot_dim == 1) { // cardan joint
        pm_rs_f_Rcs = &m_rs_f_Rcs_buffer;
	
	// The fixed axis of  Ars into Rrs:
	IVP_U_Point fixed_axisArs_Acs; m_Ars_f_Acs.get_row(mapping_uRrs_f_Rrs[IVP_INDEX_X], &fixed_axisArs_Acs);      // The fixed axis of objectA ...
	IVP_U_Point fixed_axisArs_ws;  m_ws_f_Acs.vmult3(&fixed_axisArs_Acs, &fixed_axisArs_ws);
	IVP_U_Point fixed_axisArs_Rcs; m_ws_f_Rcs.vimult3(&fixed_axisArs_ws, &fixed_axisArs_Rcs); // ... transformed into Rcs
	IVP_U_Point fixed_axisRrs_Rcs; m_Rrs_f_Rcs.get_row(mapping_uRrs_f_Rrs[IVP_INDEX_X], &fixed_axisRrs_Rcs); // The fixed reference axis of objectR in Rcs
	IVP_U_Point vertical_axisRA_Rcs; // The axis which is vertical to both axes
	vertical_axisRA_Rcs.calc_cross_product(&fixed_axisArs_Rcs, &fixed_axisRrs_Rcs); // what if vertical_axisRA_Rcs == nullvec?

	// find rs:
	// If (fixed_axisArs_Rcs, fixed_axisRrs_Rcs, vertical_axisRA_Rcs) is seen as a coordinate space m_rs_f_fixedaxisRcs,
	// then the x-axis of rs is on the line "y == -x" in this "fixedaxis"-system. y and z only orthonormized
	// Therefore, x_freeaxis == (1, 1, 0). => m_rs_f_Rcs * (1,0,0) == fixed_axisArs_Rfs - fixed_axisRrs_Rfs.

	// Add both axes. rsx_Rcs is parallel to rs_x only because both matrices are _ortho_normized.
        IVP_U_Point rsx_Rcs; rsx_Rcs.add(&fixed_axisArs_Rcs, &fixed_axisRrs_Rcs); 
	m_rs_f_Rcs_buffer.init_normized3_col(&rsx_Rcs, mapping_uRrs_f_Rrs[IVP_INDEX_X]); m_rs_f_Rcs_buffer.transpose3(); // #+# use init normized3_row or program init_normized3_inv
	pm_rs_f_Rcs->mi2mult3(&m_ws_f_Rcs, &m_rs_f_ws);
	m_rs_f_ws.mmult3(&m_ws_f_Acs, &m_rs_f_Acs);

	// Calculate orientation
	IVP_U_Point vertical_axisRA_Rrs; m_Rrs_f_Rcs.vmult3(&vertical_axisRA_Rcs, &vertical_axisRA_Rrs);

	// orientation in Rrs of the common Vertical -- Ausrichtung der gemeinsamen Senkrechten im Rrs:
        IVP_DOUBLE vertical_anglezy_Rrs = IVP_Inline_Math::atan2d(vertical_axisRA_Rrs.k[mapping_uRrs_f_Rrs[IVP_INDEX_Y]], vertical_axisRA_Rrs.k[mapping_uRrs_f_Rrs[IVP_INDEX_Z]]);

	IVP_U_Point vertical_axisRA_Acs; IVP_U_Point vertical_axisRA_Ars, vertical_axisRA_ws;
	m_ws_f_Rcs.vmult3(&vertical_axisRA_Rcs, &vertical_axisRA_ws);
	m_ws_f_Acs.vimult3(&vertical_axisRA_ws, &vertical_axisRA_Acs);
	m_Ars_f_Acs.vmult3(&vertical_axisRA_Acs, &vertical_axisRA_Ars);

	// Orientation in Ars of the common Vertical -- Ausrichtung der gemeinsamen Senkrechten im Ars:
        IVP_DOUBLE vertical_anglezy_Ars = IVP_Inline_Math::atan2d(vertical_axisRA_Ars.k[mapping_uRrs_f_Rrs[IVP_INDEX_Y]], vertical_axisRA_Ars.k[mapping_uRrs_f_Rrs[IVP_INDEX_Z]]);

	// The difference of orientations == the current twist:
	IVP_DOUBLE diff_axis = vertical_anglezy_Ars - vertical_anglezy_Rrs; 
	if (diff_axis > IVP_PI) diff_axis -= 2.0f * IVP_PI;
	if (diff_axis < -IVP_PI) diff_axis += 2.0f * IVP_PI;
	// IVP_DOUBLE qlen_rsx = rsx_Rcs.quad_length();  // 0 .. 4
	// if (qlen_rsx > 3 ) qlen_rsx = 3;
	drRA_now_rs.k[mapping_uRrs_f_Rrs[IVP_INDEX_X]] = diff_axis;   // h鋘gt von known_axis ab
	drRA_now_rs.k[mapping_uRrs_f_Rrs[IVP_INDEX_Y]] = 0.0f;
	drRA_now_rs.k[mapping_uRrs_f_Rrs[IVP_INDEX_Z]] = 0.0f;
    } else {
	pm_rs_f_Rcs->mi2mult3(&m_ws_f_Rcs, &m_rs_f_ws); //  #+# move this lines into the next ifs 
	m_rs_f_ws.mmult3(&m_ws_f_Acs, &m_rs_f_Acs);
    }
    
    const IVP_U_Matrix3 &m_rs_f_Rcs = *pm_rs_f_Rcs;
    
    if (fixedrot_dim + limitedrot_dim == 3) { // same orientation
	IVP_U_Matrix3 m_rs_f_Ars; m_rs_f_Acs.mi2mult3(&m_Ars_f_Acs, &m_rs_f_Ars);
	
	IVP_U_Quat quaternion_relatedsystem;
	quaternion_relatedsystem.set_quaternion(&m_rs_f_Ars);
	drRA_now_rs.set(2.0f * IVP_Inline_Math::fast_asin(quaternion_relatedsystem.x),
		    2.0f * IVP_Inline_Math::fast_asin(quaternion_relatedsystem.y),
		    2.0f * IVP_Inline_Math::fast_asin(quaternion_relatedsystem.z));
	if ( quaternion_relatedsystem.w < 0.0f){
	    drRA_now_rs.mult(-1.0f);
	}
    } else if (fixedrot_dim + limitedrot_dim == 2) { // hinge
	// #+# free_axisArs_Acs und mit vimult3 nach free_axisArs_Rrs
	IVP_U_Matrix3 m_rs_f_Ars; m_rs_f_Acs.mi2mult3(&m_Ars_f_Acs, &m_rs_f_Ars);

	IVP_U_Point free_axisArs_Rrs;
	m_rs_f_Ars.get_col(mapping_uRrs_f_Rrs[IVP_INDEX_Z], &free_axisArs_Rrs);
	drRA_now_rs.set_to_zero();
	drRA_now_rs.k[mapping_uRrs_f_Rrs[IVP_INDEX_X]] = -IVP_Inline_Math::atan2d(
										free_axisArs_Rrs.k[mapping_uRrs_f_Rrs[IVP_INDEX_Y]],
										free_axisArs_Rrs.k[mapping_uRrs_f_Rrs[IVP_INDEX_Z]]);
	drRA_now_rs.k[mapping_uRrs_f_Rrs[IVP_INDEX_Y]] = IVP_Inline_Math::atan2d(
									       free_axisArs_Rrs.k[mapping_uRrs_f_Rrs[IVP_INDEX_X]],
									       free_axisArs_Rrs.k[mapping_uRrs_f_Rrs[IVP_INDEX_Z]]);
						     
    } else if (fixedrot_dim + limitedrot_dim == 0) { // free rotation
	drRA_now_rs.set_to_zero();
    }


    /********************************************************************************
     *	Description:	calculate the translation differences
     ********************************************************************************/
    // At this point drRA_rs is virtually the same as rRA_rs
    IVP_IF (0) {
	IVP_U_Float_Point world_vec;
	world_vec.set(0.0f,0.01f,0.0f);
	if (coreR) coreR->environment->add_draw_vector(m_ws_f_Rcs.get_position(), &world_vec, m_Rfs_f_Rcs.object->get_name(), 3);
	if (coreA) coreA->environment->add_draw_vector(m_ws_f_Acs.get_position(), &world_vec, m_Afs_f_Acs.object->get_name(), 3);
    }

    IVP_U_Point nullvec; nullvec.set_to_zero();
    IVP_U_Float_Point pointR_Rcs; m_Rfs_f_Rcs.vimult4(&nullvec, &pointR_Rcs);
    IVP_U_Float_Point pointA_Acs; m_Afs_f_Acs.vimult4(&nullvec, &pointA_Acs);
    IVP_U_Point pointR_ws; m_ws_f_Rcs.vmult4(&pointR_Rcs, &pointR_ws);
    IVP_U_Point pointA_ws; m_ws_f_Acs.vmult4(&pointA_Acs, &pointA_ws);
    IVP_U_Point pointR_Acs; m_ws_f_Acs.vimult4(&pointR_ws, &pointR_Acs);

    // All calculations will be done in fs and rs
    IVP_DOUBLE inv_dtime = es->i_delta_time;
  
    // better name it next_dvRA_fs and next_drRA_rs -- eigentlich next_dvRA_fs und next_drRA_rs    
    IVP_U_Point dvRA_fs; // desired differences of speed
    IVP_U_Point drRA_rs; // desired differences of angular velocity
    { // Calc desired velocity difference
        // Wie siehts im n鋍hsten PSI aus?

        // Echte Kurvenberechnung ginge so:
        // aktuelle Position += coreX->speed * d_time
        // aktuelle Position += ... hmm, mueszte mit quaternions gehen, die man aufeinander addiert
        // oder indem man die Achse bestimmt, um die sich das Objekt dreht, und dann in der Rotationsebene
        // den neuen Punkt bestimmt 
        // Statt echter Kurvenberechnung mache ich nur eine lineare Ann鋒erung.

        // Position:
        IVP_U_Float_Point  v_pointR_ws, v_pointA_ws;
	
	if (coreR){
	    coreR->get_surface_speed_on_test(&pointR_Rcs, &coreR->speed, &coreR->rot_speed, &v_pointR_ws);
	}else{
	    v_pointR_ws.set_to_zero();
	}
        if (coreA){
	    coreA->get_surface_speed_on_test(&pointA_Acs, &coreA->speed, &coreA->rot_speed, &v_pointA_ws);
	}else{
	    v_pointA_ws.set_to_zero();
	}
        IVP_U_Float_Point v_pointR_fs; m_fs_f_ws.vmult3(&v_pointR_ws, &v_pointR_fs);
        IVP_U_Float_Point v_pointA_fs; m_fs_f_ws.vmult3(&v_pointA_ws, &v_pointA_fs);
        // The velocity needed to reduce the position difference in the next PSI is needed. (dvRA)
	// current position == cpR, cpA; current velocity == vR, vA; d_time == t; next position == npR, npA.
	// dvRA == (npA - npR) / t  &&  npX == cpX + vX * t  =>
	// dvRA == (cpA - cpR) / t + vA - vR
        // the same calculation for rotation some lines below.
	{
	    IVP_U_Point dRA_now_fs;
	    IVP_U_Point dvRA_ws;    dvRA_ws.subtract(&pointA_ws, &pointR_ws);
	    m_fs_f_ws.vmult3(&dvRA_ws, &dRA_now_fs);

	    IVP_U_Point dv_fs; dv_fs.subtract( &v_pointA_fs, &v_pointR_fs);
	    dvRA_fs.set( &dRA_now_fs );
	    dvRA_fs.add_multiple( &dv_fs, d_time * damp_factor_div_force );

	    for (int i = 0; i < 3; i++) if (fixed[i] & IVP_CONSTRAINT_AXIS_LIMITED) {
		// Where A would be relative to R in next PSI:
		// npA - npR == dvRA * t
		if (dvRA_fs.k[i] < borderleft_Rfs[i]) {
		    dvRA_fs.k[i] = (dRA_now_fs.k[i] - borderleft_Rfs[i]) * limited_axis_stiffness + dv_fs.k[i] * d_time;

		    (int &) fixed[i] |= IVP_CONSTRAINT_AXIS_FIXED;
		} else if (dvRA_fs.k[i] > borderright_Rfs[i]) {
		    dvRA_fs.k[i] = (dRA_now_fs.k[i] - borderright_Rfs[i]) * limited_axis_stiffness + dv_fs.k[i] * d_time;
		    (int &) fixed[i] |= IVP_CONSTRAINT_AXIS_FIXED;
		} else {
		    (int &) fixed[i] &= ~IVP_CONSTRAINT_AXIS_FIXED;
		}
	    }
    	    dvRA_fs.mult(-force_factor * inv_dtime);
	}

        // Rotation: Attention: angles >  2 PI (?)
	IVP_U_Float_Point rR_rs, rA_rs;
	if (coreR) m_rs_f_Rcs.vmult3(&coreR->rot_speed, &rR_rs); else rR_rs.set_to_zero();
	if (coreA) m_rs_f_Acs.vmult3(&coreA->rot_speed, &rA_rs); else rA_rs.set_to_zero();
	
	// drRA_fs still contains the value for rRA_rs.
	{
	    IVP_U_Point drs_rs;	drs_rs.subtract( & rA_rs, &rR_rs );
	    drRA_rs.set( &drRA_now_rs );
	    drRA_rs.add_multiple( &drs_rs, d_time * damp_factor_div_force );

	    for (int i = 0; i < 3; i++){
		if (fixed[i + 3] & IVP_CONSTRAINT_AXIS_LIMITED) {
		    if (drRA_rs.k[i]  < borderleft_Rfs[i + 3]) {
			// dpos - rangenear 
			drRA_rs.k[i] = ( drRA_now_rs.k[i] - borderleft_Rfs[i + 3] ) * limited_axis_stiffness + drs_rs.k[i] * d_time;
			
			(int &) fixed[i + 3] |= IVP_CONSTRAINT_AXIS_FIXED;
			
		    } else if (drRA_rs.k[i] > borderright_Rfs[i + 3]) {
			drRA_rs.k[i] = ( drRA_now_rs.k[i] - borderright_Rfs[i + 3] ) * limited_axis_stiffness + drs_rs.k[i] * d_time;
			(int &) fixed[i + 3] |= IVP_CONSTRAINT_AXIS_FIXED;
		    } else {
			(int &) fixed[i + 3] &= ~IVP_CONSTRAINT_AXIS_FIXED;
		    }
		}
	    }
	    drRA_rs.mult(- force_factor * inv_dtime);
	}
	// the needed components (in RS and RS.rot) don't need to be extracted
	// due to the selective dimension of the bahaviour matrix
    }
    {
	int i;
	for (matrix_size = 0, i = 0; i < 6; i++){
	    matrix_size += fixed[i] & IVP_CONSTRAINT_AXIS_FIXED;
	}
    }
    
    if (fixedrot_dim == 1) {
	// Modification for cardan joints:
	// if both R-spaces form angles [90..180], the forces should be reduced

	IVP_U_Point fixed_axisArs_Acs; m_Ars_f_Acs.get_row(mapping_uRrs_f_Rrs[IVP_INDEX_X], &fixed_axisArs_Acs);      // Die feste Achse von objectA ...
	IVP_U_Point fixed_axisArs_ws;  m_ws_f_Acs.vmult3(&fixed_axisArs_Acs, &fixed_axisArs_ws);
	IVP_U_Point fixed_axisArs_Rcs; m_ws_f_Rcs.vimult3(&fixed_axisArs_ws, &fixed_axisArs_Rcs); // ... transformiert ins Rcs
	IVP_U_Point fixed_axisRrs_Rcs; m_Rrs_f_Rcs.get_row(mapping_uRrs_f_Rrs[IVP_INDEX_X], &fixed_axisRrs_Rcs); // Die feste Referenzachse von objectR im Rcs
	
	IVP_DOUBLE angleRA = IVP_Inline_Math::save_acosf(fixed_axisArs_Rcs.dot_product(&fixed_axisRrs_Rcs)); // vector angle
	IVP_DOUBLE slowrange = IVP_DOUBLE(IVP_PI - IVP_Inline_Math::fabsd(angleRA));  // 0 .. IVP_PI
        if (slowrange < 1.0f) drRA_rs.mult(slowrange);
    }
    
    // Testpushes. Vielleicht kann ich sie rausziehen, wenn die Resultate immer gleich sind?
    IVP_U_Point testnext_dvRA_fs[6], testnext_drRA_rs[6];

    // Translation
    {
	for (int i = 0; i < 3; i++) if (fixed[i] & IVP_CONSTRAINT_AXIS_FIXED) {
	    IVP_U_Float_Point testimpulseR_pointR_fs, testimpulseA_pointR_fs;
	    IVP_U_Float_Point testimpulseR_pointR_Rcs, testimpulseA_pointR_Acs;
	    IVP_U_Float_Point testimpulseR_pointR_ws, testimpulseA_pointR_ws;
	    testimpulseR_pointR_fs.set_to_zero();
	    testimpulseR_pointR_fs.k[i] = 1.0f;
	    testimpulseA_pointR_fs.set_negative(&testimpulseR_pointR_fs);  // #+# can be highly optimized, use get_row instead of vimult3
	    m_fs_f_Rcs.vimult3(&testimpulseR_pointR_fs, &testimpulseR_pointR_Rcs);
	    m_fs_f_Acs.vimult3(&testimpulseA_pointR_fs, &testimpulseA_pointR_Acs);
	    m_fs_f_ws.vimult3(&testimpulseR_pointR_fs, &testimpulseR_pointR_ws);
	    m_fs_f_ws.vimult3(&testimpulseA_pointR_fs, &testimpulseA_pointR_ws);
    /* So funktioniert die Berechnung unter der Annahme, da