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; }