Eigen::VectorXd Joint::getDampingForces() const
{
int numDofs = getNumGenCoords();
Eigen::VectorXd dampingForce(numDofs);
for (int i = 0; i < numDofs; ++i)
dampingForce(i) = -mDampingCoefficient[i] * getGenCoord(i)->get_dq();
return dampingForce;
}
// calculate the bend force between point p at (i, j, k) and its following neighbour point b
void bendForce(world *jello, int i, int j, int k, point& a)
{
point f;
if (i > 1) // point p at (i, j, k) has its following neighbour (i-2, j, k), calculate the hook force and damping force
{
hookForceBend(jello->p[i][j][k], jello->p[i - 2][j][k], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i - 2][j][k], jello->v[i][j][k], jello->v[i - 2][j][k], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (i < 6) // point p at (i, j, k) has its following neighbour (i+2, j, k), calculate the hook force and damping force
{
hookForceBend(jello->p[i][j][k], jello->p[i + 2][j][k], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i + 2][j][k], jello->v[i][j][k], jello->v[i + 2][j][k], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (j > 1) // point p at (i, j, k) has its following neighbour (i, j-2, k), calculate the hook force and damping force
{
hookForceBend(jello->p[i][j][k], jello->p[i][j - 2][k], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i][j - 2][k], jello->v[i][j][k], jello->v[i][j - 2][k], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (j < 6) // point p at (i, j, k) has its following neighbour (i, j+2, k), calculate the hook force and damping force
{
hookForceBend(jello->p[i][j][k], jello->p[i][j + 2][k], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i][j + 2][k], jello->v[i][j][k], jello->v[i][j + 2][k], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (k > 1) // point p at (i, j, k) has its following neighbour (i, j, k-2), calculate the hook force and damping force
{
hookForceBend(jello->p[i][j][k], jello->p[i][j][k - 2], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i][j][k - 2], jello->v[i][j][k], jello->v[i][j][k - 2], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (k < 6) // point p at (i, j, k) has its following neighbour (i, j, k+2), calculate the hook force and damping force
{
hookForceBend(jello->p[i][j][k], jello->p[i][j][k + 2], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i][j][k + 2], jello->v[i][j][k], jello->v[i][j][k + 2], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
}
// calculate the structual force between point p at (i, j, k) and its neighbour point b
void structualForce(world *jello, int i, int j, int k, point& a)
{
point f;
if (i > 0) // point p at (i, j, k) has its left neighbour (i-1, j, k), calculate the hook force and damping force from left
{
hookForce(jello->p[i][j][k], jello->p[i - 1][j][k], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i - 1][j][k], jello->v[i][j][k], jello->v[i - 1][j][k], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (i < 7) // point p at (i, j, k) has its right neighbour (i+1, j, k), calculate the hook force and damping force from right
{
hookForce(jello->p[i][j][k], jello->p[i + 1][j][k], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i + 1][j][k], jello->v[i][j][k], jello->v[i + 1][j][k], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (j > 0) // point p at (i, j, k) has its upper neighbour (i, j-1, k), calculate the hook force and damping force from upper
{
hookForce(jello->p[i][j][k], jello->p[i][j - 1][k], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i][j - 1][k], jello->v[i][j][k], jello->v[i][j - 1][k], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (j < 7) // point p at (i, j, k) has its bottom neighbour (i, j+1, k), calculate the hook force and damping force from bottom
{
hookForce(jello->p[i][j][k], jello->p[i][j + 1][k], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i][j + 1][k], jello->v[i][j][k], jello->v[i][j + 1][k], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (k > 0) // point p at (i, j, k) has its front neighbour (i, j, k-1), calculate the hook force and damping force from front
{
hookForce(jello->p[i][j][k], jello->p[i][j][k-1], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i][j][k-1], jello->v[i][j][k], jello->v[i][j][k-1], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (k < 7) // point p at (i, j, k) has its behind neighbour (i, j, k+1), calculate the hook force and damping force from behind
{
hookForce(jello->p[i][j][k], jello->p[i][j][k+1], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i][j][k+1], jello->v[i][j][k], jello->v[i - 1][j][k], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
}
// calculate the shear force between point p at (i, j, k) and its diagonal neighbour point b
void shearForce(world *jello, int i, int j, int k, point& a)
{
point f;
if (i > 0)
{
// on 2D surface
if (k > 0) // point p at (i, j, k) has its neighbour (i-1, j, k-1), calculate the hook force and damping force
{
hookForceDiagonal2D(jello->p[i][j][k], jello->p[i - 1][j][k - 1], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i - 1][j][k - 1], jello->v[i][j][k], jello->v[i - 1][j][k - 1], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (k < 7) // point p at (i, j, k) has its neighbour (i-1, j, k+1), calculate the hook force and damping force
{
hookForceDiagonal2D(jello->p[i][j][k], jello->p[i - 1][j][k + 1], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i - 1][j][k + 1], jello->v[i][j][k], jello->v[i - 1][j][k + 1], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (j > 0) // point p at (i, j, k) has its neighbour (i-1, j-1, k), calculate the hook force and damping force
{
hookForceDiagonal2D(jello->p[i][j][k], jello->p[i - 1][j - 1][k], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i - 1][j - 1][k], jello->v[i][j][k], jello->v[i - 1][j - 1][k], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (j < 7) // point p at (i, j, k) has its neighbour (i-1, j+1, k), calculate the hook force and damping force
{
hookForceDiagonal2D(jello->p[i][j][k], jello->p[i - 1][j + 1][k], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i - 1][j + 1][k], jello->v[i][j][k], jello->v[i - 1][j + 1][k], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
// diagonals in 3D cube
if (j > 0 && k > 0) // point p at (i, j, k) has its neighbour (i-1, j-1, k-1), calculate the hook force and damping force
{
hookForceDiagonal3D(jello->p[i][j][k], jello->p[i - 1][j - 1][k - 1], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i - 1][j - 1][k - 1], jello->v[i][j][k], jello->v[i - 1][j - 1][k - 1], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (j > 0 && k < 7) // point p at (i, j, k) has its neighbour (i-1, j-1, k+1), calculate the hook force and damping force
{
hookForceDiagonal3D(jello->p[i][j][k], jello->p[i - 1][j - 1][k + 1], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i - 1][j - 1][k + 1], jello->v[i][j][k], jello->v[i - 1][j - 1][k + 1], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (j < 7 && k > 0) // point p at (i, j, k) has its neighbour (i-1, j+1, k-1), calculate the hook force and damping force
{
hookForceDiagonal3D(jello->p[i][j][k], jello->p[i - 1][j + 1][k - 1], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i - 1][j + 1][k - 1], jello->v[i][j][k], jello->v[i - 1][j + 1][k - 1], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (j < 7 && k < 7) // point p at (i, j, k) has its neighbour (i-1, j+1, k+1), calculate the hook force and damping force
{
hookForceDiagonal3D(jello->p[i][j][k], jello->p[i - 1][j + 1][k + 1], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i - 1][j + 1][k + 1], jello->v[i][j][k], jello->v[i - 1][j + 1][k + 1], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
}
if (i < 7)
{
// on 2D surface
if (k > 0) // point p at (i, j, k) has its neighbour (i+1, j, k-1), calculate the hook force and damping force
{
hookForceDiagonal2D(jello->p[i][j][k], jello->p[i + 1][j][k - 1], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i + 1][j][k - 1], jello->v[i][j][k], jello->v[i + 1][j][k - 1], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (k < 7) // point p at (i, j, k) has its neighbour (i+1, j, k+1), calculate the hook force and damping force
{
hookForceDiagonal2D(jello->p[i][j][k], jello->p[i + 1][j][k + 1], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i + 1][j][k + 1], jello->v[i][j][k], jello->v[i + 1][j][k + 1], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (j > 0) // point p at (i, j, k) has its neighbour (i+1, j-1, k), calculate the hook force and damping force
{
hookForceDiagonal2D(jello->p[i][j][k], jello->p[i + 1][j - 1][k], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i + 1][j - 1][k], jello->v[i][j][k], jello->v[i + 1][j - 1][k], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (j < 7) // point p at (i, j, k) has its neighbour (i+1, j+1, k), calculate the hook force and damping force
{
hookForceDiagonal2D(jello->p[i][j][k], jello->p[i + 1][j + 1][k], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i + 1][j + 1][k], jello->v[i][j][k], jello->v[i + 1][j + 1][k], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
// diagonals in 3D cube
if (j > 0 && k > 0) // point p at (i, j, k) has its neighbour (i+1, j-1, k-1), calculate the hook force and damping force
{
hookForceDiagonal3D(jello->p[i][j][k], jello->p[i + 1][j - 1][k - 1], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i + 1][j - 1][k - 1], jello->v[i][j][k], jello->v[i + 1][j - 1][k - 1], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (j > 0 && k < 7) // point p at (i, j, k) has its neighbour (i+1, j-1, k+1), calculate the hook force and damping force
{
hookForceDiagonal3D(jello->p[i][j][k], jello->p[i + 1][j - 1][k + 1], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i + 1][j - 1][k + 1], jello->v[i][j][k], jello->v[i + 1][j - 1][k + 1], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (j < 7 && k > 0) // point p at (i, j, k) has its neighbour (i+1, j+1, k-1), calculate the hook force and damping force
{
hookForceDiagonal3D(jello->p[i][j][k], jello->p[i + 1][j + 1][k - 1], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i + 1][j + 1][k - 1], jello->v[i][j][k], jello->v[i + 1][j + 1][k - 1], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (j < 7 && k < 7) // point p at (i, j, k) has its neighbour (i+1, j+1, k+1), calculate the hook force and damping force
{
hookForceDiagonal3D(jello->p[i][j][k], jello->p[i + 1][j + 1][k + 1], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i + 1][j + 1][k + 1], jello->v[i][j][k], jello->v[i + 1][j + 1][k + 1], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
}
if (j > 0)
{
// on 2D surface
if (k > 0) // point p at (i, j, k) has its neighbour (i, j-1, k-1), calculate the hook force and damping force
{
hookForceDiagonal2D(jello->p[i][j][k], jello->p[i][j - 1][k - 1], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i][j - 1][k - 1], jello->v[i][j][k], jello->v[i][j - 1][k - 1], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (k < 7) // point p at (i, j, k) has its neighbour (i, j-1, k+1), calculate the hook force and damping force
{
hookForceDiagonal2D(jello->p[i][j][k], jello->p[i][j - 1][k + 1], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i][j - 1][k + 1], jello->v[i][j][k], jello->v[i][j - 1][k + 1], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
}
if (j < 7)
{
// on 2D surface
if (k > 0) // point p at (i, j, k) has its neighbour (i, j+1, k-1), calculate the hook force and damping force
{
hookForceDiagonal2D(jello->p[i][j][k], jello->p[i][j + 1][k - 1], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i][j + 1][k - 1], jello->v[i][j][k], jello->v[i][j + 1][k - 1], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
if (k < 7) // point p at (i, j, k) has its neighbour (i, j+1, k+1), calculate the hook force and damping force
{
hookForceDiagonal2D(jello->p[i][j][k], jello->p[i][j + 1][k + 1], jello->kElastic, f);
// accumulate the hook force at point p
pSUM(f, a, a);
dampingForce(jello->p[i][j][k], jello->p[i][j + 1][k + 1], jello->v[i][j][k], jello->v[i][j + 1][k + 1], jello->dElastic, f);
// accumulate the damping force at point p
pSUM(f, a, a);
}
}
}
