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;
}
}
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::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;
}
}
}
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;
}
}
