/* 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;
}
#include "cloth.h"
int gRenderMode;
int gPin, gNstep, gMovePin, gScale = 0, gWhichCloth = 0, gWind = 0;
float gGravity, gDelta, gTstep, gWindAmountX = 0.0, gWindAmountZ = 0.0;
float gKThread, gDThread, gKWall, gDWall;
int RANDXLIMIT = 400, RANDYLIMIT = 100, RANDZLIMIT = 400;
point underWaterDamp = vMake(0.0), gravity;
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;
/* Function: phyzxInit
* Description: Creates and initializes the phyzx object
* Input: inputModel - Object model information
* phyzxObj - current object structure
* Output: None
*/
void phyzxInit(phyzx *phyzxObj)
{
int numVertices = 0;
int size = 0;
point v1, v2, v3;
numVertices = phyzxObj->model->numvertices + 1; // Count of the number of vertices in the Model
phyzxObj->h = gTStep;
phyzxObj->n = gNStep;
phyzxObj->alpha = gAlpha;
phyzxObj->beta = gBeta;
phyzxObj->delta = gDelta;
phyzxObj->kWall = gKCol;
phyzxObj->dWall = gDCol;
phyzxObj->kSphere = 50.0;
phyzxObj->dSphere = 0.2;
phyzxObj->totalMass = 0.0;
phyzxObj->avgVel = vMake(0.0);
phyzxObj->velocity = (point *)calloc(numVertices, sizeof(point));
phyzxObj->extForce = (point *)calloc(numVertices, sizeof(point));
phyzxObj->stable = (point *)calloc(numVertices, sizeof(point));
phyzxObj->goal = (point *)calloc(numVertices, sizeof(point));
phyzxObj->relStableLoc = (point *)calloc(numVertices, sizeof(point));
phyzxObj->relDeformedLoc = (point *)calloc(numVertices, sizeof(point));
phyzxObj->mass = (double *)calloc(numVertices, sizeof(double));
phyzxObj->triAreas = (double *)calloc(phyzxObj->model->numtriangles, sizeof(double));
phyzxObj->q = (matrix *)calloc(numVertices, sizeof(matrix));
phyzxObj->qT = (matrix *)calloc(numVertices, sizeof(matrix));
// Initialise attributes with stable values
for(int index = STARTFROM; index <= numVertices; index++)
{
phyzxObj->stable[index].x = phyzxObj->model->vertices[3*index];
phyzxObj->stable[index].y = phyzxObj->model->vertices[3*index+1];
phyzxObj->stable[index].z = phyzxObj->model->vertices[3*index+2];
phyzxObj->mass[index] = 0.0;//param.MASS;
phyzxObj->extForce[index] = vMake(0.0, gGravity, 0.0);
phyzxObj->velocity[index] = vMake(0.01, 0.0, 0.0);
}
for(unsigned int index = 0; index < phyzxObj->model->numtriangles; index++)
{
v1 = vMake(phyzxObj->model->vertices[3*phyzxObj->model->triangles[index].vindices[0]], phyzxObj->model->vertices[3*phyzxObj->model->triangles[index].vindices[0]+1], phyzxObj->model->vertices[3*phyzxObj->model->triangles[index].vindices[0]+2]);
v2 = vMake(phyzxObj->model->vertices[3*phyzxObj->model->triangles[index].vindices[1]], phyzxObj->model->vertices[3*phyzxObj->model->triangles[index].vindices[1]+1], phyzxObj->model->vertices[3*phyzxObj->model->triangles[index].vindices[1]+2]);
v3 = vMake(phyzxObj->model->vertices[3*phyzxObj->model->triangles[index].vindices[2]], phyzxObj->model->vertices[3*phyzxObj->model->triangles[index].vindices[2]+1], phyzxObj->model->vertices[3*phyzxObj->model->triangles[index].vindices[2]+2]);
phyzxObj->triAreas[index] = AreaOfTri(v1, v2, v3);
(*phyzxObj).surArea += phyzxObj->triAreas[index];
}
phyzxObj->NBTStruct = glmBuildNeighborStructure(phyzxObj->model);
phyzxObj->NBVStruct = vertexList(phyzxObj->model, phyzxObj->NBTStruct, 0.05);
filterNBV(phyzxObj->model, phyzxObj->NBVStruct);
compMass(phyzxObj->NBTStruct, phyzxObj);
CalcCM(0, phyzxObj);
CalcRelLoc(0, phyzxObj);
CalcAqq(phyzxObj);
calcTAqq(phyzxObj);
}
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]);
}
}
}
}
}
// 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;
}
}
}
void updateForce(node curP, node typeP, cloth *clothItem, int springType)
{
int inOrOut = 0; // 1 = inside , 0 = outside
int parsedOrNot = 0; // 1 = unparsed, 0 = already parsed
point hooksF, dampF; // to store the force computed in the above defined functions
int indexC, indexT;
memset( (void*)&hooksF, 0, sizeof(hooksF));
memset( (void*)&dampF, 0, sizeof(dampF));
inOrOut = checkIfInsideCloth(typeP, clothItem);
if(inOrOut)
{
// type# point is inside the jello cube
parsedOrNot = checkIfAlreadyParsed(typeP, curP);
if(parsedOrNot)
{
indexC = FindIndexInArray(curP.y, curP.x, clothItem->width);
indexT = FindIndexInArray(typeP.y, typeP.x, clothItem->width);
// type# point has not been parsed yet
hooksF = computeHooksForce(indexC, indexT, vMake(0.0), clothItem, springType, KELASTIC);
dampF = computeDampingForce(indexC, indexT, vMake(0.0), clothItem, DELASTIC);
// Add the forces to the current point in the loop
clothItem->force[indexC].x += hooksF.x + dampF.x;
clothItem->force[indexC].y += hooksF.y + dampF.y;
clothItem->force[indexC].z += hooksF.z + dampF.z;
// Add the forces to the current type# point
clothItem->force[indexT].x += -hooksF.x + (-dampF.x);
clothItem->force[indexT].y += -hooksF.y + (-dampF.y);
clothItem->force[indexT].z += -hooksF.z + (-dampF.z);
if(gWind == 1)
{
if(windCount < 30 && flag != 1)
{
clothItem->force[indexC].x += gWindAmountX;
clothItem->force[indexC].y += 0.0;
clothItem->force[indexC].z += gWindAmountZ;
windCount++;
if(windCount == 30)
{
flag = 1;
}
}
else
{
windCount -= 1;
if(windCount == 0)
{
flag = 0;
}
}
}
}
}
}
