void CIKFoot::set_ref_bone ( )
{
set_ref_bone(
get_ref_bone( Kinematics()->LL_GetTransform( m_foot_bone_id ) ,
Kinematics()->LL_GetTransform( m_toe_bone_id )
)
);
}
Fmatrix& CIKFoot::foot_to_ref_bone_transform ( Fmatrix& m ) const
{
if(m_ref_bone == 2)
{
m.set( Fidentity );
return m;
}
m.mul_43( Fmatrix().invert( Kinematics()->LL_GetTransform( m_foot_bone_id ) ), Kinematics()->LL_GetTransform( m_toe_bone_id ) );
return m;
}
void CIKFoot::Collide( SIKCollideData &cld, ik_foot_collider &collider, const Fmatrix &ref_bone, const Fmatrix& object_matrix, CGameObject *O, bool foot_step ) const
{
VERIFY( O->Visual( )->dcast_PKinematics() == Kinematics() );
ik_foot_geom fg;
SetFootGeom( fg ,ref_bone, object_matrix );
collider.collide( cld, fg, O, foot_step );
}
void CIKFoot::Create ( IKinematics *K, LPCSTR section, u16 bones[4] )
{
VERIFY(K);
m_K = K;
///defaults
m_ref_bone = 2;
if( m_ref_bone == 2 )
{
m_foot_normal.v .set( 1, 0, 0 );//2
m_foot_normal.bone = 2;
m_foot_direction.v .set( 0, 0, 1 );//2
m_foot_direction.bone = 2;
} else
{
m_foot_normal.v .set( 0, 0, -1 );//3
m_foot_normal.bone = 3;
m_foot_direction.v .set( 1, 0, 0 );//3
m_foot_direction.bone = 3;
}
// m_foot_normal.v .set( 1, 0, 0 );//2
// m_foot_normal.bone = 2;
//load settings
if( section )
{
if( !!K->LL_UserData()->r_bool( section, "align_toe" ))
m_ref_bone = 3;
m_foot_normal.bone = m_ref_bone;
m_foot_direction.bone = m_ref_bone;
m_foot_normal.v = Kinematics()->LL_UserData()->r_fvector3( section, "foot_normal" );
m_foot_direction.v = Kinematics()->LL_UserData()->r_fvector3( section, "foot_direction" );
}
set_toe( bones );
}
// ----------------------------------------------------------------------------
// ----------------------------------------------------------------------------
// ----------------------------------------------------------------------------
void starsim( Int_t nevents=1, Int_t rngSeed=1234 )
{
gROOT->ProcessLine(".L bfc.C");
{
TString simple = "y2012 geant gstar usexgeom agml ";
bfc(0, simple );
}
gSystem->Load( "libVMC.so");
gSystem->Load( "StarGeneratorUtil.so" );
gSystem->Load( "StarGeneratorEvent.so" );
gSystem->Load( "StarGeneratorBase.so" );
gSystem->Load( "libMathMore.so" );
gSystem->Load( "xgeometry.so" );
// Setup RNG seed and captuire ROOT TRandom
StarRandom::seed(rngSeed);
StarRandom::capture();
//
// Create the primary event generator and insert it
// before the geant maker
//
// StarPrimaryMaker *
_primary = new StarPrimaryMaker();
{
_primary -> SetFileName( "kinematics.starsim.root");
chain -> AddBefore( "geant", _primary );
}
Particles();
Kinematics();
//
// Initialize primary event generator and all sub makers
//
_primary -> Init();
//
// Setup geometry and set starsim to use agusread for input
//
geometry("y2012");
command("gkine -4 0");
command("gfile o pythia6.starsim.fzd");
//
// Setup PT and ETA distributions
//
Double_t pt0 = 3.0;
ptDist = new TF1("ptDist","(x/[0])/(1+(x/[0])^2)^6",0.0,10.0);
ptDist->SetParameter(0, pt0);
ptDist->Draw();
etaDist = new TF1("etaDist","-TMath::Erf(x+2.6)*TMath::Erf(x-2.6)",-0.8,+0.8);
//
// Trigger on nevents
//
trig( nevents );
// command("call agexit"); // Make sure that STARSIM exits properly
}
void CIKFoot::set_toe( u16 bones[4] )
{
VERIFY( Kinematics() );
m_foot_bone_id = bones[2];
m_toe_bone_id = bones[3];
xr_vector<Fmatrix> binds;
Kinematics()->LL_GetBindTransform( binds );
const Fmatrix bind_ref = binds[ bones[m_ref_bone] ];
const Fmatrix ibind_ref = Fmatrix().invert( bind_ref );
const Fmatrix bind2 = binds[ bones[2] ] ;
const Fmatrix ibind2 = Fmatrix().invert( bind2 );
//const Fmatrix ref_to_b2 = Fmatrix().mul_43( ibind2, bind_ref );
const Fmatrix b2to_ref = Fmatrix().mul_43( ibind_ref, bind2 );
const Fmatrix bind3 = binds[ bones[3] ] ;
const Fmatrix ibind3 = Fmatrix().invert( bind3 );
m_bind_b2_to_b3.mul_43( ibind2, bind3 );
///////////////////////////////////////////////////////
Fvector ax ,foot_normal, foot_dir;
get_local_vector( 2, foot_normal, m_foot_normal );
get_local_vector( 2, foot_dir, m_foot_direction );
//ref_to_b2.transform_tiny( foot_normal, m_foot_normal.v );
//ref_to_b2.transform_tiny( foot_dir, m_foot_direction.v );
ax.add( foot_normal, foot_dir );
ax.normalize();
///////////////////////////////////////////////////////
Fvector pos; pos.set( 0, 0, 0 );
Fmatrix ibind = ibind3;
envc pred( ibind, ax, pos );
/////////////////////////////////////////////////////////
Kinematics()->EnumBoneVertices( pred, bones[3] );
bind3.transform_tiny( pos );
ibind2.transform_tiny( pos );
m_toe_position.v.set( pos );
/////////////////////////////////////////////////////////
ibind.set( ibind2 );
ax.set( foot_normal );
Kinematics()->EnumBoneVertices( pred, bones[2] );
m_toe_position.v.x = _max( pos.x, m_toe_position.v.x );
/////////////////////////////////////////////////////////
ax.sub( foot_normal, foot_dir );
ax.normalize();
pred.start_pos.set(0,0,0);pos.set( 0, 0, 0 );
Kinematics()->EnumBoneVertices( pred, bones[2] );
m_heel_position.v = pred.pos ;
m_heel_position.v.add( Fvector().mul( foot_dir,
Fvector().sub( m_toe_position.v, pos ).dotproduct( foot_dir ) * 0.2f )
);
m_heel_position.bone = 2;
///////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////
bind2.transform_tiny( m_toe_position.v );
ibind_ref.transform_tiny( m_toe_position.v );
m_toe_position.bone = ref_bone();
/////////////////////////////////////////////////////////
get_local_vector( foot_normal, m_foot_normal );
m_foot_width = ( Fvector().sub( m_toe_position.v, b2to_ref.c ) ).dotproduct( foot_normal );
}
