ivp_mindist_friction.cxx

hl2 source code. Do not use it illegal.

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


#include <ivp_physics.hxx>
#include <ivp_debug_manager.hxx>

#include <ivp_material.hxx>
#include <ivp_hull_manager.hxx>
#include <ivp_mindist_intern.hxx>
#include "ivp_mindist_macros.hxx"
#include <ivp_friction.hxx>

#include <ivp_compact_ledge.hxx>

#include <ivp_cache_object.hxx>
#include <ivp_cache_ledge_point.hxx>
#include <ivp_compact_ledge_solver.hxx>
#include <ivu_memory.hxx>

// calculates:
//    diff_contact_vec
//    surf_normal
// 	contact_point_ws
//    gap_distance
//    span_friction_v[0]
//    q,r and friction_situation_went_invalid
void IVP_Contact_Point::p_calc_friction_qr_PF(const IVP_U_Point *pp,
						 const  IVP_Compact_Edge *F,
						 IVP_Cache_Ledge_Point *m_cache_F,
						 IVP_Impact_Solver_Long_Term *info,
						 IVP_U_Float_Point *diff_contact_vec)
{
    // Punkt/Flaeche
    // F ist aufgespannt durch Q, R (und S)
    // (Q=F, R=F->next, S=F->prev)
  IVP_USE(diff_contact_vec);
    info->contact_point_ws.set(pp);

    IVP_U_Point pp_Fos;
    m_cache_F->get_object_cache()->transform_position_to_object_coords(pp, &pp_Fos);

#if !defined(  IVP_USE_S_VALS_FOR_PRETENSION )
    if ( cp_status == IVP_CPBS_NEEDS_RECHECK) { // do checking
      this->cp_status = IVP_CPBS_NORMAL;
      IVP_Unscaled_QR_Result qr;
      IVP_CLS.calc_unscaled_qr_vals_F_space(m_cache_F->get_compact_ledge(), F, &pp_Fos, &qr);
      if (qr.is_outside()){
	  info->friction_is_broken = IVP_TRUE;
      }
    }
    { // calc surface normal
	IVP_U_Point wHesse_vecF_Fos;
	IVP_CLS.calc_hesse_vec_object_not_normized(F, m_cache_F->get_compact_ledge(), &wHesse_vecF_Fos);
	wHesse_vecF_Fos.mult(inv_triangle_det);
	//wHesse_vecF_Fos.fast_normize();
      
      IVP_U_Point hesse_vec_ws;
      m_cache_F->get_object_cache()->transform_vector_to_world_coords( &wHesse_vecF_Fos, &hesse_vec_ws);
    
      const IVP_U_Float_Point *F_O_Fos = IVP_CLS.give_object_coords(F,m_cache_F);
      const IVP_U_Float_Point *F_1_Fos = IVP_CLS.give_object_coords(F->get_next(),m_cache_F);
      last_gap_len = wHesse_vecF_Fos.dot_product( &pp_Fos ) -  wHesse_vecF_Fos.dot_product(F_O_Fos);
      info->surf_normal.set_multiple( &hesse_vec_ws, -1.0f);
      
      // calc span_friction_v
      // =+= replace span_friction_v by global pre_tension
      IVP_U_Float_Point qvec_Fos;
      qvec_Fos.subtract( F_1_Fos, F_O_Fos);
      m_cache_F->get_object_cache()->transform_vector_to_world_coords( &qvec_Fos, &info->span_friction_v[0]);
    }
    
#endif
#ifdef IVP_USE_S_VALS_FOR_PRETENSION
    IVP_U_Point tp;      IVP_CLS.give_world_coords_AT(F, m_cache_F,&tp);
    IVP_U_Point tp_next; IVP_CLS.give_world_coords_AT(F->get_next(), m_cache_F, &tp_next);
    IVP_U_Point tp_prev; IVP_CLS.give_world_coords_AT(F->get_prev(), m_cache_F, &tp_prev);
    
    IVP_U_Point R, Q, Pvec;
    Q.subtract(&tp, &tp_next);      
    R.subtract(&tp_prev, &tp_next);

    IVP_DOUBLE QQ = Q.quad_length();
    IVP_DOUBLE RR = R.quad_length();
    
    Pvec.subtract( pp, &tp_next);
    
    IVP_DOUBLE QR = R.dot_product(&Q);

    IVP_DOUBLE QQRR = QQ * RR;
    IVP_DOUBLE QRQR = QR * QR;
    IVP_DOUBLE Det = (QQRR - QRQR);

    IVP_DOUBLE sq = Pvec.dot_product(&Q);
    IVP_DOUBLE sr = Pvec.dot_product(&R);
    
    IVP_DOUBLE unscaled_q = ( RR * sq - sr * QR );
    IVP_DOUBLE unscaled_r = ( QQ * sr - sq * QR );

    if ( unscaled_q < 0.0f || unscaled_r < 0.0f || unscaled_r+unscaled_q > Det) {
	info->friction_situation_went_invalid = IVP_TRUE;
    }

    info->surf_normal.calc_cross_product(&Q,&R);   // @@@OS optimization: store surf_normal in CS coords, or at least 1.0f/sqrt(len) 
    info->surf_normal.fast_normize();


    IVP_DOUBLE iQ = sqrtf(1.0f / QQ );
    info->span_friction_v[0].set_multiple(&Q, iQ);

    info->gap_distance = info->surf_normal.dot_product(&tp) - info->surf_normal.dot_product(pp);


    IVP_DOUBLE iDet = 1.0f / Det;
    IVP_DOUBLE q = unscaled_q * iDet;
    IVP_DOUBLE r = unscaled_r * iDet;

    IVP_IF(1) {
	IVP_U_Point point_on_surf;
	point_on_surf.set(&tp_next);
	point_on_surf.add_multiple(&Q,q);
	point_on_surf.add_multiple(&R,r);
	IVP_U_Point dist_vec_world;
	dist_vec_world.subtract(&point_on_surf,pp);
	IVP_DOUBLE test_val = info->surf_normal.dot_product(&dist_vec_world);
	if( test_val < 0.0f ) {
	    printf("recalc_friction_s_point_surface wrong surf_normal %f should be >0\n",test_val);
	}
    }

    IVP_DOUBLE old_q = s_coords[0];
    IVP_DOUBLE old_r = s_coords[1];

    diff_contact_vec->set_multiple(&Q,old_q - q);
    diff_contact_vec->add_multiple(&R,old_r - r);

    
    s_coords[0] = q;
    s_coords[1] = r;
#endif
}

