FerryDriver (NewtonBody* const body, const dVector& pivot, NewtonBody* const triggerPort0, NewtonBody* const triggerPort1)
:CustomKinematicController (body, pivot)
,m_triggerPort0(triggerPort0)
,m_triggerPort1(triggerPort1)
,m_state(m_driving)
{
dMatrix matrix0;
dMatrix matrix1;
NewtonBodyGetMatrix(m_triggerPort0, &matrix0[0][0]);
NewtonBodyGetMatrix(m_triggerPort1, &matrix1[0][0]);
dVector dist0 (matrix0.m_posit - pivot);
dVector dist1 (matrix1.m_posit - pivot);
//dist0.m_y = 0.0f;
//dist1.m_y = 0.0f;
if ((dist0 % dist0) < (dist1 % dist1)) {
m_target = matrix0.m_posit;
m_currentPort = m_triggerPort0;
} else {
m_target = matrix1.m_posit;
m_currentPort = m_triggerPort1;
}
//m_target.m_y = pivot.m_y;
}
void
PermutationTest::
apply()
{
this->M_pValue = 0;
this->M_dataSetPtr1->subtractMeanValue();
this->M_dataSetPtr2->subtractMeanValue();
varCov_Type varCov;
Real dist0(-1);
if ( this->M_mpiUtilityPtr->isMaster() )
{
varCov.setData( this->M_dataSetPtr1->getData() );
matrixPtr_Type varMatrixPtr1( varCov.varCovMatrix() );
varCov.setData( this->M_dataSetPtr2->getData() );
matrixPtr_Type varMatrixPtr2( varCov.varCovMatrix() );
dist0 = this->M_distPtr->compute( varMatrixPtr1, varMatrixPtr2 );
}
MPI_Bcast( & dist0, 1, MPI_DOUBLE_PRECISION, MASTER, MPI_COMM_WORLD );
const UInt N1 = this->M_dataSetPtr1->nbSamples();
const UInt N2 = this->M_dataSetPtr2->nbSamples() ;
std::vector< UInt > drawnSampleIDs;
std::vector< UInt > allSampleIDs( N1 + N2 );
for ( UInt iN(0); iN < N1 + N2; ++iN )
{
allSampleIDs[ iN ] = iN;
}
srand48( this->M_mpiUtilityPtr->myRank() * time( NULL ) );
const UInt niterSlave = static_cast< UInt >( std::ceil( this->M_niter / this->M_mpiUtilityPtr->nbThreads() ) );
const UInt niterMaster = niterSlave + this->M_niter % this->M_mpiUtilityPtr->nbThreads();
printf( "niterSlave = %d \t niterMaster = %d\n", niterSlave, niterMaster );
UInt niterMine;
this->M_mpiUtilityPtr->isMaster() ? niterMine = niterMaster : niterMine = niterSlave;
for ( UInt it(0); it < niterMine; ++it )
{
printf( " Proc %d is at iteration %d of %d\n", this->M_mpiUtilityPtr->myRank(), it, niterMine );
drawnSampleIDs.clear();
drawnSampleIDs.reserve( N1 );
while( drawnSampleIDs.size() != N1 )
{
const UInt temp( static_cast< UInt >( ( N1 + N2 - 1) * drand48() ) );
if ( std::find( drawnSampleIDs.begin(), drawnSampleIDs.end(), temp ) == drawnSampleIDs.end() )
{
drawnSampleIDs.push_back( temp );
}
}
std::sort( drawnSampleIDs.begin(), drawnSampleIDs.end() );
std::vector< UInt > remainingSampleIDs( N2 );
std::set_difference( allSampleIDs.begin(), allSampleIDs.end(),
drawnSampleIDs.begin(), drawnSampleIDs.end(),
remainingSampleIDs.begin() );
dataSetPtr_Type dataUnionPtr( new dataSet_Type( M_dataSetPtr1->getData() ) );
dataUnionPtr->addSamples( this->M_dataSetPtr2->getData() );
/*
std::cout << " SAMPLES " << std::endl;
std::cout << " ** pop 1 " << std::endl;
for ( std::vector< UInt >::iterator it( drawnSampleIDs.begin() ); it != drawnSampleIDs.end(); ++it )
{
std::cout << *it << " ";
}
std::cout << std::endl;
std::cout << " ** pop 2 " << std::endl;
for ( std::vector< UInt >::iterator it( remainingSampleIDs.begin() ); it != remainingSampleIDs.end(); ++it )
{
std::cout << *it << " ";
}
std::cout << std::endl;
dataUnionPtr->showMe();
*/
matrixPtr_Type G1 = dataUnionPtr->getRowSubSet( drawnSampleIDs );
matrixPtr_Type G2 = dataUnionPtr->getRowSubSet( remainingSampleIDs );
// std::cout << " Matrix 1 " << std::endl << *G1 << std::endl << std::endl;
// std::cout << " Matrix 2 " << std::endl << *G2 << std::endl << std::endl;
matrixPtr_Type varG1, varG2;
varCov.setData( G1 ); G1.reset();
varG1 = varCov.varCovMatrix();
varCov.setData( G2 ); G2.reset();
varG2 = varCov.varCovMatrix();
// std::cout << " Var-Cov 1 " << std::endl << *varG1 << std::endl << std::endl;
// std::cout << " Var-Cov 2 " << std::endl << *varG2 << std::endl << std::endl;
const Real distCurr = this->M_distPtr->compute( varG1, varG2 );
printf( "\t DistCurr = %f \t Dist0 = %f\n", distCurr, dist0 );
if( distCurr >= dist0 )
{
this->M_pValue += 1./this->M_niter;
}
}
Real pvalue(-1);
printf( " Proc %d has p-value = %f\n", this->M_mpiUtilityPtr->myRank(), this->M_pValue );
MPI_Reduce( & this->M_pValue, & pvalue, 1, MPI_DOUBLE_PRECISION, MPI_SUM, MASTER, MPI_COMM_WORLD );
MPI_Bcast( & pvalue, 1, MPI_DOUBLE_PRECISION, MASTER, MPI_COMM_WORLD );
this->M_pValue = pvalue;
return;
}
