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_);
}
double MarkovModel::pdf(const MarkovData &dat,
bool logscale) const{
double ans;
if(!!dat.prev()){
MD * prev = dat.prev();
ans = Q(prev->value(), dat.value());
} else ans = pi0(dat.value());
return logscale? safelog(ans) : ans; }
double MarkovModel::pdf(const Ptr<DataPointType> &dp, bool logscale) const {
double ans = 0;
if (!!dp->prev()) {
ans = Q(dp->prev()->value(), dp->value());
} else
ans = pi0(dp->value());
return logscale ? safelog(ans) : ans;
}
void Foam::sixDoFRigidBodyMotion::updateAcceleration
(
bool firstIter,
const vector& fGlobal,
const vector& tauGlobal,
scalar deltaT,
scalar deltaT0
)
{
static bool first = false;
if (Pstream::master())
{
// Save the previous iteration accelerations for relaxation
vector aPrevIter = a();
vector tauPrevIter = tau();
// Calculate new accelerations
a() = fGlobal/mass_;
tau() = (Q().T() & tauGlobal);
applyRestraints();
// Relax accelerations on all but first iteration
if (!first)
{
a() = aRelax_*a() + (1 - aRelax_)*aPrevIter;
tau() = aRelax_*tau() + (1 - aRelax_)*tauPrevIter;
}
first = false;
if (firstIter)
{
// Second simplectic step:
// Complete update of linear and angular velocities
v() += tConstraints_ & aDamp_*0.5*deltaT*a();
pi() += rConstraints_ & aDamp_*0.5*deltaT*tau();
}
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()));
}
if (report_)
{
status();
}
}
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();
}
double MarkovModel::loglike()const{
const Vec &icount(suf()->init());
const Mat &tcount(suf()->trans());
Vec logpi0(log(pi0()));
Mat logQ(log(Q()));
double ans= icount.dot(logpi0);
ans+= el_mult_sum(tcount, logQ);
return ans;
}
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_);
}
double MarkovModel::loglike(const Vector &serialized_params)const{
const Vec &icount(suf()->init());
const Mat &tcount(suf()->trans());
int S = state_space_size();
TransitionProbabilityMatrix transition_probabilities(S);
Vec logpi0(log(pi0()));
Mat logQ(log(Q()));
double ans= icount.dot(logpi0);
ans+= el_mult_sum(tcount, logQ);
return ans;
}
void ofDrawRectOutline(const ofRectangle & rect, int thickness, ofDrawRectOutlineMode mode) {
float b0, b1;
if(mode == ofDrawRectOutlineModeOuter) {
b0 = thickness;
b1 = 0;
} else if(mode == ofDrawRectOutlineModeInner) {
b0 = 0;
b1 = thickness;
} else if(mode == ofDrawRectOutlineModeMiddle) {
b0 = thickness * 0.5;
b1 = thickness * 0.5;
}
ofVec2f po0(rect.x - b0, rect.y - b0);
ofVec2f po1(rect.x + rect.width + b0, rect.y - b0);
ofVec2f po2(rect.x + rect.width + b0, rect.y + rect.height + b0);
ofVec2f po3(rect.x - b0, rect.y + rect.height + b0);
ofVec2f pi0(rect.x + b1, rect.y + b1);
ofVec2f pi1(rect.x + rect.width - b1, rect.y + b1);
ofVec2f pi2(rect.x + rect.width - b1, rect.y + rect.height - b1);
ofVec2f pi3(rect.x + b1, rect.y + rect.height - b1);
ofBeginShape();
ofVertex(po0.x, po0.y);
ofVertex(po1.x, po1.y);
ofVertex(po2.x, po2.y);
ofVertex(po3.x, po3.y);
ofNextContour();
ofVertex(pi0.x, pi0.y);
ofVertex(pi1.x, pi1.y);
ofVertex(pi2.x, pi2.y);
ofVertex(pi3.x, pi3.y);
ofEndShape(true);
}
double MarkovModel::pi0(int i)const{ return pi0()(i);}
void DrrnPsiClient::recv(Channel s0, Channel s1, span<block> inputs)
{
if (inputs.size() != mClientSetSize)
throw std::runtime_error(LOCATION);
Matrix<u64> bins(mNumSimpleBins, mBinSize);
std::vector<u64> binSizes(mNumSimpleBins);
u64 cuckooSlotsPerBin = (mCuckooParams.numBins() + mNumSimpleBins) / mNumSimpleBins;
// Simple hashing with a PRP
std::vector<block> hashs(inputs.size());
AES hasher(mHashingSeed);
u64 numCuckooBins = mCuckooParams.numBins();
for (u64 i = 0; i < u64(inputs.size());)
{
auto min = std::min<u64>(inputs.size() - i, 8);
auto end = i + min;
hasher.ecbEncBlocks(inputs.data() + i, min, hashs.data() + i);
for (; i < end; ++i)
{
hashs[i] = hashs[i] ^ inputs[i];
for (u64 j = 0; j < mCuckooParams.mNumHashes; ++j)
{
u64 idx = CuckooIndex<>::getHash(hashs[i], j, numCuckooBins) * mNumSimpleBins / mCuckooParams.numBins();
// insert this item in this bin. pack together the hash index and input index
bins(idx, binSizes[idx]++) = (j << 56) | i;
}
//if (!i)
//{
// ostreamLock(std::cout) << "cinput[" << i << "] = " << inputs[i] << " -> " << hashs[i] << " ("
// << CuckooIndex<>::getHash(hashs[i], 0, numCuckooBins) << ", "
// << CuckooIndex<>::getHash(hashs[i], 1, numCuckooBins) << ", "
// << CuckooIndex<>::getHash(hashs[i], 2, numCuckooBins) << ")"
// << std::endl;
//}
}
}
// power of 2
u64 numLeafBlocks = (cuckooSlotsPerBin + mBigBlockSize * 128 - 1) / (mBigBlockSize * 128);
u64 gDepth = 2;
u64 kDepth = std::max<u64>(gDepth, log2floor(numLeafBlocks)) - gDepth;
u64 groupSize = (numLeafBlocks + (u64(1) << kDepth) - 1) / (u64(1) << kDepth);
if (groupSize > 8) throw std::runtime_error(LOCATION);
//std::cout << "kDepth: " << kDepth << std::endl;
//std::cout << "mBinSize: " << mBinSize << std::endl;
u64 numQueries = mNumSimpleBins * mBinSize;
auto permSize = numQueries * mBigBlockSize;
// mask generation
block rSeed = CCBlock;// mPrng.get<block>();
AES rGen(rSeed);
std::vector<block> shares(mClientSetSize * mCuckooParams.mNumHashes), r(permSize), piS1(permSize), s(permSize);
//std::vector<u32> rIdxs(numQueries);
//std::vector<u64> sharesIdx(shares.size());
//TODO("use real masks");
//memset(r.data(), 0, r.size() * sizeof(block));
rGen.ecbEncCounterMode(r.size() * 0, r.size(), r.data());
rGen.ecbEncCounterMode(r.size() * 1, r.size(), piS1.data());
rGen.ecbEncCounterMode(r.size() * 2, r.size(), s.data());
//auto encIter = enc.begin();
auto shareIter = shares.begin();
//auto shareIdxIter = sharesIdx.begin();
u64 queryIdx = 0, dummyPermIdx = mClientSetSize * mCuckooParams.mNumHashes;
std::unordered_map<u64, u64> inputMap;
inputMap.reserve(mClientSetSize * mCuckooParams.mNumHashes);
std::vector<u32> pi(permSize);
auto piIter = pi.begin();
u64 keySize = kDepth + 1 + groupSize;
u64 mask = (u64(1) << 56) - 1;
auto binIter = bins.begin();
for (u64 bIdx = 0; bIdx < mNumSimpleBins; ++bIdx)
{
u64 i = 0;
auto binOffset = (bIdx * numCuckooBins + mNumSimpleBins - 1) / mNumSimpleBins;
std::vector<block> k0(keySize * mBinSize), k1(keySize * mBinSize);
//std::vector<u64> idx0(mBinSize), idx1(mBinSize);
auto k0Iter = k0.data(), k1Iter = k1.data();
//auto idx0Iter = idx0.data(), idx1Iter = idx1.data();
for (; i < binSizes[bIdx]; ++i)
{
span<block>
kk0(k0Iter, kDepth + 1),
g0(k0Iter + kDepth + 1, groupSize),
kk1(k1Iter, kDepth + 1),
g1(k1Iter + kDepth + 1, groupSize);
k0Iter += keySize;
k1Iter += keySize;
u8 hashIdx = *binIter >> 56;
u64 itemIdx = *binIter & mask;
u64 cuckooIdx = CuckooIndex<>::getHash(hashs[itemIdx], hashIdx, numCuckooBins) - binOffset;
++binIter;
auto bigBlockoffset = cuckooIdx % mBigBlockSize;
auto bigBlockIdx = cuckooIdx / mBigBlockSize;
BgiPirClient::keyGen(bigBlockIdx, mPrng.get<block>(), kk0, g0, kk1, g1);
// the index of the mask that will mask this item
auto rIdx = *piIter = itemIdx * mCuckooParams.mNumHashes + hashIdx * mBigBlockSize + bigBlockoffset;
// the masked value that will be inputted into the PSI
*shareIter = r[rIdx] ^ inputs[itemIdx];
//*shareIter = inputs[itemIdx];
//if (itemIdx == 0)
// ostreamLock(std::cout)
// << "item[" << i << "] bin " << bIdx
// << " block " << bigBlockIdx
// << " offset " << bigBlockoffset
// << " psi " << *shareIter << std::endl;
// This will be used to map itemed items in the intersection back to their input item
inputMap.insert({ queryIdx, itemIdx });
++shareIter;
++piIter;
++queryIdx;
}
u64 rem = mBinSize - i;
binIter += rem;
for (u64 i = 0; i < rem; ++i)
{
*piIter++ = dummyPermIdx++;
}
//s0.asyncSendCopy(k0);
//s0.asyncSendCopy(k1);
//s1.asyncSendCopy(k1);
//s1.asyncSendCopy(k0);
s0.asyncSend(std::move(k0));
s1.asyncSend(std::move(k1));
}
std::vector<u32> pi1(permSize), pi0(permSize), pi1Inv(permSize);
for (u32 i = 0; i < pi1.size(); ++i) pi1[i] = i;
PRNG prng(rSeed ^ OneBlock);
std::random_shuffle(pi1.begin(), pi1.end(), prng);
//std::vector<block> pi1RS(pi.size());
for (u64 i = 0; i < permSize; ++i)
{
//auto pi1i = pi1[i];
//pi1RS[i] = r[pi1i] ^ s[pi1i];
pi1Inv[pi1[i]] = i;
//std::cout << "pi1(r + s)[" << i << "] " << pi1RS[i] << std::endl;
}
std::vector<block> piS0(r.size());
for (u64 i = 0; i < permSize; ++i)
{
//std::cout << "r[" << i << "] " << r[i] << std::endl;
//std::cout << "pi(r + s)[" << i << "]=" << (r[pi[i]] ^ s[pi[i]]) << std::endl;
pi0[i] = pi1Inv[pi[i]];
piS0[i] = piS1[i] ^ s[pi[i]];
//std::cout << "pi (r + s)[" << i << "] = " << (r[pi[i]] ^ s[pi[i]]) << " = " << r[pi[i]] << " ^ " << s[pi[i]] << " c " << pi[i] << std::endl;
//std::cout << "pi`(r + s)[" << i << "] = " << pi1RS[pi0[i]] <<" c " << pi0[pi1[i]] << std::endl;
}
s0.asyncSend(std::move(pi0));
s0.asyncSend(std::move(piS0));
//rGen.ecbEncBlocks(r.data(), r.size(), r.data());
//for (u64 i = 0; i < shares.size(); ++i)
//{
// std::cout << IoStream::lock << "cshares[" << i << "] " << shares[i] << " input[" << sharesIdx[i]<<"]" << std::endl << IoStream::unlock;
//}
mPsi.sendInput(shares, s0);
mIntersection.reserve(mPsi.mIntersection.size());
for (u64 i = 0; i < mPsi.mIntersection.size(); ++i) {
// divide index by #hashes
mIntersection.emplace(inputMap[mPsi.mIntersection[i]]);
}
}