void IVP_Contact_Point::p_calc_friction_s_PP(const IVP_U_Point *pp0,const IVP_U_Point *pp1,IVP_Impact_Solver_Long_Term *info, IVP_U_Float_Point *diff_contact_vec) {
  IVP_USE(diff_contact_vec);
    IVP_U_Point dist_vec;
    dist_vec.subtract(pp1,pp0);
    last_gap_len = dist_vec.real_length_plus_normize();
    info->surf_normal.set(&dist_vec);

    IVP_U_Float_Point magic_world_random_vec;
    
    // surf_normal shall not point in same direction as magic_world_random_vec
    if( info->surf_normal.k[0] * info->surf_normal.k[0] < 0.9f) {
      magic_world_random_vec.set(1,0,0);
    }else{
      magic_world_random_vec.set(0,0,1);
    }
    info->span_friction_v[0].inline_calc_cross_product_and_normize(&info->surf_normal,&magic_world_random_vec);
    info->contact_point_ws.set(pp0);
#ifdef IVP_USE_S_VALS_FOR_PRETENSION    
    diff_contact_vec->set_to_zero();
#endif   
}

void IVP_Contact_Point::p_calc_friction_s_PK(const IVP_U_Point *pp, const IVP_Compact_Edge *K,
					    IVP_Cache_Ledge_Point *m_cache_K, IVP_Impact_Solver_Long_Term *info, IVP_U_Float_Point *diff_contact_vec)
{
    // if status indicates friction mode, point is always kept the same
  IVP_USE(diff_contact_vec);
    IVP_U_Point tp;      IVP_CLS.give_world_coords_AT(K, m_cache_K,&tp);
    IVP_U_Point tp_next; IVP_CLS.give_world_coords_AT(K->get_next(), m_cache_K, &tp_next);
    
    IVP_U_Float_Point vec1, vec2;
    vec1.subtract(&tp_next, &tp);
    vec2.subtract(pp, &tp);
    IVP_DOUBLE i_len = IVP_Inline_Math::isqrt_double( vec1.quad_length() );

    IVP_U_Float_Point vert_12;
    vert_12.inline_calc_cross_product( &vec1, &vec2 );
    IVP_DOUBLE distance = vert_12.real_length() * i_len;
    last_gap_len = distance;

    if(  distance * distance > P_DOUBLE_EPS ) {
	vert_12.set_multiple( &vert_12, i_len / distance );
    } else {
	vert_12.set(1.0f,0.0f,0.0f);
    }
    info->surf_normal.inline_calc_cross_product_and_normize(&vec1,&vert_12);
    info->contact_point_ws.set(pp);
    
    info->span_friction_v[0].set(&vert_12);
    IVP_IF(1) {
	IVP_DOUBLE len= info->span_friction_v[0].real_length();
	len=1.0f-len;
	if( IVP_Inline_Math::fabsd(len) > 0.001f) {
	    printf("span_friction_v_pk not normized\n");
	}	
    }

    if (cp_status == IVP_CPBS_NEEDS_RECHECK){
	this->cp_status = IVP_CPBS_NORMAL;
	IVP_DOUBLE iqlen =  i_len * i_len;
	IVP_DOUBLE s = vec1.dot_product(&vec2) * iqlen;
	if (s < 0.0f || s > 1.0f)  {
	  info->friction_is_broken = IVP_TRUE;
	}
    }

#ifdef IVP_USE_S_VALS_FOR_PRETENSION    
    IVP_DOUBLE old_s = s_coords[0];
    s_coords[0] = s;
    diff_contact_vec->add_multiple(&vec1,old_s - s);
#endif    
}


