/* 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;
}
bool Pick(int x, int y)
{
double length;
point ray_orig, ray_dir;
ViewUnProject(x, y, 0.0f, &ray_orig);
ViewUnProject(x, y, 1.0f, &ray_dir);
pDIFFERENCE(ray_dir, ray_orig, ray_dir);
pNORMALIZE(ray_dir);
}
/* 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
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
/* 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
// Hook's Law in 3D
// F = -k hook ( |L| - R) L / |L|
// springType 1 = Structural
// 2 = Shear side
// 3 = Shear diagonal
// 4 = Bend
// 5 = Collision
point computeHooksForce(int indexA, int indexB, point collisionP, cloth *clothItem, int springType, int coEffType)
{
point L, unitV;
double mag = 0;
double restLength = 0, length = 0.0;
point hooksForce;
memset( (void*)&L, 0, sizeof(L));
memset( (void*)&unitV, 0, sizeof(unitV));
memset( (void*)&hooksForce, 0, sizeof(hooksForce));
if(coEffType == KELASTIC)
{
L.x = clothItem->positions[indexA].x - clothItem->positions[indexB].x;
L.y = clothItem->positions[indexA].y - clothItem->positions[indexB].y;
L.z = clothItem->positions[indexA].z - clothItem->positions[indexB].z;
mag = sqrt((L.x * L.x) + (L.y * L.y) + (L.z * L.z));
unitV.x = L.x / mag;
unitV.y = L.y / mag;
unitV.z = L.z / mag;
// Find the rest length for the current type of spring
switch(springType)
{
case 1: //Structural spring
restLength = clothItem->strSprLen;
break;
case 2: //Shear side spring
restLength = clothItem->shrSprLen;
break;
case 3: //Bend spring
restLength = clothItem->bendSprLen;
break;
case 4: //Collision spring
restLength = 0;
break;
}
hooksForce.x = -(clothItem->kThread) * (mag - restLength) * (unitV.x);
hooksForce.y = -(clothItem->kThread) * (mag - restLength) * (unitV.y);
hooksForce.z = -(clothItem->kThread) * (mag - restLength) * (unitV.z);
}
else if(coEffType == KCOLLISION)
{
L.x = clothItem->positions[indexA].x - collisionP.x;
L.y = clothItem->positions[indexA].y - collisionP.y;
L.z = clothItem->positions[indexA].z - collisionP.z;
mag = sqrt((L.x * L.x) + (L.y * L.y) + (L.z * L.z));
// unitV.x = clothItem->normals[indexA].x / clothItem->normalsCount[indexA];
// unitV.y = clothItem->normals[indexA].y / clothItem->normalsCount[indexA];
// unitV.z = clothItem->normals[indexA].z / clothItem->normalsCount[indexA];
pNORMALIZE(unitV);
unitV.x = L.x / mag;
unitV.y = L.y / mag;
unitV.z = L.z / mag;
hooksForce.x = -(clothItem->kWall) * (mag) * (unitV.x);
hooksForce.y = -(clothItem->kWall) * (mag) * (unitV.y);
hooksForce.z = -(clothItem->kWall) * (mag) * (unitV.z);
}
return hooksForce;
}
