/* 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]);
}
}
/* 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;
}
void CopyToBuffer(cloth *buffer, cloth*clothItem)
{
int index = 0;
// Time step and nstep for the simulation
buffer->nStep = clothItem->nStep;
buffer->tStep = clothItem->tStep;
// Set the lengths of all the springs in the cloth based of the input strSprLen
buffer->strSprLen = clothItem->strSprLen;
buffer->shrSprLen = clothItem->shrSprLen;
buffer->bendSprLen = clothItem->bendSprLen;
// count of the number of vertives in the cloth
buffer->cArrayLength = clothItem->cArrayLength;
// Hooks coefficients
buffer->kThread = clothItem->kThread;
buffer->kWall = clothItem->kWall;
buffer->dThread = clothItem->dThread;
buffer->dWall = clothItem->dWall;
buffer->width = clothItem->width;
buffer->height = clothItem->height;
buffer->mass = clothItem->mass;
buffer->positions = (point*)calloc(buffer->cArrayLength, sizeof(point));
buffer->velocities = (point*)calloc(buffer->cArrayLength, sizeof(point));
buffer->acceleration = (point*)calloc(buffer->cArrayLength, sizeof(point));
buffer->force = (point*)calloc(buffer->cArrayLength, sizeof(point));
buffer->normals = (point*)calloc(buffer->cArrayLength, sizeof(point));
buffer->normalsCount = (int*)calloc(buffer->cArrayLength, sizeof(int));
for(int row = 0; row < clothItem->height; row++)
{
for(int col = 0; col < clothItem->width; col++)
{
index = FindIndexInArray(row, col, clothItem->width);
pCPY(clothItem->positions[index], buffer->positions[index]);
pCPY(clothItem->velocities[index], buffer->velocities[index]);
pCPY(clothItem->velocities[index], buffer->velocities[index]);
pCPY(clothItem->force[index], buffer->force[index]);
pCPY(clothItem->normals[index], buffer->normals[index]);
buffer->normalsCount[index] = clothItem->normalsCount[index];
}
}
}
void ClothInit(cloth *clothItem, double strSprLen, point birthPosition, int clothWidth, int clothHeight)
{
point cur;
memset( (void*)&cur, 0, sizeof(point));
int index = 0;
// Time step and nstep for the simulation
clothItem->nStep = gNstep;
clothItem->tStep = gTstep;
// Set the lengths of all the springs in the cloth based of the input strSprLen
clothItem->strSprLen = strSprLen;
clothItem->shrSprLen = sqrt(2.0) * strSprLen;
clothItem->bendSprLen = 2.0 * strSprLen;
// count of the number of vertives in the cloth
clothItem->cArrayLength = clothWidth * clothHeight;
// Hooks coefficients
clothItem->kThread = gKThread;
clothItem->kWall = gKWall;
clothItem->dThread = gDThread;
clothItem->dWall = gDWall;
clothItem->width = clothWidth;
clothItem->height = clothHeight;
// Same mass for all the vertices of the cloth
clothItem->mass = 0.02;
// Set the initial positions based on the structural spring length
clothItem->positions = (point*)calloc(clothItem->cArrayLength, sizeof(point));
for(int row = 0; row < clothHeight; row++)
{
memset( (void*)&cur, 0, sizeof(point));
// new row position
cur.y = birthPosition.y - (row * strSprLen);
for(int col = 0; col < clothWidth; col++)
{
// new col position
cur.x = birthPosition.x + (col * strSprLen);
index = FindIndexInArray(row, col, clothWidth);
cur.z = birthPosition.z;
pCPY(cur, clothItem->positions[index]);
}
}
// set the initial velocities for all the vertices of the cloth
clothItem->velocities = (point*)calloc(clothItem->cArrayLength, sizeof(point));
// set the initial acceleration for all the vertices of the cloth
clothItem->acceleration = (point*)calloc(clothItem->cArrayLength, sizeof(point));
for(int row = 0; row < clothHeight; row++)
{
for(int col = 0; col < clothWidth; col++)
{
index = FindIndexInArray(row, col, clothWidth);
// Change these for the initial velocities
clothItem->acceleration[index].x = 0.0;
clothItem->acceleration[index].y = 0.0;
clothItem->acceleration[index].z = 0.0;
}
}
clothItem->force = (point*)calloc(clothItem->cArrayLength, sizeof(point));
for(int row = 0; row < clothHeight; row++)
{
for(int col = 0; col < clothWidth; col++)
{
index = FindIndexInArray(row, col, clothWidth);
// Change these for the initial velocities
clothItem->force[index].x = 0.0;
clothItem->force[index].y = 0.0;
clothItem->force[index].z = 0.0;
}
}
clothItem->normals = (point*)calloc(clothItem->cArrayLength, sizeof(point));
clothItem->normalsCount = (int*)calloc(clothItem->cArrayLength, sizeof(int));
}// End of cloth initialization
/* 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;
}
// Computes acceleration to every point of the cloth
void computeAcceleration(cloth *clothItem)
{
node typeP, curP; // typeP = one of the 12 nodes to which the curP point is connected
int collided = 0, index;
memset( (void*)&typeP, 0, sizeof(typeP));
memset( (void*)&curP, 0, sizeof(curP));
/*
// FORCE FIELD COMPUTATION
if(ActivateFF)
{
// Check for force field
if(jello->resolution)
{
// compute the force field cube size and store it in the global variable
forceFieldCubeSize = ((double)4)/jello->resolution;
noOfCubesInRow = (int)(((double)jello->resolution) / forceFieldCubeSize);
// Compute the effect of force field and update the forces due to it on all the nodes
computeForceField(jello);
}
}
*/
// COLLISION DETECTION AND RESPONSE
for(int row = 0; row < clothItem->height; row++)
{
for(int col = 0; col < clothItem->width; col++)
{
memset( (void*)&curP, 0, sizeof(curP));
index = FindIndexInArray(curP.y, curP.x, clothItem->width);
pCPY(vMake(0.0), clothItem->force[index]);
curP.x = col;
curP.y = row;
// Checks whether any of the nodes is colliding with the bounding box and Sphere
checkForCollision(curP, clothItem);
}
}
for(int row = 0; row < clothItem->height; row++)
{
for(int col = 0; col < clothItem->width; col++)
{
curP.x = col;
curP.y = row;
// Check for all the springs possible = 12
// STRUCTURAL SPRINGS
//-----------------------------------------------------------------
// TYPE 1 -> (x+1, y, z) structural spring
typeP.x = col + 1;
typeP.y = row;
updateForce(curP, typeP, clothItem, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 2 -> (x-1, y, z)
typeP.x = col - 1;
typeP.y = row;
updateForce(curP, typeP, clothItem, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 3 -> (x, y+1, z)
typeP.x = col;
typeP.y = row + 1;
updateForce(curP, typeP, clothItem, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 4 -> (x, y-1, z)
typeP.x = col;
typeP.y = row - 1;
updateForce(curP, typeP, clothItem, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// SHEAR SPRINGS
//-----------------------------------------------------------------
// TYPE 5 -> (x+1, y+1, z)
typeP.x = col + 1;
typeP.y = row + 1;
updateForce(curP, typeP, clothItem, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 6 -> (x-1, y+1, z)
typeP.x = col - 1;
typeP.y = row + 1;
updateForce(curP, typeP, clothItem, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 7 -> (x+1, y-1, z)
typeP.x = col + 1;
typeP.y = row - 1;
updateForce(curP, typeP, clothItem, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 8 -> (x-1, y-1, z)
typeP.x = col - 1;
typeP.y = row - 1;
updateForce(curP, typeP, clothItem, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// BEND SPRINGS
//-----------------------------------------------------------------
// TYPE 9 -> (x+2, y, z)
typeP.x = col + 2;
typeP.y = row;
updateForce(curP, typeP, clothItem, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 10 -> (x-2, y, z)
typeP.x = col - 2;
typeP.y = row;
updateForce(curP, typeP, clothItem, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 11 -> (x, y+2, z)
typeP.x = col;
typeP.y = row + 2;
updateForce(curP, typeP, clothItem, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 12 -> (x, y-2, z)
typeP.x = col;
typeP.y = row - 2;
updateForce(curP, typeP, clothItem, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
}// for col
}// for row
for(int row = 0; row < clothItem->height; row++)
{
for(int col = 0; col < clothItem->width; col++)
{
index = FindIndexInArray(row, col, clothItem->width);
clothItem->acceleration[index].x = clothItem->force[index].x / clothItem->mass;
clothItem->acceleration[index].y = clothItem->force[index].y / clothItem->mass;
clothItem->acceleration[index].z = clothItem->force[index].z / clothItem->mass;
}
}
}
