void ParticleSystem::derivative( LargeVector<Matx33d> &K, LargeVector<Vec3d> &V,
LargeVector<Vec3d> &F, LargeVector<Matx33d> &M )
{
clearForces(); // zero the force accumulators
computeForces(K); // magic force function
computeConstraintForces(); // constrain force function
V.resize(pts.size());
F.resize(pts.size());
M.resize(pts.size());
for (size_t i = 0; i < pts.size(); i++)
{
if (pts[i].isPinned)
{
V[i] = V3dZero;
F[i] = V3dZero;
}
else
{
V[i] = pts[i].v;
F[i] = pts[i].f;
}
M[i] = pts[i].m * I33d;
}
}
LargeVector EpsilonGreedySampler::calculateBaseDistribution(const LargeVector &epsilon) const
{
// Calculate base distribution
LargeVector distribution = LargeVector::Constant(epsilon.size(), 0.);
std::vector<double> e;
fromVector(epsilon, e);
e.push_back(0.); // So we can start at ii=1.
std::sort(e.begin(), e.end());
for (size_t ii=1; ii < e.size(); ++ii)
{
if (e[ii] != e[ii-1])
{
// Divvy up difference in probability among eligible actions.
size_t eligible = 0;
for (size_t jj=0; jj < epsilon.size(); ++jj)
if (e[ii] <= epsilon[jj])
eligible++;
for (size_t jj=0; jj < epsilon.size(); ++jj)
if (e[ii] <= epsilon[jj])
distribution[jj] += (e[ii]-e[ii-1])/eligible;
}
}
TRACE("Base distribution is " << distribution);
return distribution;
}
void SparseGridMtxLU :: SolveLV(const LargeVector &b, LargeVector &x)
{
if ( & x != & b ) {
x.Initialize(b, 0, n);
}
SolveLU( x.DataPtr() );
}
void ParticleSystem::getState( LargeVector<Vec3d> &P, LargeVector<Vec3d> &V )
{
P.resize(pts.size());
V.resize(pts.size());
for (size_t i = 0; i < pts.size(); i++)
{
P[i] = pts[i].cp;
V[i] = pts[i].v;
}
}
void ParticleSystem::setState( LargeVector<Vec3d> P, LargeVector<Vec3d> V, bool updateOld )
{
assert(P.size() == pts.size() && V.size() == pts.size());
for (size_t i = 0; i < pts.size(); i++)
{
if (updateOld)
{
pts[i].op = pts[i].cp;
}
pts[i].cp = P[i];
pts[i].v = V[i];
}
}
void EpsilonGreedySampler::distribution(const LargeVector &values, LargeVector *distribution) const
{
if (epsilon_.size() > 1)
{
*distribution = distribution_;
(*distribution)[GreedySampler::sample(values)] += 1 - distribution_sum_;
}
else
{
GreedySampler::distribution(values, distribution);
for (size_t ii=0; ii < values.size(); ++ii)
{
if ((*distribution)[ii] == 1)
(*distribution)[ii] = 1-epsilon_[0];
(*distribution)[ii] += epsilon_[0]/values.size();
}
}
}
void ParticleSystem::derivative( LargeVector<Vec3d> &V, LargeVector<Vec3d> &A )
{
clearForces();
computeForces();
computeConstraintForces(); // constrain force function
V.resize(pts.size());
A.resize(pts.size());
for (size_t i = 0; i < pts.size(); i++)
{
if (pts[i].isPinned)
{
V[i] = V3dZero;
A[i] = V3dZero;
}
else
{
V[i] = pts[i].v;
A[i] = pts[i].f / pts[i].m;
}
}
}
size_t EpsilonGreedySampler::sample(const LargeVector &values, ActionType *at) const
{
double r = rand_->get();
if (epsilon_.size() > 1)
{
if (epsilon_.size() != values.size())
throw bad_param("sampler/epsilon_greedy:epsilon");
// Find number of eligible actions
size_t eligible=0;
for (size_t ii=0; ii < epsilon_.size(); ++ii)
if (r < epsilon_[ii])
eligible++;
if (eligible > 0)
{
// Chose one randomly
size_t ri = rand_->getInteger(eligible);
for (size_t ii=0; ii < epsilon_.size(); ++ii)
if (r < epsilon_[ii])
if (!ri--)
{
if (at)
*at = atExploratory;
return ii;
}
}
}
else if (r < epsilon_[0])
{
if (at)
*at = atExploratory;
return rand_->getInteger(values.size());
}
return GreedySampler::sample(values, at);
}
void ParticleSystem::computeForces( LargeVector<Matx33d> &K )
{
K.resize(pts.size());
K.clear();
for (size_t i = 0; i < pts.size(); i++)
{
pts[i].f = pts[i].m * g + pts[i].fu + airDamping * pts[i].v;
}
for (size_t i = 0; i < springs.size(); i++)
{
int p1 = springs[i].p1;
int p2 = springs[i].p2;
double l0 = springs[i].l0;
double ks = springs[i].ks;
double kd = springs[i].kd;
Vec3d dx = pts[p1].cp - pts[p2].cp;
Vec3d dv = pts[p1].v - pts[p2].v;
double d2 = dx.ddot(dx);
double d = sqrt(d2);
Vec3d dxdir = dx / d;
Matx31d tmpDx = Matx31d(dx);
Matx33d tmpK = ks * (-I33d + l0 / d *
(I33d - 1 / d2 * tmpDx * tmpDx.t()));
K[p1] += tmpK;
K[p2] += tmpK;
Vec3d f = -ks * (d - l0) * dxdir
+ kd * dv.ddot(dxdir) * dxdir;
pts[p1].f += f;
pts[p2].f -= f;
}
}
size_t GreedySampler::sample(const LargeVector &values, ActionType *at) const
{
size_t mai = 0;
for (size_t ii=1; ii < values.size(); ++ii)
if (values[ii] > values[mai])
mai = ii;
if (rand_max_)
{
LargeVector same_values = ConstantVector(values.size(), 0);
size_t jj = 0;
for (size_t ii=0; ii < values.size(); ++ii)
if (values[ii] == values[mai])
same_values[jj++] = ii;
if (jj != 0)
mai = same_values[rand_->getInteger(jj)];
}
if (at)
*at = atGreedy;
return mai;
}
void GreedySampler::distribution(const LargeVector &values, LargeVector *distribution) const
{
*distribution = LargeVector::Constant(values.size(), 0.);
(*distribution)[GreedySampler::sample(values)] = 1;
}
