// calculate the damping force for collision springs
void dampingForceCollision(const point& a, const point& b, const point& va, const point& vb, double kDamp, point& n, point& f)
{
// initialize temp force f
f.x = 0;
f.y = 0;
f.z = 0;
point diff;
point diffVelocity;
point normalizedDiff;
double diffLength;
double elasticForceScalar;
double cosF;
// let diff be the vector pointing from b to a
pDIFFERENCE(a, b, diff);
diffLength = sqrt(diff.x * diff.x + diff.y * diff.y + diff.z * diff.z);
// here va and vb are velocities of points a and b
pDIFFERENCE(va, vb, diffVelocity);
elasticForceScalar = (-kDamp) * ((diffVelocity.x * diff.x + diffVelocity.y * diff.y + diffVelocity.z * diff.z) / diffLength);
// normalized vector diff
normalizedDiff.x = diff.x / diffLength;
normalizedDiff.y = diff.y / diffLength;
normalizedDiff.z = diff.z / diffLength;
pMULTIPLY(normalizedDiff, elasticForceScalar, f);
DOTPRODUCTp(f, n, cosF);
pMULTIPLY(n, cosF, f);
}
/* 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]);
}
}
/* 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]);
}
}
}
}
// calculate the hook force for collision springs
void hookForceCollision(const point& a, const point& b, double kHook, point& n, point& f)
{
// initialize temp force f
f.x = 0;
f.y = 0;
f.z = 0;
point diff;
double elasticForceScalar;
// let diff be the vector pointing from b to a
pDIFFERENCE(a, b, diff);
pMULTIPLY(diff, -kHook, diff);
DOTPRODUCTp(diff, n, elasticForceScalar);
pMULTIPLY(n, elasticForceScalar, f);
}
/* Function: computeHooksForce
* Description: Compute Hook's Law in 3D
* Input: index - index of the vertex which collided
* wallP - point on the wall where the vertex is colliding
* Output: Computed hooks force
*/
point computeHooksForce(int index, point B, phyzx *phyzxObj)
{
point L, unitV;
double mag = 0;
double restLength = 0, length;
point hooksForce, A;
memset( (void*)&L, 0, sizeof(L));
memset( (void*)&unitV, 0, sizeof(unitV));
memset( (void*)&hooksForce, 0, sizeof(hooksForce));
memset( (void*)&A, 0, sizeof(A));
A = vMake(phyzxObj->model->vertices[3*index], phyzxObj->model->vertices[3*index + 1], phyzxObj->model->vertices[3*index + 2]);
pDIFFERENCE(A, B, L);
pCPY(L, unitV);
pNORMALIZE(unitV);
//xxx
/*length = A.y - B.y;
unitV.x = 0;
unitV.y = 1;
unitV.z = 0;*/
pMULTIPLY(unitV, -(phyzxObj->kWall) * length * phyzxObj->mass[index], hooksForce);
//pMULTIPLY(unitV, -(phyzxObj->kWall) * length, hooksForce);
return hooksForce;
}
/* 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
// calculate the hook force between point a and its the next neighbour point b
void hookForceBend(const point& a, const point& b, double kHook, point& f)
{
// initialize temp force f
f.x = 0;
f.y = 0;
f.z = 0;
point diff;
point normalizedDiff;
double diffLength;
double elasticForceScalar;
// let diff be the vector pointing from b to a
pDIFFERENCE(a, b, diff);
diffLength = sqrt(diff.x * diff.x + diff.y * diff.y + diff.z * diff.z);
// normalized vector diff
normalizedDiff.x = diff.x / diffLength;
normalizedDiff.y = diff.y / diffLength;
normalizedDiff.z = diff.z / diffLength;
// the elastic force exerted on a is
elasticForceScalar = (-kHook) * (diffLength - restLengthBend);
pMULTIPLY(normalizedDiff, elasticForceScalar, f);
}
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: 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
/* 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;
}
/* 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;
}
// 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;
}
}