/* Function: PenaltyPushBack
* Description: Responds to the collision that is detected and performs penalty method for model spheres
* Input: cur - current model structure
* p2 - center of the other model
* r1 - radius of the other model
* Output: void
*/
void PenaltyPushBack(pModel *cur, pModel *next)
{
point distCI, distIC, inter, velDir, cVel;
double length;
pDIFFERENCE(cur->cModel, next->cModel, inter);
pNORMALIZE(inter);
pCPY(next->pObj->avgVel, velDir);
pNORMALIZE(velDir);
pCPY(cur->pObj->avgVel, cVel);
pNORMALIZE(cVel);
pMULTIPLY(inter, next->radius, inter);
for (unsigned int index = STARTFROM; index <= cur->pObj->model->numvertices; index++)
{
point cP = vMake(cur->pObj->model->vertices[3*index], cur->pObj->model->vertices[3*index + 1], cur->pObj->model->vertices[3*index + 2]);
length = vecLeng(cP, inter);
point pForce = penaltyForce(cP, cur->pObj->velocity[index], inter, velDir, cur->pObj->kSphere, cur->pObj->dSphere);
// Add the forces to the collided vertex
pMULTIPLY(pForce, (1.0 / (length * length)) * cur->pObj->mass[index], pForce);
pSUM(cur->pObj->extForce[index] , pForce, cur->pObj->extForce[index]);
point nP = vMake(next->pObj->model->vertices[3*index], next->pObj->model->vertices[3*index + 1], next->pObj->model->vertices[3*index + 2]);
length = vecLeng(nP, inter);
point nForce = penaltyForce(nP, next->pObj->velocity[index], inter, cVel, next->pObj->kSphere, next->pObj->dSphere);
// Add the forces to the collided vertex
pMULTIPLY(nForce, -(1.0 / (length * length)) * next->pObj->mass[index], nForce);
pSUM(next->pObj->extForce[index], nForce, next->pObj->extForce[index]);
}
}
// calculate the collision response force by the bounding box walls
void collisionForce(world *jello, int i, int j, int k, point& a)
{
// collision detection, check if point (i, j, k) is outside the bounding box
if ((jello->p[i][j][k].x <= -2) || (jello->p[i][j][k].x >= 2) ||
(jello->p[i][j][k].y <= -2) || (jello->p[i][j][k].y >= 2) ||
(jello->p[i][j][k].z <= -2) || (jello->p[i][j][k].z >= 2))
{
point f;
// initialize the normal vector of the wall
point normal;
normal.x = 0;
normal.y = 0;
normal.z = 0;
// initialize the collision point on the wall
point wall;
wall = jello->p[i][j][k];
// since the wall is static, the velocity is (0, 0, 0)
point vWall;
vWall.x = 0;
vWall.y = 0;
vWall.z = 0;
// using the penalty method, calculate the force direction and the collision point on the wall
if(jello->p[i][j][k].x <= -2)
{
normal.x = 1;
wall.x = -2;
}
if(jello->p[i][j][k].x >= 2)
{
normal.x = -1;
wall.x = 2;
}
if(jello->p[i][j][k].y <= -2)
{
normal.y = 1;
wall.y = -2;
}
if(jello->p[i][j][k].y >= 2)
{
normal.y = -1;
wall.y = 2;
}
if(jello->p[i][j][k].z <= -2)
{
normal.z = 1;
wall.z = -2;
}
if(jello->p[i][j][k].z >= 2)
{
normal.z = -1;
wall.z = 2;
}
hookForceCollision(jello->p[i][j][k], wall, jello->kCollision, normal, f);
pSUM(f, a, a);
dampingForceCollision(jello->p[i][j][k], wall, jello->v[i][j][k], vWall, jello->dCollision, normal, f);
pSUM(f, a, a);
}
}
/* Computes acceleration to every control point of the jello cube,
which is in state given by 'jello'.
Returns result in array 'a'. */
void computeAcceleration(struct world * jello, struct point a[8][8][8])
{
bool side;
point planeNormal;
// check the position of the jello cube
inclinedPlaneSide(jello, jello->p[0][0][0], side, planeNormal);
if (side == false)
{
pMULTIPLY(planeNormal, -1, planeNormal);
}
for (int i = 0; i < 8; i++)
{
for (int j = 0; j < 8; j++)
{
for (int k = 0; k < 8; k++)
{
a[i][j][k].x = 0;
a[i][j][k].y = 0;
a[i][j][k].z = 0;
// calculate the internal force of the jello
structualForce(jello, i, j, k, a[i][j][k]);
shearForce(jello, i, j, k, a[i][j][k]);
bendForce(jello, i, j, k, a[i][j][k]);
////internalForce(jello, i, j, k, );
//point internalForce = a[i][j][k];
// calculate the external force in the bounding box
point externF;
externalForce(jello, i, j, k, externF);
pSUM(a[i][j][k], externF, a[i][j][k]);
// calculate the collision force in the bounding box
//point collisionF;
collisionForce(jello, i, j, k, a[i][j][k]);
// check the point position
bool s;
inclinedPlaneSide(jello, jello->p[i][j][k], s, planeNormal);
// calculate the inclined plane response force in the bounding box
//point pCollisionF;
planeCollisionForce(jello, i, j, k, s, planeNormal, a[i][j][k]);
// calculate the acceleration using F = m * a, a = F / m
//point F;
/*pSUM(internF, externF, F);
pSUM(F, collisionF, F);
pSUM(F, pCollisionF, F);*/
pMULTIPLY(a[i][j][k], (1 / jello->mass), a[i][j][k]);
}
}
}
}
/* Function: CalcGoalPos
* Description: Computes the Goal position of the model
* g = R(xi0 - xcm0) + xcm
* Input: None
* Output: None
*/
void CalcGoalPos(phyzx *phyzxObj)
{
point temp;
memset( (void*)&temp, 0, sizeof(temp));
for(unsigned int index = STARTFROM; index <= phyzxObj->model->numvertices; index++)
{
//pDIFFERENCE(phyzxObj->stable[index], phyzxObj->cmStable, temp); // xi0 - xcm0
matMult3331(phyzxObj->R, phyzxObj->relStableLoc[index], &temp); // R(xi0 - xcm0)
pSUM(temp, phyzxObj->cmDeformed, phyzxObj->goal[index]); // g = R(xi0 - xcm0) + xcm
}
}
void planeCollisionForce(world * jello, int i, int j, int k, const bool& s, const point& pn, point &a)
{
if (s == false) // the point collided with the plane
{
double distance;
point normal;
double normalLength;
point normalizedN;
point pointOnPlane;
point f;
point vPlane;
vPlane.x = 0;
vPlane.y = 0;
vPlane.z = 0;
// D = (ax + by + cz + d) / sqrt(a*a + b*b + c*c)
distance = (jello->a * jello->p[i][j][k].x + jello->b * jello->p[i][j][k].y + jello->c * jello->p[i][j][k].z + jello->d);
distance = distance / sqrt(jello->a * jello->a + jello->b * jello->b + jello->c * jello->c);
normalLength = sqrt(pn.x * pn.x + pn.y * pn.y + pn.z * pn.z);
// normalized normal, get the orientation of the normal
normalizedN.x = (pn.x / normalLength);
normalizedN.y = (pn.y / normalLength);
normalizedN.z = (pn.z / normalLength);
pMULTIPLY(normalizedN, distance, normalizedN);
// calculate the point on plane
pDIFFERENCE(jello->p[i][j][k], normalizedN, pointOnPlane);
// calculate the direction
normal.x = (pn.x / normalLength);
normal.y = (pn.y / normalLength);
normal.z = (pn.z / normalLength);
hookForceCollision(jello->p[i][j][k], pointOnPlane, jello->kCollision, normal, f);
pSUM(f, a, a);
dampingForceCollision(jello->p[i][j][k], pointOnPlane, jello->v[i][j][k], vPlane, jello->dCollision, normal, f);
pSUM(f, a, a);
}
}
/* Function: penaltyForce
* Description: Computes the penalty force between two points.
* Input: p - Coordinates of first point
* pV - Velocity of first point
* I - Intersection point
* V - Velocity of Intersection point
* kH - K value for Hook's Law
* kD - K value for damping force
* Output: Penalty force vector
*/
point penaltyForce(point p, point pV, point I, point V, double kH, double kD)
{
double mag, length, dot;
point dist, hForce, dForce, pVel, vDiff, pForce;
// Initialize force computation variables
pDIFFERENCE(p, I, dist);
pDIFFERENCE(pV, V, vDiff);
dot = dotProd(vDiff, dist);
// Compute Hooks Force
pNORMALIZE(dist);
pMULTIPLY(dist, -(kH * length), hForce);
// Compute Damping Forces
mag = length;
pNORMALIZE(pV);
pMULTIPLY(pV, (kD * (dot/length)), dForce);
// Compute Penalty Force
pSUM(hForce, dForce, pForce);
return pForce;
} //end penaltyForce
/* Function: modEuler
* Description: Modified Euler Integrator using Implicit and Explicit
* vi(t + h) = vi(t) + (ALPHA / h) * (gi(t) - xi(t)) + (h / mi) * Fext(t)
* xi(t + h) = xi(t) + h * vi(t + h)
* Input: None
* Output: None
*/
void ModEuler(phyzx *phyzxObj, int mIndex, int deformMode)
{
point vertex, velocity, extVel, position, velDamp;
point vDiff, velTotal, newPos, temp;
matrix R, matTemp;
memset( (void*)&temp, 0, sizeof(temp));
memset((void*)&extVel, 0, sizeof(point));
memset((void*)&velocity, 0, sizeof(point));
memset((void*)&position, 0, sizeof(point));
memset((void*)&vDiff, 0, sizeof(point));
memset((void*)&velTotal, 0, sizeof(point));
memset((void*)&newPos, 0, sizeof(point));
memset((void*)&phyzxObj->avgVel, 0, sizeof(point));
matInit(&R, 0, 0);
matInit(&matTemp, 0, 0);
if (deformMode == 3)
quadDeformRot(&R, phyzxObj);
for (unsigned int index = STARTFROM; index <= phyzxObj->model->numvertices; index++)
{
if (deformMode == 3)
{
// Compute Quadratic Deformation Goal Positions
matMult(R, phyzxObj->q[index], &matTemp); // R(q)
temp = matToPoint(matTemp); // Data type conversion
pSUM(temp, phyzxObj->cmDeformed, phyzxObj->goal[index]); // g = R(q) + xcm
} //end if
else
{
// Compute Goal Positions
matMult3331(phyzxObj->R, phyzxObj->relStableLoc[index], &temp); // R(xi0 - xcm0)
pSUM(temp, phyzxObj->cmDeformed, phyzxObj->goal[index]); // g = R(xi0 - xcm0) + xcm
} //end if
vertex.x = phyzxObj->model->vertices[3*index];
vertex.y = phyzxObj->model->vertices[3*index + 1];
vertex.z = phyzxObj->model->vertices[3*index + 2];\
if (stickyFloor == 1)
if (vertex.y <= -WALLDIST)
continue;
// Add user force
if (mIndex == iMouseModel && lMouseVal == 2 && objectName != -1)// && index == objectName)
{
//point uForce;
/*GLMnode *node;
node = NBVStruct[objectName];
while (node->next != NULL)
{
pSUM(phyzxObj->extForce[node->index], userForce, phyzxObj->extForce[node->index]);
node = node->next;
} //end while*/
/*if (index != objectName)
{
point extPos = vMake(phyzxObj->model->vertices[3*objectName], phyzxObj->model->vertices[3*objectName+1], phyzxObj->model->vertices[3*objectName+2]);
double dist = vecLeng(extPos, vertex);
//if (dist > 0.04)
//{
pMULTIPLY(userForce, (1.0/dist), uForce);
pSUM(phyzxObj->extForce[index], uForce, phyzxObj->extForce[index]);
//pDisp("user", userForce);
//} //end if
//else
//{
//pSUM(phyzxObj->extForce[index], userForce, phyzxObj->extForce[index]);
//} //end else
} //end if
else
{*/
pSUM(phyzxObj->extForce[index], userForce, phyzxObj->extForce[index]);
//} //end else
} //end if
// Explicit Euler Integrator for veloctiy -> vi(t + h)
pDIFFERENCE(phyzxObj->goal[index], vertex, vDiff); // gi(t) - xi(t)
pMULTIPLY(vDiff, (phyzxObj->alpha / phyzxObj->h), velocity); // vi(h) = (ALPHA / h) * (gi(t) - xi(t))
pMULTIPLY(phyzxObj->extForce[index], (phyzxObj->h / phyzxObj->mass[index]), extVel); // (h / mi) * Fext(t)
// pMULTIPLY(phyzxObj->extForce[index], phyzxObj->h, extVel); // (h / mi) * Fext(t)
pSUM(velocity, extVel, velTotal); // vi(h) = (ALPHA / h) * (gi(t) - xi(t)) + (h / mi) * Fext(t)
pSUM(phyzxObj->velocity[index], velTotal, phyzxObj->velocity[index]); // vi(t + h) = vi(t) + vi(h)
// Velocity Damping
pMULTIPLY(phyzxObj->velocity[index], -phyzxObj->delta, velDamp);
pSUM(phyzxObj->velocity[index], velDamp, phyzxObj->velocity[index]);
// Implicity Euler Integrator for position
pMULTIPLY(phyzxObj->velocity[index], phyzxObj->h, position); // xi(h) = h * vi(t + h)
pSUM(vertex, position, newPos); // xi(t + h) = xi(t) + xi(h)
// Store new position into data structure
phyzxObj->model->vertices[3*index] = newPos.x;
phyzxObj->model->vertices[3*index + 1] = newPos.y;
phyzxObj->model->vertices[3*index + 2] = newPos.z;
pSUM(phyzxObj->avgVel, phyzxObj->velocity[index], phyzxObj->avgVel);
//if (objCollide)
CheckForCollision(index, phyzxObj, mIndex);
} //end for
pMULTIPLY(phyzxObj->avgVel, 1.0 / phyzxObj->model->numvertices, phyzxObj->avgVel);
delete[] R.data;
delete[] matTemp.data;
} //end ModEuler()
void RK4(struct world * jello)
{
point F1p[999], F1v[999],
F2p[999], F2v[999],
F3p[999], F3v[999],
F4p[999], F4v[999];
struct world buffer;
int i,j,k;
buffer = *jello; // make a copy of jello
computeAcceleration(jello);
for (i=0; i<particleNum; i++){
pMULTIPLY(jello->v[i],jello->dt,F1p[i]);
pMULTIPLY(jello->a[i],jello->dt,F1v[i]);
pMULTIPLY(F1p[i],0.5,buffer.p[i]);
pMULTIPLY(F1v[i],0.5,buffer.v[i]);
pSUM(jello->p[i],buffer.p[i],buffer.p[i]);
pSUM(jello->v[i],buffer.v[i],buffer.v[i]);
}
computeAcceleration(&buffer);
for (i=0; i<particleNum; i++){
// F2p = dt * buffer.v;
pMULTIPLY(buffer.v[i],jello->dt,F2p[i]);
// F2v = dt * a(buffer.p,buffer.v);
pMULTIPLY(buffer.a[i],jello->dt,F2v[i]);
pMULTIPLY(F2p[i],0.5,buffer.p[i]);
pMULTIPLY(F2v[i],0.5,buffer.v[i]);
pSUM(jello->p[i],buffer.p[i],buffer.p[i]);
pSUM(jello->v[i],buffer.v[i],buffer.v[i]);
}
computeAcceleration(&buffer);
for (i=0; i<particleNum; i++)
{
// F3p = dt * buffer.v;
pMULTIPLY(buffer.v[i],jello->dt,F3p[i]);
// F3v = dt * a(buffer.p,buffer.v);
pMULTIPLY(buffer.a[i],jello->dt,F3v[i]);
pMULTIPLY(F3p[i],0.5,buffer.p[i]);
pMULTIPLY(F3v[i],0.5,buffer.v[i]);
pSUM(jello->p[i],buffer.p[i],buffer.p[i]);
pSUM(jello->v[i],buffer.v[i],buffer.v[i]);
}
computeAcceleration(&buffer);
for (i=0; i<particleNum; i++)
{
// F3p = dt * buffer.v;
pMULTIPLY(buffer.v[i],jello->dt,F4p[i]);
// F3v = dt * a(buffer.p,buffer.v);
pMULTIPLY(jello->a[i],jello->dt,F4v[i]);
pMULTIPLY(F2p[i],2,buffer.p[i]);
pMULTIPLY(F3p[i],2,buffer.v[i]);
pSUM(buffer.p[i],buffer.v[i],buffer.p[i]);
pSUM(buffer.p[i],F1p[i],buffer.p[i]);
pSUM(buffer.p[i],F4p[i],buffer.p[i]);
pMULTIPLY(buffer.p[i],1.0 / 6,buffer.p[i]);
pSUM(buffer.p[i],jello->p[i],jello->p[i]);
pMULTIPLY(F2v[i],2,buffer.p[i]);
pMULTIPLY(F3v[i],2,buffer.v[i]);
pSUM(buffer.p[i],buffer.v[i],buffer.p[i]);
pSUM(buffer.p[i],F1v[i],buffer.p[i]);
pSUM(buffer.p[i],F4v[i],buffer.p[i]);
pMULTIPLY(buffer.p[i],1.0 / 6,buffer.p[i]);
pSUM(buffer.p[i],jello->v[i],jello->v[i]);
}
return;
}
/* as a result, updates the chain structure */
void RK4(struct chain * chain)
{
point F1p[chain->number], F1v[chain->number],
F2p[chain->number], F2v[chain->number],
F3p[chain->number], F3v[chain->number],
F4p[chain->number], F4v[chain->number];
point a[chain->number];
struct chain buffer = *chain;
computeAcceleration(chain, a);
for (int i=1; i<=chain->number; i++)
{
pMULTIPLY(chain->v[i],chain->dt,F1p[i-1]);
pMULTIPLY(a[i-1],chain->dt,F1v[i-1]);
pMULTIPLY(F1p[i-1],0.5,buffer.p[i]);
pMULTIPLY(F1v[i-1],0.5,buffer.v[i]);
pSUM(chain->p[i],buffer.p[i],buffer.p[i]);
pSUM(chain->v[i],buffer.v[i],buffer.v[i]);
}
computeAcceleration(&buffer, a);
for (int i=1; i<=chain->number; i++)
{
// F2p = dt * buffer.v;
pMULTIPLY(buffer.v[i], chain->dt, F2p[i-1]);
// F2v = dt * a(buffer.p,buffer.v);
pMULTIPLY(a[i-1], chain->dt, F2v[i-1]);
pMULTIPLY(F2p[i-1], 0.5, buffer.p[i]);
pMULTIPLY(F2v[i-1], 0.5, buffer.v[i]);
pSUM(chain->p[i], buffer.p[i], buffer.p[i]);
pSUM(chain->v[i], buffer.v[i], buffer.v[i]);
}
computeAcceleration(&buffer, a);
for (int i=1; i<=chain->number; i++)
{
// F3p = dt * buffer.v;
pMULTIPLY(buffer.v[i], chain->dt, F3p[i-1]);
// F3v = dt * a(buffer.p,buffer.v);
pMULTIPLY(a[i-1], chain->dt, F3v[i-1]);
pMULTIPLY(F3p[i-1], 0.5, buffer.p[i]);
pMULTIPLY(F3v[i-1], 0.5, buffer.v[i]);
pSUM(chain->p[i], buffer.p[i], buffer.p[i]);
pSUM(chain->v[i], buffer.v[i], buffer.v[i]);
}
computeAcceleration(&buffer, a);
for (int i=1; i<=chain->number; i++)
{
// F3p = dt * buffer.v;
pMULTIPLY(buffer.v[i], chain->dt, F4p[i-1]);
// F3v = dt * a(buffer.p,buffer.v);
pMULTIPLY(a[i-1], chain->dt, F4v[i-1]);
pMULTIPLY(F2p[i-1], 2, buffer.p[i]);
pMULTIPLY(F3p[i-1], 2, buffer.v[i]);
pSUM(buffer.p[i], buffer.v[i], buffer.p[i]);
pSUM(buffer.p[i], F1p[i-1], buffer.p[i]);
pSUM(buffer.p[i], F4p[i-1], buffer.p[i]);
pMULTIPLY(buffer.p[i], 1.0 / 6, buffer.p[i]);
pSUM(buffer.p[i], chain->p[i], chain->p[i]);
pMULTIPLY(F2v[i-1], 2, buffer.p[i]);
pMULTIPLY(F3v[i-1], 2, buffer.v[i]);
pSUM(buffer.p[i], buffer.v[i], buffer.p[i]);
pSUM(buffer.p[i], F1v[i-1], buffer.p[i]);
pSUM(buffer.p[i], F4v[i-1], buffer.p[i]);
pMULTIPLY(buffer.p[i], 1.0 / 6, buffer.p[i]);
pSUM(buffer.p[i], chain->v[i], chain->v[i]);
}
return;
}
/* Function: cameraFreeMove
* Description: Free movement camera computations. Allow camera to move
* forward, backword, up, down, strafe left, strafe right,
* tilt up/down and turn right/left.
* Input: dir - Direction of movement
* 0. Move Camera Forward
* 1. Move Camera Backward
* 2. Move Camera Up
* 3. Move Camera Down
* 4. Move Camera Right
* 5. Move Camera Left
* 6. Tilt Camera Up (Not Implemented)
* 7. Tilt Camera Down (Not Implemented)
* 8. Turn Camera Right (Not Implemented)
* 9. Turn Camera Left (Not Implemented)
* Output: None
*/
void cameraFreeMove(int dir)
{
point mouse, vec, pVec, cVec, cPos;
double ang, length;
pDIFFERENCE(lineOfSight, cameraPos, vec);
pNORMALIZE(vec);
if (dir == 0) // Move Camera Forward
{
pMULTIPLY(vec, CAMSPEED, vec);
pSUM(cameraPos, vec, cameraPos);
pSUM(lineOfSight, vec, lineOfSight);
} //end if
else if (dir == 1) // Move Camera Backward
{
pMULTIPLY(vec, CAMSPEED, vec);
pDIFFERENCE(cameraPos, vec, cameraPos);
pDIFFERENCE(lineOfSight, vec, lineOfSight);
} //end else
else if (dir == 2) // Move Camera Up
{
ang = Phi + CAMROT;
cPos.x = lineOfSight.x + (R * cos(ang) * cos(Theta));
cPos.y = lineOfSight.y + (R * sin(Theta));
cPos.z = lineOfSight.z + (-R * sin(ang) * cos(Theta));
pDIFFERENCE(cPos, cameraPos, pVec);
pNORMALIZE(pVec);
CROSSPRODUCTp(pVec, vec, cVec);
pMULTIPLY(cVec, CAMSPEED, cVec);
pSUM(cameraPos, cVec, cameraPos);
pSUM(lineOfSight, cVec, lineOfSight);
} //end else
else if (dir == 3) // Move Camera Down
{
ang = Phi + CAMROT;
cPos.x = lineOfSight.x + (R * cos(ang) * cos(Theta));
cPos.y = lineOfSight.y + (R * sin(Theta));
cPos.z = lineOfSight.z + (-R * sin(ang) * cos(Theta));
pDIFFERENCE(cPos, cameraPos, pVec);
pNORMALIZE(pVec);
CROSSPRODUCTp(pVec, vec, cVec);
pMULTIPLY(cVec, CAMSPEED, cVec);
pDIFFERENCE(cameraPos, cVec, cameraPos);
pDIFFERENCE(lineOfSight, cVec, lineOfSight);
} //end else
else if (dir == 4) // Move Camera Strafe Right
{
ang = Theta + CAMROT;
cPos.x = lineOfSight.x + (R * cos(Phi) * cos(ang));
cPos.y = lineOfSight.y + (R * sin(ang));
cPos.z = lineOfSight.z + (-R * sin(Phi) * cos(ang));
pDIFFERENCE(cPos, cameraPos, pVec);
pNORMALIZE(pVec);
CROSSPRODUCTp(vec, pVec, cVec);
pMULTIPLY(cVec, CAMSPEED, cVec);
pSUM(cameraPos, cVec, cameraPos);
pSUM(lineOfSight, cVec, lineOfSight);
} //end else
else if (dir == 5) // Move Camera Strafe Left
{
ang = Theta + CAMROT;
cPos.x = lineOfSight.x + (R * cos(Phi) * cos(ang));
cPos.y = lineOfSight.y + (R * sin(ang));
cPos.z = lineOfSight.z + (-R * sin(Phi) * cos(ang));
pDIFFERENCE(cPos, cameraPos, pVec);
pNORMALIZE(pVec);
CROSSPRODUCTp(vec, pVec, cVec);
pMULTIPLY(cVec, CAMSPEED, cVec);
pDIFFERENCE(cameraPos, cVec, cameraPos);
pDIFFERENCE(lineOfSight, cVec, lineOfSight);
} //end else
} //end cameraFreeMove
// 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);
}
}
}
/* as a result, updates the jello structure */
void RK4(cloth *clothItem)
{
int index;
point *F1p, *F1v,
*F2p, *F2v,
*F3p, *F3v,
*F4p, *F4v;
F1p = (point*)calloc(clothItem->cArrayLength, sizeof(point));
F1v = (point*)calloc(clothItem->cArrayLength, sizeof(point));
F2p = (point*)calloc(clothItem->cArrayLength, sizeof(point));
F2v = (point*)calloc(clothItem->cArrayLength, sizeof(point));
F3p = (point*)calloc(clothItem->cArrayLength, sizeof(point));
F3v = (point*)calloc(clothItem->cArrayLength, sizeof(point));
F4p = (point*)calloc(clothItem->cArrayLength, sizeof(point));
F4v = (point*)calloc(clothItem->cArrayLength, sizeof(point));
cloth *buffer = (cloth*)malloc(1 * sizeof(cloth));
CopyToBuffer(buffer, clothItem);
computeAcceleration(clothItem);
for(int row = 0; row < clothItem->height; row++)
{
for(int col = 0; col < clothItem->width; col++)
{
index = FindIndexInArray(row, col, clothItem->width);
// Pin Check
if(gPin == PINNED)
{
if(index == 0 || index == clothItem->width-1)
continue;
}
else if(gPin == UNPINLEFT)
{
if(index == clothItem->width-1)
continue;
}
else if(gPin == UNPINRIGHT)
{
if(index == 0)
continue;
}
pMULTIPLY(clothItem->velocities[index], clothItem->tStep, F1p[index]);
pMULTIPLY(clothItem->acceleration[index], clothItem->tStep, F1v[index]);
pMULTIPLY(F1p[index], 0.5, buffer->positions[index]);
pMULTIPLY(F1v[index], 0.5, buffer->velocities[index]);
pSUM(clothItem->positions[index], buffer->positions[index], buffer->positions[index]);
pSUM(clothItem->velocities[index], buffer->velocities[index], buffer->velocities[index]);
}
}
for(int i= 0; i < clothItem->cArrayLength; i++)
{
pCPY(clothItem->acceleration[i], buffer->acceleration[i]);
}
computeAcceleration(buffer);
for(int row = 0; row < clothItem->height; row++)
{
for(int col = 0; col < clothItem->width; col++)
{
index = FindIndexInArray(row, col, clothItem->width);
// Pin Check
if(gPin == PINNED)
{
if(index == 0 || index == clothItem->width-1)
continue;
}
else if(gPin == UNPINLEFT)
{
if(index == clothItem->width-1)
continue;
}
else if(gPin == UNPINRIGHT)
{
if(index == 0)
continue;
}
pMULTIPLY(buffer->velocities[index], clothItem->tStep, F2p[index]);
pMULTIPLY(buffer->acceleration[index], clothItem->tStep, F2v[index]);
pMULTIPLY(F2p[index], 0.5, buffer->positions[index]);
pMULTIPLY(F2v[index], 0.5, buffer->velocities[index]);
pSUM(clothItem->positions[index], buffer->positions[index], buffer->positions[index]);
pSUM(clothItem->velocities[index], buffer->velocities[index], buffer->velocities[index]);
}
}
computeAcceleration(buffer);
for(int row = 0; row < clothItem->height; row++)
{
for(int col = 0; col < clothItem->width; col++)
{
index = FindIndexInArray(row, col, clothItem->width);
// Pin Check
if(gPin == PINNED)
{
if(index == 0 || index == clothItem->width-1)
continue;
}
else if(gPin == UNPINLEFT)
{
if(index == clothItem->width-1)
continue;
}
else if(gPin == UNPINRIGHT)
{
if(index == 0)
continue;
}
pMULTIPLY(buffer->velocities[index], clothItem->tStep, F3p[index]);
pMULTIPLY(buffer->acceleration[index], clothItem->tStep, F3v[index]);
pMULTIPLY(F3p[index], 0.5, buffer->positions[index]);
pMULTIPLY(F3v[index], 0.5, buffer->velocities[index]);
pSUM(clothItem->positions[index], buffer->positions[index], buffer->positions[index]);
pSUM(clothItem->velocities[index], buffer->velocities[index], buffer->velocities[index]);
}
}
computeAcceleration(buffer);
for(int row = 0; row < clothItem->height; row++)
{
for(int col = 0; col < clothItem->width; col++)
{
index = FindIndexInArray(row, col, clothItem->width);
// Pin Check
if(gPin == PINNED)
{
if(index == 0 || index == clothItem->width-1)
continue;
}
else if(gPin == UNPINLEFT)
{
if(index == clothItem->width-1)
continue;
}
else if(gPin == UNPINRIGHT)
{
if(index == 0)
continue;
}
pMULTIPLY(buffer->velocities[index], clothItem->tStep, F4p[index]);
pMULTIPLY(buffer->acceleration[index], clothItem->tStep, F4v[index]);
pMULTIPLY(F2p[index], 2, buffer->positions[index]);
pMULTIPLY(F3p[index], 2, buffer->velocities[index]);
pSUM(buffer->positions[index], buffer->velocities[index], buffer->positions[index]);
pSUM(buffer->positions[index], F1p[index], buffer->positions[index]);
pSUM(buffer->positions[index], F4p[index], buffer->positions[index]);
pMULTIPLY(buffer->positions[index], 1.0 / 6, buffer->positions[index]);
pSUM(buffer->positions[index], clothItem->positions[index], clothItem->positions[index]);
pMULTIPLY(F2v[index], 2, buffer->positions[index]);
pMULTIPLY(F3v[index], 2, buffer->velocities[index]);
pSUM(buffer->positions[index], buffer->velocities[index], buffer->positions[index]);
pSUM(buffer->positions[index], F1v[index], buffer->positions[index]);
pSUM(buffer->positions[index], F4v[index], buffer->positions[index]);
pMULTIPLY(buffer->positions[index], 1.0 / 6, buffer->positions[index]);
pSUM(buffer->positions[index], clothItem->velocities[index], clothItem->velocities[index]);
}
}
for(int i= 0; i < clothItem->cArrayLength; i++)
{
pCPY(buffer->acceleration[i], clothItem->acceleration[i]);
}
for(int i= 0; i < clothItem->cArrayLength; i++)
{
// Pin Check
if(gPin == PINNED)
{
if(index == 0 || index == clothItem->width-1)
continue;
}
else if(gPin == UNPINLEFT)
{
if(index == clothItem->width-1)
continue;
}
else if(gPin == UNPINRIGHT)
{
if(index == 0)
continue;
}
pMULTIPLY(clothItem->velocities[i], gDelta, underWaterDamp);
pSUM(clothItem->velocities[i], underWaterDamp, clothItem->velocities[i]);
pSUM(clothItem->force[i], gravity, clothItem->force[i]);
}
free(buffer);
return;
}
void MoveClothXZ(cloth *clothItem, int direction)
{
for(int i= 0; i < clothItem->cArrayLength; i++)
{
if(gMovePin == MOVELEFTPIN)
{
if(i == 0)
{
if(direction == FORWARD)
{
pSUM(clothItem->positions[i], vMake(0.0, 0.0, -0.01), clothItem->positions[i]);
}
else if(direction == BACKWARD)
{
pSUM(clothItem->positions[i], vMake(0.0, 0.0, 0.01), clothItem->positions[i]);
}
else if(direction == RIGHT)
{
pSUM(clothItem->positions[i], vMake(0.01, 0.0, 0.0), clothItem->positions[i]);
}
else if(direction == LEFT)
{
pSUM(clothItem->positions[i], vMake(-0.01, 0.0, 0.0), clothItem->positions[i]);
}
}
}
if(gMovePin == MOVERIGHTPIN)
{
if(i == clothItem->width -1)
{
if(direction == FORWARD)
{
pSUM(clothItem->positions[i], vMake(0.0, 0.0, -0.01), clothItem->positions[i]);
}
else if(direction == BACKWARD)
{
pSUM(clothItem->positions[i], vMake(0.0, 0.0, 0.01), clothItem->positions[i]);
}
else if(direction == RIGHT)
{
pSUM(clothItem->positions[i], vMake(0.01, 0.0, 0.0), clothItem->positions[i]);
}
else if(direction == LEFT)
{
pSUM(clothItem->positions[i], vMake(-0.01, 0.0, 0.0), clothItem->positions[i]);
}
}
}
if(gMovePin == MOVEBOTTOMLEFTPIN)
{
if(i == (clothItem->width * (clothItem->height-1)))
{
if(direction == FORWARD)
{
pSUM(clothItem->positions[i], vMake(0.0, 0.0, -0.1), clothItem->positions[i]);
}
else if(direction == BACKWARD)
{
pSUM(clothItem->positions[i], vMake(0.0, 0.0, 0.1), clothItem->positions[i]);
}
else if(direction == RIGHT)
{
pSUM(clothItem->positions[i], vMake(0.1, 0.0, 0.0), clothItem->positions[i]);
}
else if(direction == LEFT)
{
pSUM(clothItem->positions[i], vMake(-0.1, 0.0, 0.0), clothItem->positions[i]);
}
}
}
if(gMovePin == MOVEBOTTOMRIGHTPIN)
{
if(i == clothItem->cArrayLength - 1)
{
if(direction == FORWARD)
{
pSUM(clothItem->positions[i], vMake(0.0, 0.0, -0.1), clothItem->positions[i]);
}
else if(direction == BACKWARD)
{
pSUM(clothItem->positions[i], vMake(0.0, 0.0, 0.1), clothItem->positions[i]);
}
else if(direction == RIGHT)
{
pSUM(clothItem->positions[i], vMake(0.1, 0.0, 0.0), clothItem->positions[i]);
}
else if(direction == LEFT)
{
pSUM(clothItem->positions[i], vMake(-0.1, 0.0, 0.0), clothItem->positions[i]);
}
}
}
if(gMovePin == MOVEBOTHPINS)
{
if(i == 0 || i == clothItem->width -1)
{
if(direction == FORWARD)
{
pSUM(clothItem->positions[i], vMake(0.0, 0.0, -0.01), clothItem->positions[i]);
}
else if(direction == BACKWARD)
{
pSUM(clothItem->positions[i], vMake(0.0, 0.0, 0.01), clothItem->positions[i]);
}
else if(direction == RIGHT)
{
pSUM(clothItem->positions[i], vMake(0.01, 0.0, 0.0), clothItem->positions[i]);
}
else if(direction == LEFT)
{
pSUM(clothItem->positions[i], vMake(-0.01, 0.0, 0.0), clothItem->positions[i]);
}
}
}
}
}
// calculate the external force field, add it to point p at (i, j, k)
void externalForce(world *jello, int x, int y, int z, point& a)
{
// the external force index in resolution array
int i, j, k;
// forces at 8 corners in a specific grid
point f000, f001;
point f010, f011;
point f100, f101;
point f110, f111;
// the external force position in grid
double px, py, pz;
// the force field grid
double grid;
// the external force field value
point externalForce;
externalForce.x = 0;
externalForce.y = 0;
externalForce.z = 0;
// the array resolution represents the force field, the external force point position is
// ((-2 + i * 4 / (jello->resolution - 1)), (-2 + j * 4 / (jello->resolution - 1)), (-2 + k * 4 / (jello->resolution - 1))) by i, j, k in the bounding box
// since (-2 + i * 4 / (jello->resolution - 1) <= pt.x <= (-2 + (i + 1) * 4 / (jello->resolution - 1), we have
i = int((jello->p[x][y][z].x + 2) * (jello->resolution - 1) / 4);
j = int((jello->p[x][y][z].y + 2) * (jello->resolution - 1) / 4);
k = int((jello->p[x][y][z].z + 2) * (jello->resolution - 1) / 4);
// check if the index is at the wall of the bounding box
if (i == (jello->resolution - 1))
{
i--;
}
if (j == (jello->resolution - 1))
{
j--;
}
if (k == (jello->resolution - 1))
{
k--;
}
// check if the point is inside the bounding box, read the force field value
if (((i >= 0) && (i <= jello->resolution - 1)) && ((j >= 0) && (j <= jello->resolution - 1)) && ((j >= 0) && (j <= jello->resolution - 1)))
{
f000 = jello->forceField[(i * jello->resolution * jello->resolution + j * jello->resolution + k)];
f001 = jello->forceField[(i * jello->resolution * jello->resolution + j * jello->resolution + (k + 1))];
f010 = jello->forceField[(i * jello->resolution * jello->resolution + (j + 1) * jello->resolution + k)];
f011 = jello->forceField[(i * jello->resolution * jello->resolution + (j + 1) * jello->resolution + (k + 1))];
f100 = jello->forceField[((i + 1) * jello->resolution * jello->resolution + j * jello->resolution + k)];
f101 = jello->forceField[((i + 1) * jello->resolution * jello->resolution + j * jello->resolution + (k + 1))];
f110 = jello->forceField[((i + 1) * jello->resolution * jello->resolution + (j + 1) * jello->resolution + k)];
f111 = jello->forceField[((i + 1) * jello->resolution * jello->resolution + (j + 1) * jello->resolution + (k + 1))];
// 3D interpolation
grid = 1.0 * 4 / (jello->resolution - 1);
px = (jello->p[x][y][z].x - (-2 + 1.0 * 4 * i / (jello->resolution - 1))) / grid;
py = (jello->p[x][y][z].y - (-2 + 1.0 * 4 * j / (jello->resolution - 1))) / grid;
pz = (jello->p[x][y][z].z - (-2 + 1.0 * 4 * k / (jello->resolution - 1))) / grid;
pMULTIPLY(f000, (1 - px) * (1 - py) * (1 - pz), f000);
pMULTIPLY(f001, (1 - px) * (1 - py) * pz, f001);
pMULTIPLY(f010, (1 - px) * py * (1 - pz), f010);
pMULTIPLY(f011, (1 - px) * py * pz, f011);
pMULTIPLY(f100, px * (1 - py) * (1 - pz), f100);
pMULTIPLY(f101, px * (1 - py) * pz, f101);
pMULTIPLY(f110, px * py * (1 - pz), f110);
pMULTIPLY(f111, px * py * pz, f111);
pSUM(externalForce, f000, externalForce);
pSUM(externalForce, f001, externalForce);
pSUM(externalForce, f010, externalForce);
pSUM(externalForce, f011, externalForce);
pSUM(externalForce, f100, externalForce);
pSUM(externalForce, f101, externalForce);
pSUM(externalForce, f110, externalForce);
pSUM(externalForce, f111, externalForce);
a.x = externalForce.x;
a.y = externalForce.y;
a.z = externalForce.z;
}
}
// 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);
}
}
/* as a result, updates the jello structure */
void RK4(struct world * jello)
{
point F1p[8][8][8], F1v[8][8][8],
F2p[8][8][8], F2v[8][8][8],
F3p[8][8][8], F3v[8][8][8],
F4p[8][8][8], F4v[8][8][8];
point a[8][8][8];
struct world buffer;
int i,j,k;
buffer = *jello; // make a copy of jello
computeAcceleration(jello, a);
for (i=0; i<=7; i++)
for (j=0; j<=7; j++)
for (k=0; k<=7; k++)
{
pMULTIPLY(jello->v[i][j][k],jello->dt,F1p[i][j][k]);
pMULTIPLY(a[i][j][k],jello->dt,F1v[i][j][k]);
pMULTIPLY(F1p[i][j][k],0.5,buffer.p[i][j][k]);
pMULTIPLY(F1v[i][j][k],0.5,buffer.v[i][j][k]);
pSUM(jello->p[i][j][k],buffer.p[i][j][k],buffer.p[i][j][k]);
pSUM(jello->v[i][j][k],buffer.v[i][j][k],buffer.v[i][j][k]);
}
computeAcceleration(&buffer, a);
for (i=0; i<=7; i++)
for (j=0; j<=7; j++)
for (k=0; k<=7; k++)
{
// F2p = dt * buffer.v;
pMULTIPLY(buffer.v[i][j][k],jello->dt,F2p[i][j][k]);
// F2v = dt * a(buffer.p,buffer.v);
pMULTIPLY(a[i][j][k],jello->dt,F2v[i][j][k]);
pMULTIPLY(F2p[i][j][k],0.5,buffer.p[i][j][k]);
pMULTIPLY(F2v[i][j][k],0.5,buffer.v[i][j][k]);
pSUM(jello->p[i][j][k],buffer.p[i][j][k],buffer.p[i][j][k]);
pSUM(jello->v[i][j][k],buffer.v[i][j][k],buffer.v[i][j][k]);
}
computeAcceleration(&buffer, a);
for (i=0; i<=7; i++)
for (j=0; j<=7; j++)
for (k=0; k<=7; k++)
{
// F3p = dt * buffer.v;
pMULTIPLY(buffer.v[i][j][k],jello->dt,F3p[i][j][k]);
// F3v = dt * a(buffer.p,buffer.v);
pMULTIPLY(a[i][j][k],jello->dt,F3v[i][j][k]);
pMULTIPLY(F3p[i][j][k],0.5,buffer.p[i][j][k]);
pMULTIPLY(F3v[i][j][k],0.5,buffer.v[i][j][k]);
pSUM(jello->p[i][j][k],buffer.p[i][j][k],buffer.p[i][j][k]);
pSUM(jello->v[i][j][k],buffer.v[i][j][k],buffer.v[i][j][k]);
}
computeAcceleration(&buffer, a);
for (i=0; i<=7; i++)
for (j=0; j<=7; j++)
for (k=0; k<=7; k++)
{
// F3p = dt * buffer.v;
pMULTIPLY(buffer.v[i][j][k],jello->dt,F4p[i][j][k]);
// F3v = dt * a(buffer.p,buffer.v);
pMULTIPLY(a[i][j][k],jello->dt,F4v[i][j][k]);
pMULTIPLY(F2p[i][j][k],2,buffer.p[i][j][k]);
pMULTIPLY(F3p[i][j][k],2,buffer.v[i][j][k]);
pSUM(buffer.p[i][j][k],buffer.v[i][j][k],buffer.p[i][j][k]);
pSUM(buffer.p[i][j][k],F1p[i][j][k],buffer.p[i][j][k]);
pSUM(buffer.p[i][j][k],F4p[i][j][k],buffer.p[i][j][k]);
pMULTIPLY(buffer.p[i][j][k],1.0 / 6,buffer.p[i][j][k]);
pSUM(buffer.p[i][j][k],jello->p[i][j][k],jello->p[i][j][k]);
pMULTIPLY(F2v[i][j][k],2,buffer.p[i][j][k]);
pMULTIPLY(F3v[i][j][k],2,buffer.v[i][j][k]);
pSUM(buffer.p[i][j][k],buffer.v[i][j][k],buffer.p[i][j][k]);
pSUM(buffer.p[i][j][k],F1v[i][j][k],buffer.p[i][j][k]);
pSUM(buffer.p[i][j][k],F4v[i][j][k],buffer.p[i][j][k]);
pMULTIPLY(buffer.p[i][j][k],1.0 / 6,buffer.p[i][j][k]);
pSUM(buffer.p[i][j][k],jello->v[i][j][k],jello->v[i][j][k]);
}
return;
}
void RenderClothSystem(cloth *clothItem, GLuint texId)
{
double length = 0.0f;
struct tex
{
float u;
float v;
};
struct tex texture, increment;
if(gRenderMode == THREAD)
{
glLineWidth(1);
glPointSize(5);
glDisable(GL_LIGHTING);
// Render the cloth points
glBegin(GL_POINTS);
RenderPoints(clothItem);
glEnd();
// Render the cloth springs
for(int row = 0; row < clothItem->height; row++)
{
for(int col = 0; col < clothItem->width; col++)
{
glBegin(GL_LINES);
// Render the structural springs
RenderSpring(clothItem, nMake(col, row, 0), nMake(1, 0, 0));
RenderSpring(clothItem, nMake(col, row, 0), nMake(-1, 0, 0));
RenderSpring(clothItem, nMake(col, row, 0), nMake(0, 1, 0));
RenderSpring(clothItem, nMake(col, row, 0), nMake(0, -1, 0));
// Render shear springs along the side
RenderSpring(clothItem, nMake(col, row, 0), nMake(1, 1, 0));
RenderSpring(clothItem, nMake(col, row, 0), nMake(1, -1, 0));
RenderSpring(clothItem, nMake(col, row, 0), nMake(-1, 1, 0));
RenderSpring(clothItem, nMake(col, row, 0), nMake(-1, -1, 0));
// Render bend springs
RenderSpring(clothItem, nMake(col, row, 0), nMake(2, 0, 0));
RenderSpring(clothItem, nMake(col, row, 0), nMake(-2, 0, 0));
RenderSpring(clothItem, nMake(col, row, 0), nMake(0, 2, 0));
RenderSpring(clothItem, nMake(col, row, 0), nMake(0, -2, 0));
glEnd();
}
}
glEnable(GL_LIGHTING);
}
if(gRenderMode == TRIANGLE)
{
glDisable(GL_LIGHTING);
glDisable(GL_CULL_FACE);
glPolygonMode(GL_FRONT, GL_FILL);
// Accumulate the normals for all the points in the cloth
point len1, len2, len3;
int index, index1, index2, index3;
for(int row = 0; row < clothItem->height - 1; row++)
{
for(int col = 0; col < clothItem->width - 1; col ++)
{
/*
index index1
-----------
| /|
| T1 / |
| / |
| / |
| / |
| / |
| / |
| / T2 |
|/ |
-----------
index2 index3
*/
index = FindIndexInArray(row, col, clothItem->width);
index1 = FindIndexInArray(row, col + 1, clothItem->width);
index2 = FindIndexInArray(row + 1, col, clothItem->width);
index3 = FindIndexInArray(row + 1, col + 1, clothItem->width);
// First triangle - left top
pDIFFERENCE(clothItem->positions[index1], clothItem->positions[index], len1);
pDIFFERENCE(clothItem->positions[index2], clothItem->positions[index], len2);
CROSSPRODUCTp(len1, len2, len3);
pNORMALIZE(len3);
pSUM(clothItem->normals[index1], len3, clothItem->normals[index1]);
clothItem->normalsCount[index1]++;
pSUM(clothItem->normals[index2], len3, clothItem->normals[index2]);
clothItem->normalsCount[index2]++;
pSUM(clothItem->normals[index], len3, clothItem->normals[index]);
clothItem->normalsCount[index]++;
// Second triangle - right bottom
pDIFFERENCE(clothItem->positions[index1], clothItem->positions[index3], len1);
pDIFFERENCE(clothItem->positions[index2], clothItem->positions[index3], len2);
CROSSPRODUCTp(len1, len2, len3);
pNORMALIZE(len3);
pSUM(clothItem->normals[index1], len3, clothItem->normals[index1]);
clothItem->normalsCount[index1]++;
pSUM(clothItem->normals[index2], len3, clothItem->normals[index2]);
clothItem->normalsCount[index2]++;
pSUM(clothItem->normals[index3], len3, clothItem->normals[index]);
clothItem->normalsCount[index3]++;
}
}
texture.u = 0.0;
texture.v = 0.0;
increment.u = 1.0 / clothItem->width;
increment.v = 1.0 / clothItem->height;
// Render the triangles
for(int row = 0; row < clothItem->height - 1; row++)
{
glBindTexture(GL_TEXTURE_2D, texId);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glBegin(GL_TRIANGLE_STRIP);
for(int col = 0; col < clothItem->width - 1; col ++)
{
//index index 2 index 1 index 3
index = FindIndexInArray(row, col, clothItem->width);
index1 = FindIndexInArray(row, col + 1, clothItem->width);
index2 = FindIndexInArray(row + 1, col, clothItem->width);
index3 = FindIndexInArray(row + 1, col + 1, clothItem->width);
// CCW order for the triangle strip
// left top (0,0)
glNormal3f(clothItem->normals[index].x / clothItem->normalsCount[index], clothItem->normals[index].y / clothItem->normalsCount[index], clothItem->normals[index].z / clothItem->normalsCount[index]);
glTexCoord2f(texture.u, texture.v);
glVertex3f(clothItem->positions[index].x, clothItem->positions[index].y, clothItem->positions[index].z);
// left bottom (0, 1)
glNormal3f(clothItem->normals[index2].x / clothItem->normalsCount[index2], clothItem->normals[index2].y / clothItem->normalsCount[index2], clothItem->normals[index2].z / clothItem->normalsCount[index2]);
glTexCoord2f(texture.u, texture.v + increment.v);
glVertex3f(clothItem->positions[index2].x, clothItem->positions[index2].y, clothItem->positions[index2].z);
// right top (1, 0)
glNormal3f(clothItem->normals[index1].x / clothItem->normalsCount[index1], clothItem->normals[index1].y / clothItem->normalsCount[index1], clothItem->normals[index1].z / clothItem->normalsCount[index1]);
glTexCoord2f(texture.u + increment.u, texture.v);
glVertex3f(clothItem->positions[index1].x, clothItem->positions[index1].y, clothItem->positions[index1].z);
// right bottom (1, 1)
glNormal3f(clothItem->normals[index3].x / clothItem->normalsCount[index3], clothItem->normals[index3].y / clothItem->normalsCount[index3], clothItem->normals[index3].z / clothItem->normalsCount[index3]);
glTexCoord2f(texture.u + increment.u, texture.v + increment.v);
glVertex3f(clothItem->positions[index3].x, clothItem->positions[index3].y, clothItem->positions[index3].z);
texture.u += increment.u;
}
texture.u = 0.0;
texture.v += increment.v;
glEnd();
}
glEnable(GL_LIGHTING);
}// if gRenderMode
}// end RenderClothSystem
// 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);
}
}
