void Body::addGlobalForce( Ogre::Vector3& force, Ogre::Vector3& pos )
{
Ogre::Vector3 bodypos;
Ogre::Quaternion bodyorient;
getPositionOrientation( bodyorient, bodypos );
Ogre::Vector3 topoint = pos - bodypos;
Ogre::Vector3 torque = topoint.crossProduct( force );
addForce( force );
addTorque( torque );
}
/*! Convenience function to apply force and torque from one force at contact point. Not sure if this is the right place for it. */
void applyForceAtContactPoint(const Vector3r& force, const Vector3r& contactPoint, const Body::id_t id1, const Vector3r& pos1, const Body::id_t id2, const Vector3r& pos2){
addForce(id1, force,scene); addTorque(id1, (contactPoint-pos1).cross(force),scene);
addForce(id2,-force,scene); addTorque(id2,-(contactPoint-pos2).cross(force),scene);
}
/* Law2_ScGeom_ViscElPhys_Basic */
void Law2_ScGeom_ViscElPhys_Basic::go(shared_ptr<IGeom>& _geom, shared_ptr<IPhys>& _phys, Interaction* I){
const ScGeom& geom=*static_cast<ScGeom*>(_geom.get());
ViscElPhys& phys=*static_cast<ViscElPhys*>(_phys.get());
const int id1 = I->getId1();
const int id2 = I->getId2();
if (geom.penetrationDepth<0) {
if (phys.liqBridgeCreated and -geom.penetrationDepth<phys.sCrit and phys.Capillar) {
phys.normalForce = -calculateCapillarForce(geom, phys)*geom.normal;
if (I->isActive) {
addForce (id1,-phys.normalForce,scene);
addForce (id2, phys.normalForce,scene);
};
return;
} else {
scene->interactions->requestErase(I);
return;
};
};
const BodyContainer& bodies = *scene->bodies;
const State& de1 = *static_cast<State*>(bodies[id1]->state.get());
const State& de2 = *static_cast<State*>(bodies[id2]->state.get());
/*
* This part for implementation of the capillar model.
* All main equations are in calculateCapillarForce function.
* There is only the determination of critical distance between spheres,
* after that the liquid bridge will be broken.
*/
if (not(phys.liqBridgeCreated) and phys.Capillar) {
phys.liqBridgeCreated = true;
Sphere* s1=dynamic_cast<Sphere*>(bodies[id1]->shape.get());
Sphere* s2=dynamic_cast<Sphere*>(bodies[id2]->shape.get());
if (s1 and s2) {
phys.R = 2 * s1->radius * s2->radius / (s1->radius + s2->radius);
} else if (s1 and not(s2)) {
phys.R = s1->radius;
} else {
phys.R = s2->radius;
}
const Real Vstar = phys.Vb/(phys.R*phys.R*phys.R);
const Real Sstar = (1+0.5*phys.theta)*(pow(Vstar,1/3.0) + 0.1*pow(Vstar,2.0/3.0)); // [Willett2000], equation (15), use the full-length e.g 2*Sc
phys.sCrit = Sstar*phys.R;
}
Vector3r& shearForce = phys.shearForce;
if (I->isFresh(scene)) shearForce=Vector3r(0,0,0);
const Real& dt = scene->dt;
shearForce = geom.rotate(shearForce);
// Handle periodicity.
const Vector3r shift2 = scene->isPeriodic ? scene->cell->intrShiftPos(I->cellDist): Vector3r::Zero();
const Vector3r shiftVel = scene->isPeriodic ? scene->cell->intrShiftVel(I->cellDist): Vector3r::Zero();
const Vector3r c1x = (geom.contactPoint - de1.pos);
const Vector3r c2x = (geom.contactPoint - de2.pos - shift2);
const Vector3r relativeVelocity = (de1.vel+de1.angVel.cross(c1x)) - (de2.vel+de2.angVel.cross(c2x)) + shiftVel;
const Real normalVelocity = geom.normal.dot(relativeVelocity);
const Vector3r shearVelocity = relativeVelocity-normalVelocity*geom.normal;
// As Chiara Modenese suggest, we store the elastic part
// and then add the viscous part if we pass the Mohr-Coulomb criterion.
// See http://www.mail-archive.com/[email protected]/msg01391.html
shearForce += phys.ks*dt*shearVelocity; // the elastic shear force have a history, but
Vector3r shearForceVisc = Vector3r::Zero(); // the viscous shear damping haven't a history because it is a function of the instant velocity
// Prevent appearing of attraction forces due to a viscous component
// [Radjai2011], page 3, equation [1.7]
// [Schwager2007]
const Real normForceReal = phys.kn * geom.penetrationDepth + phys.cn * normalVelocity;
if (normForceReal < 0) {
phys.normalForce = Vector3r::Zero();
} else {
phys.normalForce = normForceReal * geom.normal;
}
Vector3r momentResistance = Vector3r::Zero();
if (phys.mR>0.0) {
const Vector3r relAngVel = de1.angVel - de2.angVel;
relAngVel.normalized();
if (phys.mRtype == 1) {
momentResistance = -phys.mR*phys.normalForce.norm()*relAngVel; // [Zhou1999536], equation (3)
} else if (phys.mRtype == 2) {
momentResistance = -phys.mR*(c1x.cross(de1.angVel) - c2x.cross(de2.angVel)).norm()*phys.normalForce.norm()*relAngVel; // [Zhou1999536], equation (4)
}
}
const Real maxFs = phys.normalForce.squaredNorm() * std::pow(phys.tangensOfFrictionAngle,2);
if( shearForce.squaredNorm() > maxFs )
{
// Then Mohr-Coulomb is violated (so, we slip),
// we have the max value of the shear force, so
// we consider only friction damping.
const Real ratio = sqrt(maxFs) / shearForce.norm();
shearForce *= ratio;
}
else
{
// Then no slip occurs we consider friction damping + viscous damping.
shearForceVisc = phys.cs*shearVelocity;
}
if (I->isActive) {
const Vector3r f = phys.normalForce + shearForce + shearForceVisc;
addForce (id1,-f,scene);
addForce (id2, f,scene);
addTorque(id1,-c1x.cross(f)+momentResistance,scene);
addTorque(id2, c2x.cross(f)-momentResistance,scene);
}
}
void TimeStep::operator()()
{
if( numberOfSubIterations_ == 1 )
{
forceEvaluationFunc_();
collisionResponse_.timestep( timeStepSize_ );
synchronizeFunc_();
}
else
{
// during the intermediate time steps of the collision response, the currently acting forces
// (interaction forces, gravitational force, ...) have to remain constant.
// Since they are reset after the call to collision response, they have to be stored explicitly before.
// Then they are set again after each intermediate step.
// generate map from all known bodies (process local) to total forces/torques
// this has to be done on a block-local basis, since the same body could reside on several blocks from this process
using BlockID_T = domain_decomposition::IBlockID::IDType;
std::map< BlockID_T, std::map< walberla::id_t, std::array< real_t, 6 > > > forceTorqueMap;
for( auto blockIt = blockStorage_->begin(); blockIt != blockStorage_->end(); ++blockIt )
{
BlockID_T blockID = blockIt->getId().getID();
auto& blockLocalForceTorqueMap = forceTorqueMap[blockID];
// iterate over local and remote bodies and store force/torque in map
for( auto bodyIt = pe::BodyIterator::begin(*blockIt, bodyStorageID_); bodyIt != pe::BodyIterator::end(); ++bodyIt )
{
auto & f = blockLocalForceTorqueMap[ bodyIt->getSystemID() ];
const auto & force = bodyIt->getForce();
const auto & torque = bodyIt->getTorque();
f = {{force[0], force[1], force[2], torque[0], torque[1], torque[2] }};
}
}
// perform pe time steps
const real_t subTimeStepSize = timeStepSize_ / real_c( numberOfSubIterations_ );
for( uint_t i = 0; i != numberOfSubIterations_; ++i )
{
// in the first iteration, forces are already set
if( i != 0 )
{
for( auto blockIt = blockStorage_->begin(); blockIt != blockStorage_->end(); ++blockIt )
{
BlockID_T blockID = blockIt->getId().getID();
auto& blockLocalForceTorqueMap = forceTorqueMap[blockID];
// re-set stored force/torque on bodies
for( auto bodyIt = pe::BodyIterator::begin(*blockIt, bodyStorageID_); bodyIt != pe::BodyIterator::end(); ++bodyIt )
{
const auto f = blockLocalForceTorqueMap.find( bodyIt->getSystemID() );
if( f != blockLocalForceTorqueMap.end() )
{
const auto & ftValues = f->second;
bodyIt->addForce ( ftValues[0], ftValues[1], ftValues[2] );
bodyIt->addTorque( ftValues[3], ftValues[4], ftValues[5] );
}
}
}
}
// evaluate forces (e.g. lubrication forces)
forceEvaluationFunc_();
collisionResponse_.timestep( subTimeStepSize );
synchronizeFunc_();
}
}
}
/* Law2_ScGeom_ViscElPhys_Basic */
void Law2_ScGeom_ViscElPhys_Basic::go(shared_ptr<IGeom>& _geom, shared_ptr<IPhys>& _phys, Interaction* I){
const ScGeom& geom=*static_cast<ScGeom*>(_geom.get());
ViscElPhys& phys=*static_cast<ViscElPhys*>(_phys.get());
const int id1 = I->getId1();
const int id2 = I->getId2();
if (geom.penetrationDepth<0) {
scene->interactions->requestErase(I);
return;
}
const BodyContainer& bodies = *scene->bodies;
const State& de1 = *static_cast<State*>(bodies[id1]->state.get());
const State& de2 = *static_cast<State*>(bodies[id2]->state.get());
Vector3r& shearForce = phys.shearForce;
if (I->isFresh(scene)) shearForce=Vector3r(0,0,0);
const Real& dt = scene->dt;
//Vector3r axis = phys.prevNormal.cross(geom.normal);
//shearForce -= shearForce.cross(axis);
//const Real angle = dt*0.5*geom.normal.dot(de1.angVel + de2.angVel);
//axis = angle*geom.normal;
//shearForce -= shearForce.cross(axis);
shearForce = geom.rotate(shearForce);
// Handle periodicity.
const Vector3r shift2 = scene->isPeriodic ? scene->cell->intrShiftPos(I->cellDist): Vector3r::Zero();
const Vector3r shiftVel = scene->isPeriodic ? scene->cell->intrShiftVel(I->cellDist): Vector3r::Zero();
const Vector3r c1x = (geom.contactPoint - de1.pos);
const Vector3r c2x = (geom.contactPoint - de2.pos - shift2);
const Vector3r relativeVelocity = (de1.vel+de1.angVel.cross(c1x)) - (de2.vel+de2.angVel.cross(c2x)) + shiftVel;
const Real normalVelocity = geom.normal.dot(relativeVelocity);
const Vector3r shearVelocity = relativeVelocity-normalVelocity*geom.normal;
// As Chiara Modenese suggest, we store the elastic part
// and then add the viscous part if we pass the Mohr-Coulomb criterion.
// See http://www.mail-archive.com/[email protected]/msg01391.html
shearForce += phys.ks*dt*shearVelocity; // the elastic shear force have a history, but
Vector3r shearForceVisc = Vector3r::Zero(); // the viscous shear damping haven't a history because it is a function of the instant velocity
phys.normalForce = ( phys.kn * geom.penetrationDepth + phys.cn * normalVelocity ) * geom.normal;
//phys.prevNormal = geom.normal;
const Real maxFs = phys.normalForce.squaredNorm() * std::pow(phys.tangensOfFrictionAngle,2);
if( shearForce.squaredNorm() > maxFs )
{
// Then Mohr-Coulomb is violated (so, we slip),
// we have the max value of the shear force, so
// we consider only friction damping.
const Real ratio = sqrt(maxFs) / shearForce.norm();
shearForce *= ratio;
}
else
{
// Then no slip occurs we consider friction damping + viscous damping.
shearForceVisc = phys.cs*shearVelocity;
}
const Vector3r f = phys.normalForce + shearForce + shearForceVisc;
addForce (id1,-f,scene);
addForce (id2, f,scene);
addTorque(id1,-c1x.cross(f),scene);
addTorque(id2, c2x.cross(f),scene);
}
void RigidBody::addForceAtWorldPoint(const Mat<float>& force, const Mat<float>& pointW)
{
addForce(force);
addTorque( crossproductV( pointW-extract(Pose->exp(), 1,4, 3,4), force) );
}
/*! Constitutive law */
void FCPM::go(shared_ptr<IGeom>& _geom, shared_ptr<IPhys>& _phys, Interaction* I){
const ScGeom& geom=*static_cast<ScGeom*>(_geom.get());
FreshConcretePhys& phys=*static_cast<FreshConcretePhys*>(_phys.get());
const int id1 = I->getId1();
const int id2 = I->getId2();
const BodyContainer& bodies = *scene->bodies;
const State& de1 = *static_cast<State*>(bodies[id1]->state.get());
const State& de2 = *static_cast<State*>(bodies[id2]->state.get());
//Calculation of the max penetretion and the radius of the overlap area
Sphere* s1=dynamic_cast<Sphere*>(bodies[id1]->shape.get());
Sphere* s2=dynamic_cast<Sphere*>(bodies[id2]->shape.get());
Real dist;
Real contactRadius;
Real OverlapRadius;
if (s1 and s2) {
phys.maxPenetration=s1->radius * phys.Penetration1 + s2->radius * phys.Penetration2;
dist = s1->radius + s2->radius - geom.penetrationDepth;
OverlapRadius = pow(((4 * pow(dist,2) * pow(s1->radius,2) - pow((pow(dist,2) - pow(s2->radius,2) + pow(s1->radius,2)),2)) / (4 * pow(dist,2))),(1.0/2.0));
//contactRadius = (pow(s1->radius,2) + pow(s2->radius,2))/(s1->radius + s2->radius);
contactRadius = s1->radius;
} else if (s1 and not(s2)) {
phys.maxPenetration=s1->radius * phys.Penetration1;
dist = s1->radius - geom.penetrationDepth;
OverlapRadius =pow((pow(s1->radius,2) - pow(dist,2)),(1.0/2.0));
contactRadius = s1->radius;
} else {
phys.maxPenetration=s2->radius * phys.Penetration2;
dist = s2->radius - geom.penetrationDepth;
OverlapRadius = pow((pow(s2->radius,2) - pow(dist,2)),(1.0/2.0));
contactRadius = s2->radius;
}
Vector3r& shearForce = phys.shearForce;
if (I->isFresh(scene)) shearForce=Vector3r(0,0,0);
const Real& dt = scene->dt;
shearForce = geom.rotate(shearForce);
// Handle periodicity.
const Vector3r shift2 = scene->isPeriodic ? scene->cell->intrShiftPos(I->cellDist): Vector3r::Zero();
const Vector3r shiftVel = scene->isPeriodic ? scene->cell->intrShiftVel(I->cellDist): Vector3r::Zero();
const Vector3r c1x = (geom.contactPoint - de1.pos);
const Vector3r c2x = (geom.contactPoint - de2.pos - shift2);
const Vector3r relativeVelocity = (de1.vel+de1.angVel.cross(c1x)) - (de2.vel+de2.angVel.cross(c2x)) + shiftVel;
const Real normalVelocity = geom.normal.dot(relativeVelocity);
const Vector3r shearVelocity = relativeVelocity-normalVelocity*geom.normal;
//Normal Force
Real OverlapArea;
Real MaxArea;
OverlapArea = 3.1415 * pow(OverlapRadius,2);
MaxArea = 3.1415 * pow(contactRadius,2);
Real Mult;
if(OverlapRadius < contactRadius){
Mult = OverlapArea / MaxArea;
}
else{
Mult = 1;
}
Real Fn;
if(geom.penetrationDepth>=0){
//Compression
if(normalVelocity>=0){
if (geom.penetrationDepth >= phys.maxPenetration){
Fn = phys.knI * (geom.penetrationDepth - phys.previousun) + phys.previousElasticN + phys.cnI * normalVelocity;
phys.previousElasticN += phys.knI * (geom.penetrationDepth - phys.previousun);
phys.normalForce = Fn * geom.normal;
phys.t = 0; phys.t2 = 0;
//phys.previousElasticTensionN = 0;
}
else{
Fn = Mult * phys.kn * (geom.penetrationDepth - phys.previousun) + Mult * phys.previousElasticN + phys.cn * normalVelocity;
phys.previousElasticN += Mult * phys.kn * (geom.penetrationDepth - phys.previousun);
phys.finalElasticN += Mult * phys.kn * (geom.penetrationDepth - phys.previousun);
phys.normalForce = Fn * geom.normal;
phys.t = 0; phys.t2 = 0;
//phys.previousElasticTensionN = 0;
}
}
//Tension
else if(normalVelocity<0){
if(phys.t == 0){
phys.maxNormalComp = - phys.finalElasticN;
//printf("------> %E \n", phys.maxNormalComp);
phys.RupOverlap = phys.previousun;
//throw runtime_error("STOP");
phys.t = 1;
}
Real MaxTension = phys.maxNormalComp * 0.25;
Real FnTension = phys.previousElasticTensionN + Mult * phys.kn * dt * normalVelocity;
//printf("===:: %E \n", MaxTension);
//printf("===:: %E \n", FnTension);
if (FnTension > MaxTension && phys.t2 == 0){
Fn = phys.previousElasticTensionN + Mult * phys.kn * dt * normalVelocity;
phys.previousElasticTensionN = Fn;
phys.normalForce = Fn * geom.normal;
phys.RupOverlap = geom.penetrationDepth;
//phys.previousElasticN = 0;
//printf("===-- %E \n", Fn);
}
else{
//phys.DamageTension = geom.penetrationDepth / (phys.maxPenetration - phys.maxPenetration * phys.RupTension);
//phys.previousElasticTensionN -= phys.DamageTension * Mult * phys.kn * dt * normalVelocity;
phys.t2 = 1;
Fn = MaxTension * geom.penetrationDepth / phys.RupOverlap;
phys.normalForce = Fn * geom.normal;
//printf("===// %E \n", Fn);
//throw runtime_error("STOP");
}
}
}
else{
Fn = 0;
phys.normalForce = Fn * geom.normal;
phys.finalElasticN = 0;
}
phys.previousun = geom.penetrationDepth;
//Shear Force
shearForce += phys.ks*dt*shearVelocity;
Vector3r shearForceVisc = Vector3r::Zero();
const Real maxFs = phys.normalForce.squaredNorm() * std::pow(phys.tangensOfFrictionAngle,2);
if( shearForce.squaredNorm() > maxFs ){
// Then Mohr-Coulomb is violated (so, we slip),
// we have the max value of the shear force, so
// we consider only friction damping.
const Real ratio = sqrt(maxFs) / shearForce.norm();
shearForce *= ratio;
}
else{
// Then no slip occurs we consider friction damping + viscous damping.
shearForceVisc = phys.cs*shearVelocity;
}
if (I->isActive) {
const Vector3r f = phys.normalForce + shearForce + shearForceVisc;
addForce (id1,-f,scene);
addForce (id2, f,scene);
addTorque(id1,-c1x.cross(f),scene);
addTorque(id2, c2x.cross(f),scene);
}
}