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