void Foam::sixDoFSolvers::CrankNicolson::solve
(
bool firstIter,
const vector& fGlobal,
const vector& tauGlobal,
scalar deltaT,
scalar deltaT0
)
{
// Update the linear acceleration and torque
updateAcceleration(fGlobal, tauGlobal);
// Correct linear velocity
v() = tConstraints()
& (v0() + aDamp()*deltaT*(aoc_*a() + (1 - aoc_)*a0()));
// Correct angular momentum
pi() = rConstraints()
& (pi0() + aDamp()*deltaT*(aoc_*tau() + (1 - aoc_)*tau0()));
// Correct position
centreOfRotation() =
centreOfRotation0() + deltaT*(voc_*v() + (1 - voc_)*v0());
// Correct orientation
Tuple2<tensor, vector> Qpi =
rotate(Q0(), (voc_*pi() + (1 - voc_)*pi0()), deltaT);
Q() = Qpi.first();
}
void Foam::sixDoFRigidBodyMotion::updatePosition
(
scalar deltaT,
scalar deltaT0
)
{
// First leapfrog velocity adjust and motion part, required before
// force calculation
if (Pstream::master())
{
v() = tConstraints_ & aDamp_*(v0() + 0.5*deltaT0*a());
pi() = rConstraints_ & aDamp_*(pi0() + 0.5*deltaT0*tau());
// Leapfrog move part
centreOfRotation() = centreOfRotation0() + deltaT*v();
// Leapfrog orientation adjustment
Tuple2<tensor, vector> Qpi = rotate(Q0(), pi(), deltaT);
Q() = Qpi.first();
pi() = rConstraints_ & Qpi.second();
}
Pstream::scatter(motionState_);
}
void Foam::sixDoFSolvers::Newmark::solve
(
bool firstIter,
const vector& fGlobal,
const vector& tauGlobal,
scalar deltaT,
scalar deltaT0
)
{
// Update the linear acceleration and torque
updateAcceleration(fGlobal, tauGlobal);
// Correct linear velocity
v() =
tConstraints()
& (v0() + aDamp()*deltaT*(gamma_*a() + (1 - gamma_)*a0()));
// Correct angular momentum
pi() =
rConstraints()
& (pi0() + aDamp()*deltaT*(gamma_*tau() + (1 - gamma_)*tau0()));
// Correct position
centreOfRotation() =
centreOfRotation0()
+ (
tConstraints()
& (
deltaT*v0()
+ aDamp()*sqr(deltaT)*(beta_*a() + (0.5 - beta_)*a0())
)
);
// Correct orientation
vector piDeltaT =
rConstraints()
& (
deltaT*pi0()
+ aDamp()*sqr(deltaT)*(beta_*tau() + (0.5 - beta_)*tau0())
);
Tuple2<tensor, vector> Qpi = rotate(Q0(), piDeltaT, 1);
Q() = Qpi.first();
}
void Foam::sixDoFRigidBodyMotion::updatePosition
(
bool firstIter,
scalar deltaT,
scalar deltaT0
)
{
if (Pstream::master())
{
if (firstIter)
{
// First simplectic step:
// Half-step for linear and angular velocities
// Update position and orientation
v() = tConstraints_ & (v0() + aDamp_*0.5*deltaT0*a());
pi() = rConstraints_ & (pi0() + aDamp_*0.5*deltaT0*tau());
centreOfRotation() = centreOfRotation0() + deltaT*v();
}
else
{
// For subsequent iterations use Crank-Nicolson
v() = tConstraints_
& (v0() + aDamp_*0.5*deltaT*(a() + motionState0_.a()));
pi() = rConstraints_
& (pi0() + aDamp_*0.5*deltaT*(tau() + motionState0_.tau()));
centreOfRotation() =
centreOfRotation0() + 0.5*deltaT*(v() + motionState0_.v());
}
// Correct orientation
Tuple2<tensor, vector> Qpi = rotate(Q0(), pi(), deltaT);
Q() = Qpi.first();
pi() = rConstraints_ & Qpi.second();
}
Pstream::scatter(motionState_);
}
int getBucket(const Tuple2<node,node> &t) override {
return t.x2()->index();
}
void UpwardPlanRep::augment()
{
if (isAugmented)
return;
OGDF_ASSERT(hasSingleSource(*this));
List<adjEntry> switches;
hasSingleSource(*this, s_hat);
OGDF_ASSERT(this == &m_Gamma.getGraph());
for(adjEntry adj : s_hat->adjEntries)
m_isSourceArc[adj->theEdge()] = true;
FaceSinkGraph fsg(m_Gamma, s_hat);
List<adjEntry> dummyList;
FaceArray< List<adjEntry> > sinkSwitches(m_Gamma, dummyList);
fsg.sinkSwitches(sinkSwitches);
m_sinkSwitchOf.init(*this, nullptr);
List<Tuple2<adjEntry, adjEntry>> list;
for(face f : m_Gamma.faces) {
adjEntry adj_top;
switches = sinkSwitches[f];
if (switches.empty() || f == m_Gamma.externalFace())
continue;
else
adj_top = switches.popFrontRet(); // first switch in the list is a top sink switch
while (!switches.empty()) {
adjEntry adj = switches.popFrontRet();
Tuple2<adjEntry, adjEntry> pair(adj, adj_top);
list.pushBack(pair);
}
}
// construct sink arcs
// for the ext. face
extFaceHandle = getAdjEntry(m_Gamma, s_hat, m_Gamma.externalFace());
node t = this->newNode();
switches = sinkSwitches[m_Gamma.externalFace()];
OGDF_ASSERT(!switches.empty());
while (!switches.empty()) {
adjEntry adj = switches.popFrontRet();
edge e_new;
if (t->degree() == 0) {
e_new = m_Gamma.addEdgeToIsolatedNode(adj, t);
}
else {
adjEntry adjTgt = getAdjEntry(m_Gamma, t, m_Gamma.rightFace(adj));
e_new = m_Gamma.splitFace(adj, adjTgt);
}
m_isSinkArc[e_new] = true;
m_Gamma.setExternalFace(m_Gamma.rightFace(extFaceHandle));
}
/*
* set ext. face handle
* we add a additional node t_hat and an addtional edge e=(t, t_hat)
* e will never been crossed. we use e as the ext. face handle
*/
t_hat = this->newNode();
adjEntry adjSource = getAdjEntry(m_Gamma, t, m_Gamma.externalFace());
extFaceHandle = m_Gamma.addEdgeToIsolatedNode(adjSource, t_hat)->adjTarget();
m_isSinkArc[extFaceHandle->theEdge()] = true; // not really a sink arc !! TODO??
m_Gamma.setExternalFace(m_Gamma.rightFace(extFaceHandle));
//for int. faces
while (!list.empty()) {
Tuple2<adjEntry, adjEntry> pair = list.popFrontRet();
edge e_new = nullptr;
if (pair.x2()->theNode()->degree() == 0 ) {
e_new = m_Gamma.addEdgeToIsolatedNode(pair.x1(), pair.x2()->theNode());
}
else {
adjEntry adjTgt = getAdjEntry(m_Gamma, pair.x2()->theNode(), m_Gamma.rightFace(pair.x1()));
if(!m_Gamma.getGraph().searchEdge(pair.x1()->theNode(),adjTgt->theNode())) // post-hoi-ming bugfix: prohibit the same edge twice...
e_new = m_Gamma.splitFace(pair.x1(), adjTgt);
}
if(e_new!=nullptr)
m_isSinkArc[e_new] = true;
}
isAugmented = true;
OGDF_ASSERT(isSimple(*this));
computeSinkSwitches();
}
