void collisionTester::initAlgo(){
btCollisionObjectWrapper sA(0,sphereA.getCollisionShape(), &sphereA, sphereA.getWorldTransform());
btCollisionObjectWrapper sB(0,sphereB.getCollisionShape(), &sphereB, sphereB.getWorldTransform());
btCollisionObjectWrapper cA(0,cylinderA.getCollisionShape(), &cylinderA, sphereA.getWorldTransform());
btCollisionObjectWrapper cB(0,cylinderB.getCollisionShape(), &cylinderB, sphereB.getWorldTransform());
algoSS = collisionWorld->getDispatcher()->findAlgorithm(&sA, &sB);
algoSC = collisionWorld->getDispatcher()->findAlgorithm(&sA, &cA);
algoCC = collisionWorld->getDispatcher()->findAlgorithm(&cA, &cB);
}
/**
* This method should be used both as a solution to 3D cases, and as a general termination method for algorithms that reduce D-dimensional problem to 3-dimensional one.
*
* This is the implementation of the algorithm for computing hypervolume as it was presented by Nicola Beume et al.
* The implementation uses std::multiset (which is based on red-black tree data structure) as a container for the sweeping front.
* Original implementation by Beume et. al uses AVL-tree.
* The difference is insiginificant as the important characteristics (maintaining order when traversing, self-balancing) of both structures and the asymptotic times (O(log n) updates) are guaranteed.
* Computational complexity: O(n*log(n))
*
* @param[in] points vector of points containing the 3-dimensional points for which we compute the hypervolume
* @param[in] r_point reference point for the points
*
* @return hypervolume.
*/
double hv3d::compute(std::vector<fitness_vector> &points, const fitness_vector &r_point) const
{
if (m_initial_sorting) {
sort(points.begin(), points.end(), fitness_vector_cmp(2,'<'));
}
double V = 0.0; // hypervolume
double A = 0.0; // area of the sweeping plane
std::multiset<fitness_vector, fitness_vector_cmp> T(fitness_vector_cmp(0, '>'));
// sentinel points (r_point[0], -INF, r_point[2]) and (-INF, r_point[1], r_point[2])
const double INF = std::numeric_limits<double>::max();
fitness_vector sA(r_point.begin(), r_point.end()); sA[1] = -INF;
fitness_vector sB(r_point.begin(), r_point.end()); sB[0] = -INF;
T.insert(sA);
T.insert(sB);
double z3 = points[0][2];
T.insert(points[0]);
A = fabs((points[0][0] - r_point[0]) * (points[0][1] - r_point[1]));
std::multiset<fitness_vector>::iterator p;
std::multiset<fitness_vector>::iterator q;
for(std::vector<fitness_vector>::size_type idx = 1 ; idx < points.size() ; ++idx) {
p = T.insert(points[idx]);
q = (p);
++q; //setup q to be a successor of p
if ( (*q)[1] <= (*p)[1] ) { // current point is dominated
T.erase(p); // disregard the point from further calculation
} else {
V += A * fabs(z3 - (*p)[2]);
z3 = (*p)[2];
std::multiset<fitness_vector>::reverse_iterator rev_it(q);
++rev_it;
std::multiset<fitness_vector>::reverse_iterator erase_begin (rev_it);
std::multiset<fitness_vector>::reverse_iterator rev_it_pred;
while((*rev_it)[1] >= (*p)[1] ) {
rev_it_pred = rev_it;
++rev_it_pred;
A -= fabs(((*rev_it)[0] - (*rev_it_pred)[0])*((*rev_it)[1] - (*q)[1]));
++rev_it;
}
A += fabs(((*p)[0] - (*(rev_it))[0])*((*p)[1] - (*q)[1]));
T.erase(rev_it.base(),erase_begin.base());
}
}
V += A * fabs(z3 - r_point[2]);
return V;
}
int CALLBACK CLiveList::SortCallback(LPARAM lParam1, LPARAM lParam2, LPARAM lParamSort)
{
CListCtrl* pList = (CListCtrl*)lParamSort;
ASSERT_VALID( pList );
int nColumn = (int)GetWindowLongPtr( pList->GetSafeHwnd(), GWLP_USERDATA );
LV_FINDINFO pFind;
pFind.flags = LVFI_PARAM;
pFind.lParam = lParam1;
int nA = pList->FindItem( &pFind );
pFind.lParam = lParam2;
int nB = pList->FindItem( &pFind );
CString sA( pList->GetItemText( nA, abs( nColumn ) - 1 ) );
CString sB( pList->GetItemText( nB, abs( nColumn ) - 1 ) );
return ( nColumn > 0 ) ? SortProc( sB, sA ) : SortProc( sA, sB );
}
void test_kronecker_product()
{
// DM = dense matrix; SM = sparse matrix
Matrix<double, 2, 3> DM_a;
SparseMatrix<double> SM_a(2,3);
SM_a.insert(0,0) = DM_a.coeffRef(0,0) = -0.4461540300782201;
SM_a.insert(0,1) = DM_a.coeffRef(0,1) = -0.8057364375283049;
SM_a.insert(0,2) = DM_a.coeffRef(0,2) = 0.3896572459516341;
SM_a.insert(1,0) = DM_a.coeffRef(1,0) = -0.9076572187376921;
SM_a.insert(1,1) = DM_a.coeffRef(1,1) = 0.6469156566545853;
SM_a.insert(1,2) = DM_a.coeffRef(1,2) = -0.3658010398782789;
MatrixXd DM_b(3,2);
SparseMatrix<double> SM_b(3,2);
SM_b.insert(0,0) = DM_b.coeffRef(0,0) = 0.9004440976767099;
SM_b.insert(0,1) = DM_b.coeffRef(0,1) = -0.2368830858139832;
SM_b.insert(1,0) = DM_b.coeffRef(1,0) = -0.9311078389941825;
SM_b.insert(1,1) = DM_b.coeffRef(1,1) = 0.5310335762980047;
SM_b.insert(2,0) = DM_b.coeffRef(2,0) = -0.1225112806872035;
SM_b.insert(2,1) = DM_b.coeffRef(2,1) = 0.5903998022741264;
SparseMatrix<double,RowMajor> SM_row_a(SM_a), SM_row_b(SM_b);
// test DM_fixedSize = kroneckerProduct(DM_block,DM)
Matrix<double, 6, 6> DM_fix_ab = kroneckerProduct(DM_a.topLeftCorner<2,3>(),DM_b);
CALL_SUBTEST(check_kronecker_product(DM_fix_ab));
CALL_SUBTEST(check_kronecker_product(kroneckerProduct(DM_a.topLeftCorner<2,3>(),DM_b)));
for(int i=0;i<DM_fix_ab.rows();++i)
for(int j=0;j<DM_fix_ab.cols();++j)
VERIFY_IS_APPROX(kroneckerProduct(DM_a,DM_b).coeff(i,j), DM_fix_ab(i,j));
// test DM_block = kroneckerProduct(DM,DM)
MatrixXd DM_block_ab(10,15);
DM_block_ab.block<6,6>(2,5) = kroneckerProduct(DM_a,DM_b);
CALL_SUBTEST(check_kronecker_product(DM_block_ab.block<6,6>(2,5)));
// test DM = kroneckerProduct(DM,DM)
MatrixXd DM_ab = kroneckerProduct(DM_a,DM_b);
CALL_SUBTEST(check_kronecker_product(DM_ab));
CALL_SUBTEST(check_kronecker_product(kroneckerProduct(DM_a,DM_b)));
// test SM = kroneckerProduct(SM,DM)
SparseMatrix<double> SM_ab = kroneckerProduct(SM_a,DM_b);
CALL_SUBTEST(check_kronecker_product(SM_ab));
SparseMatrix<double,RowMajor> SM_ab2 = kroneckerProduct(SM_a,DM_b);
CALL_SUBTEST(check_kronecker_product(SM_ab2));
CALL_SUBTEST(check_kronecker_product(kroneckerProduct(SM_a,DM_b)));
// test SM = kroneckerProduct(DM,SM)
SM_ab.setZero();
SM_ab.insert(0,0)=37.0;
SM_ab = kroneckerProduct(DM_a,SM_b);
CALL_SUBTEST(check_kronecker_product(SM_ab));
SM_ab2.setZero();
SM_ab2.insert(0,0)=37.0;
SM_ab2 = kroneckerProduct(DM_a,SM_b);
CALL_SUBTEST(check_kronecker_product(SM_ab2));
CALL_SUBTEST(check_kronecker_product(kroneckerProduct(DM_a,SM_b)));
// test SM = kroneckerProduct(SM,SM)
SM_ab.resize(2,33);
SM_ab.insert(0,0)=37.0;
SM_ab = kroneckerProduct(SM_a,SM_b);
CALL_SUBTEST(check_kronecker_product(SM_ab));
SM_ab2.resize(5,11);
SM_ab2.insert(0,0)=37.0;
SM_ab2 = kroneckerProduct(SM_a,SM_b);
CALL_SUBTEST(check_kronecker_product(SM_ab2));
CALL_SUBTEST(check_kronecker_product(kroneckerProduct(SM_a,SM_b)));
// test SM = kroneckerProduct(SM,SM) with sparse pattern
SM_a.resize(4,5);
SM_b.resize(3,2);
SM_a.resizeNonZeros(0);
SM_b.resizeNonZeros(0);
SM_a.insert(1,0) = -0.1;
SM_a.insert(0,3) = -0.2;
SM_a.insert(2,4) = 0.3;
SM_a.finalize();
SM_b.insert(0,0) = 0.4;
SM_b.insert(2,1) = -0.5;
SM_b.finalize();
SM_ab.resize(1,1);
SM_ab.insert(0,0)=37.0;
SM_ab = kroneckerProduct(SM_a,SM_b);
CALL_SUBTEST(check_sparse_kronecker_product(SM_ab));
// test dimension of result of DM = kroneckerProduct(DM,DM)
MatrixXd DM_a2(2,1);
MatrixXd DM_b2(5,4);
MatrixXd DM_ab2 = kroneckerProduct(DM_a2,DM_b2);
CALL_SUBTEST(check_dimension(DM_ab2,2*5,1*4));
DM_a2.resize(10,9);
DM_b2.resize(4,8);
DM_ab2 = kroneckerProduct(DM_a2,DM_b2);
CALL_SUBTEST(check_dimension(DM_ab2,10*4,9*8));
for(int i = 0; i < g_repeat; i++)
{
double density = Eigen::internal::random<double>(0.01,0.5);
int ra = Eigen::internal::random<int>(1,50);
int ca = Eigen::internal::random<int>(1,50);
int rb = Eigen::internal::random<int>(1,50);
int cb = Eigen::internal::random<int>(1,50);
SparseMatrix<float,ColMajor> sA(ra,ca), sB(rb,cb), sC;
SparseMatrix<float,RowMajor> sC2;
MatrixXf dA(ra,ca), dB(rb,cb), dC;
initSparse(density, dA, sA);
initSparse(density, dB, sB);
sC = kroneckerProduct(sA,sB);
dC = kroneckerProduct(dA,dB);
VERIFY_IS_APPROX(MatrixXf(sC),dC);
sC = kroneckerProduct(sA.transpose(),sB);
dC = kroneckerProduct(dA.transpose(),dB);
VERIFY_IS_APPROX(MatrixXf(sC),dC);
sC = kroneckerProduct(sA.transpose(),sB.transpose());
dC = kroneckerProduct(dA.transpose(),dB.transpose());
VERIFY_IS_APPROX(MatrixXf(sC),dC);
sC = kroneckerProduct(sA,sB.transpose());
dC = kroneckerProduct(dA,dB.transpose());
VERIFY_IS_APPROX(MatrixXf(sC),dC);
sC2 = kroneckerProduct(sA,sB);
dC = kroneckerProduct(dA,dB);
VERIFY_IS_APPROX(MatrixXf(sC2),dC);
}
}
void ContactLoop::run(){
#ifdef CONTACTLOOP_TIMING
timingDeltas->start();
#endif
DemField& dem=field->cast<DemField>();
if(dem.contacts->removeAllPending()>0 && !alreadyWarnedNoCollider){
LOG_WARN("Contacts pending removal found (and were removed); no collider being used?");
alreadyWarnedNoCollider=true;
}
if(dem.contacts->dirty){
throw std::logic_error("ContactContainer::dirty is true; the collider should re-initialize in such case and clear the dirty flag.");
}
// update Scene* of the dispatchers
geoDisp->scene=phyDisp->scene=lawDisp->scene=scene;
geoDisp->field=phyDisp->field=lawDisp->field=field;
// ask dispatchers to update Scene* of their functors
geoDisp->updateScenePtr(); phyDisp->updateScenePtr(); lawDisp->updateScenePtr();
// cache transformed cell size
Matrix3r cellHsize; if(scene->isPeriodic) cellHsize=scene->cell->hSize;
stress=Matrix3r::Zero();
// force removal of interactions that were not encountered by the collider
// (only for some kinds of colliders; see comment for InteractionContainer::iterColliderLastRun)
bool removeUnseen=(dem.contacts->stepColliderLastRun>=0 && dem.contacts->stepColliderLastRun==scene->step);
const bool doStress=(evalStress && scene->isPeriodic);
const bool deterministic(scene->deterministic);
if(reorderEvery>0 && (scene->step%reorderEvery==0)) reorderContacts();
size_t size=dem.contacts->size();
CONTACTLOOP_CHECKPOINT("prologue");
const bool hasHook=!!hook;
#ifdef WOO_OPENMP
#pragma omp parallel for schedule(guided)
#endif
for(size_t i=0; i<size; i++){
CONTACTLOOP_CHECKPOINT("loop-begin");
const shared_ptr<Contact>& C=(*dem.contacts)[i];
if(unlikely(removeUnseen && !C->isReal() && C->stepLastSeen<scene->step)) { removeAfterLoop(C); continue; }
if(unlikely(!C->isReal() && !C->isColliding())){ removeAfterLoop(C); continue; }
/* this block is called exactly once for every potential contact created; it should check whether shapes
should be swapped, and also set minDist00Sq if used (Sphere-Sphere only)
*/
if(unlikely(!C->isReal() && C->isFresh(scene))){
bool swap=false;
const shared_ptr<CGeomFunctor>& cgf=geoDisp->getFunctor2D(C->leakPA()->shape,C->leakPB()->shape,swap);
if(!cgf) continue;
if(swap){ C->swapOrder(); }
cgf->setMinDist00Sq(C->pA.lock()->shape,C->pB.lock()->shape,C);
CONTACTLOOP_CHECKPOINT("swap-check");
}
Particle *pA=C->leakPA(), *pB=C->leakPB();
Vector3r shift2=(scene->isPeriodic?scene->cell->intrShiftPos(C->cellDist):Vector3r::Zero());
// the order is as the geometry functor expects it
shared_ptr<Shape>& sA(pA->shape); shared_ptr<Shape>& sB(pB->shape);
// if minDist00Sq is defined, we might see that there is no contact without ever calling the functor
// saving quite a few calls for sphere-sphere contacts
if(likely(dist00 && !C->isReal() && !C->isFresh(scene) && C->minDist00Sq>0 && (sA->nodes[0]->pos-(sB->nodes[0]->pos+shift2)).squaredNorm()>C->minDist00Sq)){
CONTACTLOOP_CHECKPOINT("dist00Sq-too-far");
continue;
}
CONTACTLOOP_CHECKPOINT("pre-geom");
bool geomCreated=geoDisp->operator()(sA,sB,shift2,/*force*/false,C);
CONTACTLOOP_CHECKPOINT("geom");
if(!geomCreated){
if(/* has both geo and phy */C->isReal()) LOG_ERROR("CGeomFunctor "<<geoDisp->getClassName()<<" did not update existing contact ##"<<pA->id<<"+"<<pB->id);
continue;
}
// CPhys
if(!C->phys) C->stepCreated=scene->step;
if(!C->phys || updatePhys>UPDATE_PHYS_NEVER) phyDisp->operator()(pA->material,pB->material,C);
if(!C->phys) throw std::runtime_error("ContactLoop: ##"+to_string(pA->id)+"+"+to_string(pB->id)+": con Contact.phys created from materials "+pA->material->getClassName()+" and "+pB->material->getClassName()+" (a CPhysFunctor must be available for every contacting material combination).");
if(hasHook && C->isFresh(scene) && hook->isMatch(pA->mask,pB->mask)) hook->hookNew(dem,C);
CONTACTLOOP_CHECKPOINT("phys");
// CLaw
bool keepContact=lawDisp->operator()(C->geom,C->phys,C);
if(!keepContact){
if(hasHook && hook->isMatch(pA->mask,pB->mask)) hook->hookDel(dem,C); // call before requestRemove resets contact internals
dem.contacts->requestRemoval(C);
}
CONTACTLOOP_CHECKPOINT("law");
if(applyForces && C->isReal() && likely(!deterministic)){
applyForceUninodal(C,pA);
applyForceUninodal(C,pB);
#if 0
for(const Particle* particle:{pA,pB}){
// remove once tested thoroughly
const shared_ptr<Shape>& sh(particle->shape);
if(!sh || sh->nodes.size()!=1) continue;
// if(sh->nodes.size()!=1) continue;
#if 0
for(size_t i=0; i<sh->nodes.size(); i++){
if((sh->nodes[i]->getData<DemData>().flags&DemData::DOF_ALL)!=DemData::DOF_ALL) LOG_WARN("Multinodal #"<<particle->id<<" has free DOFs, but force will not be applied; set ContactLoop.applyForces=False and use IntraForce(...) dispatcher instead.");
}
#endif
Vector3r F,T,xc;
std::tie(F,T,xc)=C->getForceTorqueBranch(particle,/*nodeI*/0,scene);
sh->nodes[0]->getData<DemData>().addForceTorque(F,xc.cross(F)+T);
}
#endif
}
// track gradV work
/* this is meant to avoid calling extra loop at every step, since the work must be evaluated incrementally */
if(doStress && /*contact law deleted the contact?*/ C->isReal()){
const auto& nnA(pA->shape->nodes); const auto& nnB(pB->shape->nodes);
if(nnA.size()!=1 || nnB.size()!=1) throw std::runtime_error("ContactLoop.trackWork not allowed with multi-nodal particles in contact (##"+lexical_cast<string>(pA->id)+"+"+lexical_cast<string>(pB->id)+")");
Vector3r branch=C->dPos(scene); // (nnB[0]->pos-nnA[0]->pos+scene->cell->intrShiftPos(C->cellDist));
Vector3r F=C->geom->node->ori*C->phys->force; // force in global coords
#ifdef WOO_OPENMP
#pragma omp critical
#endif
{
stress.noalias()+=F*branch.transpose();
}
}
CONTACTLOOP_CHECKPOINT("force+stress");
}
// process removeAfterLoop
#ifdef WOO_OPENMP
for(list<shared_ptr<Contact>>& l: removeAfterLoopRefs){
for(const shared_ptr<Contact>& c: l) dem.contacts->remove(c);
l.clear();
}
#else
for(const shared_ptr<Contact>& c: removeAfterLoopRefs) dem.contacts->remove(c);
removeAfterLoopRefs.clear();
#endif
// compute gradVWork eventually
if(doStress){
stress/=scene->cell->getVolume();
if(scene->trackEnergy){
Matrix3r midStress=.5*(stress+prevStress);
Real midVol=(!isnan(prevVol)?.5*(prevVol+scene->cell->getVolume()):scene->cell->getVolume());
Real dW=-(scene->cell->gradV*midStress).trace()*scene->dt*midVol;
scene->energy->add(dW,"gradV",gradVIx,EnergyTracker::IsIncrement | EnergyTracker::ZeroDontCreate);
}
//prevTrGradVStress=trGradVStress;
prevVol=scene->cell->getVolume();
prevStress=stress;
}
// apply forces deterministically, after the parallel loop
// this could be also loop over particles in parallel (since per-particle loop would still be deterministic) but time per particle is very small here
if(unlikely(deterministic) && applyForces){
// non-paralell loop here
for(const auto& C: *dem.contacts){
if(!C->isReal()) continue;
applyForceUninodal(C,C->leakPA());
applyForceUninodal(C,C->leakPB());
}
}
// reset updatePhys if it was to be used only once
if(updatePhys==UPDATE_PHYS_ONCE) updatePhys=UPDATE_PHYS_NEVER;
CONTACTLOOP_CHECKPOINT("epilogue");
}