void IVP_Contact_Point::p_calc_friction_ss_KK(const IVP_Compact_Edge *K, const IVP_Compact_Edge *L,
					    IVP_Cache_Ledge_Point *m_cache_K,
					    IVP_Cache_Ledge_Point *m_cache_L,IVP_Impact_Solver_Long_Term *info, IVP_U_Float_Point *diff_contact_vec)
{
  IVP_DOUBLE sl, sk;

  
  IVP_U_Point norm, Kvec, Lvec;
    // very bad code down there #+# (this is a copy paste from an ugly old OS version
  IVP_U_Point Kp;      IVP_CLS.give_world_coords_AT(K, m_cache_K, &Kp);
  IVP_U_Point Kp_next; IVP_CLS.give_world_coords_AT(K->get_next(), m_cache_K, &Kp_next);
  IVP_U_Point Lp;      IVP_CLS.give_world_coords_AT(L, m_cache_L, &Lp);
  IVP_U_Point Lp_next; IVP_CLS.give_world_coords_AT(L->get_next(), m_cache_L, &Lp_next);

  Lvec.subtract(&Lp_next, &Lp);
  Lvec.fast_normize();
  Kvec.subtract(&Kp_next, &Kp);
  Kvec.fast_normize();
  
  norm.calc_cross_product(&Kvec, &Lvec);

  IVP_DOUBLE quad_dist = norm.quad_length();
    
  if(quad_dist > (P_DOUBLE_RES * P_DOUBLE_RES)){ // not parallel
    IVP_U_Point K_area;	// area vertical to K and norm
    IVP_U_Point L_area;	// area vertical to L and norm
    K_area.calc_cross_product(&Kvec, &norm);
    L_area.calc_cross_product(&Lvec, &norm);

    IVP_DOUBLE a,b,x;
    a = K_area.dot_product( &Lp );
    b = K_area.dot_product( &Lp_next );
    IVP_DOUBLE inv_a_b = 1.0f / (a-b);
    x = K_area.dot_product( &Kp );
    sl = (a-x) * inv_a_b;

    a = L_area.dot_product( &Kp );
    b = L_area.dot_product( &Kp_next );
    inv_a_b = 1.0f / (a-b);
    x = L_area.dot_product( &Lp );
    sk = (a-x) * inv_a_b;

    //TL -- modify if necessary  #+#  very bad code down there, vec0 won't be normized soon
    IVP_U_Point vec0,vec1;
    vec0.subtract(&Kp_next, &Kp);
    vec1.subtract(&Lp_next, &Lp);

    IVP_U_Point world_edge_space0,world_edge_space1;
    world_edge_space0.set_interpolate(&Kp,&Kp_next,sk);
    world_edge_space1.set_interpolate(&Lp,&Lp_next,sl);

    IVP_U_Point world_diff_vec;
    world_diff_vec.subtract(&world_edge_space1,&world_edge_space0);

    IVP_DOUBLE length = world_diff_vec.real_length();

    if(length > P_DOUBLE_EPS) {