Ejemplo n.º 1
0
// 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);
}
Ejemplo n.º 2
0
/* 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]);
	}
}
Ejemplo n.º 3
0
/* 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]);
			}
		}
	}
}
Ejemplo n.º 4
0
// 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);
}
Ejemplo n.º 5
0
/* 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;
}
Ejemplo n.º 6
0
/* 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
Ejemplo n.º 7
0
// 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);
}
Ejemplo n.º 8
0
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);
	}
}
Ejemplo n.º 9
0
/* 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()
Ejemplo n.º 10
0
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; 
    
}
Ejemplo n.º 12
0
/* 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
Ejemplo n.º 13
0
/* 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;  
}
Ejemplo n.º 15
0
// 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;
	}
}