physenv.cpp

本程序描述的是一个三维织物动感模拟系统。计算机生成真实感服装被列入计算机图形学亟需解决的三大问题之一。

	// TAKE THE FULL STEP WITH THIS NEW INFORMATION
	IntegrateSysOverTime(m_CurrentSys,m_TempSys[0],m_TargetSys,DeltaTime);
}

///////////////////////////////////////////////////////////////////////////////
// Function:	RK4Integrate 
// Purpose:		Calculate new Positions and Velocities given a deltatime
// Arguments:	DeltaTime that has passed since last iteration
// Notes:		This integrator uses the Runga-Kutta 4 method
//				This could use a generic function 4 times instead of unrolled
//				but it was easier for me to debug.  Fun for you to optimize.
///////////////////////////////////////////////////////////////////////////////
void CPhysEnv::RK4Integrate( float DeltaTime)
{
/// Local Variables ///////////////////////////////////////////////////////////
	int loop;
	float		halfDeltaT,sixthDeltaT;
	tParticle	*source,*target,*accum1,*accum2,*accum3,*accum4;
///////////////////////////////////////////////////////////////////////////////
	halfDeltaT = DeltaTime / 2.0f;		// SOME TIME VALUES I WILL NEED
	sixthDeltaT = 1.0f / 6.0f;

	// FIRST STEP
	source = m_CurrentSys;	// CURRENT STATE OF PARTICLE
	target = m_TempSys[0];	// TEMP STORAGE FOR NEW POSITION
	accum1 = m_TempSys[1];	// ACCUMULATE THE INTEGRATED VALUES
	for (loop = 0; loop < m_ParticleCnt; loop++)
	{
		accum1->f.x = halfDeltaT * source->f.x * source->oneOverM;
		accum1->f.y = halfDeltaT * source->f.y * source->oneOverM;
		accum1->f.z = halfDeltaT * source->f.z * source->oneOverM;

		accum1->v.x = halfDeltaT * source->v.x;
		accum1->v.y = halfDeltaT * source->v.y;
		accum1->v.z = halfDeltaT * source->v.z;
		// DETERMINE THE NEW VELOCITY FOR THE PARTICLE OVER 1/2 TIME
		target->v.x = source->v.x + (accum1->f.x);
		target->v.y = source->v.y + (accum1->f.y);
		target->v.z = source->v.z + (accum1->f.z);

		target->oneOverM = source->oneOverM;

		// SET THE NEW POSITION
		target->pos.x = source->pos.x + (accum1->v.x);
		target->pos.y = source->pos.y + (accum1->v.y);
		target->pos.z = source->pos.z + (accum1->v.z);

		source++;
		target++;
		accum1++;
	}

	ComputeForces(m_TempSys[0]);  // COMPUTE THE NEW FORCES

	// SECOND STEP
	source = m_CurrentSys;	// CURRENT STATE OF PARTICLE
	target = m_TempSys[0];	// TEMP STORAGE FOR NEW POSITION
	accum1 = m_TempSys[2];	// ACCUMULATE THE INTEGRATED VALUES
	for (loop = 0; loop < m_ParticleCnt; loop++)
	{
		accum1->f.x = halfDeltaT * target->f.x * source->oneOverM;
		accum1->f.y = halfDeltaT * target->f.y * source->oneOverM;
		accum1->f.z = halfDeltaT * target->f.z * source->oneOverM;
		accum1->v.x = halfDeltaT * target->v.x;
		accum1->v.y = halfDeltaT * target->v.y;
		accum1->v.z = halfDeltaT * target->v.z;

		// DETERMINE THE NEW VELOCITY FOR THE PARTICLE
		target->v.x = source->v.x + (accum1->f.x);
		target->v.y = source->v.y + (accum1->f.y);
		target->v.z = source->v.z + (accum1->f.z);

		target->oneOverM = source->oneOverM;

		// SET THE NEW POSITION
		target->pos.x = source->pos.x + (accum1->v.x);
		target->pos.y = source->pos.y + (accum1->v.y);
		target->pos.z = source->pos.z + (accum1->v.z);

		source++;
		target++;
		accum1++;
	}

	ComputeForces(m_TempSys[0]);  // COMPUTE THE NEW FORCES

	// THIRD STEP
	source = m_CurrentSys;	// CURRENT STATE OF PARTICLE
	target = m_TempSys[0];	// TEMP STORAGE FOR NEW POSITION
	accum1 = m_TempSys[3];	// ACCUMULATE THE INTEGRATED VALUES
	for (loop = 0; loop < m_ParticleCnt; loop++)
	{
		// NOTICE I USE THE FULL DELTATIME THIS STEP
		accum1->f.x = DeltaTime * target->f.x * source->oneOverM;
		accum1->f.y = DeltaTime * target->f.y * source->oneOverM;
		accum1->f.z = DeltaTime * target->f.z * source->oneOverM;
		accum1->v.x = DeltaTime * target->v.x;
		accum1->v.y = DeltaTime * target->v.y;
		accum1->v.z = DeltaTime * target->v.z;

		// DETERMINE THE NEW VELOCITY FOR THE PARTICLE
		target->v.x = source->v.x + (accum1->f.x);
		target->v.y = source->v.y + (accum1->f.y);
		target->v.z = source->v.z + (accum1->f.z);

		target->oneOverM = source->oneOverM;

		// SET THE NEW POSITION
		target->pos.x = source->pos.x + (accum1->v.x);
		target->pos.y = source->pos.y + (accum1->v.y);
		target->pos.z = source->pos.z + (accum1->v.z);

		source++;
		target++;
		accum1++;
	}

	ComputeForces(m_TempSys[0]);  // COMPUTE THE NEW FORCES

	// FOURTH STEP
	source = m_CurrentSys;	// CURRENT STATE OF PARTICLE
	target = m_TempSys[0];	// TEMP STORAGE FOR NEW POSITION
	accum1 = m_TempSys[4];	// ACCUMULATE THE INTEGRATED VALUES
	for (loop = 0; loop < m_ParticleCnt; loop++)
	{
		// NOTICE I USE THE FULL DELTATIME THIS STEP
		accum1->f.x = DeltaTime * target->f.x * source->oneOverM;
		accum1->f.y = DeltaTime * target->f.y * source->oneOverM;
		accum1->f.z = DeltaTime * target->f.z * source->oneOverM;

		accum1->v.x = DeltaTime * target->v.x;
		accum1->v.y = DeltaTime * target->v.y;
		accum1->v.z = DeltaTime * target->v.z;

		// THIS TIME I DON'T NEED TO COMPUTE THE TEMPORARY POSITIONS
		source++;
		target++;
		accum1++;
	}

	source = m_CurrentSys;	// CURRENT STATE OF PARTICLE
	target = m_TargetSys;
	accum1 = m_TempSys[1];
	accum2 = m_TempSys[2];
	accum3 = m_TempSys[3];
	accum4 = m_TempSys[4];
	for (loop = 0; loop < m_ParticleCnt; loop++)
	{
		// DETERMINE THE NEW VELOCITY FOR THE PARTICLE USING RK4 FORMULA
		target->v.x = source->v.x + ((accum1->f.x + ((accum2->f.x + accum3->f.x) * 2.0f) + accum4->f.x) * sixthDeltaT);
		target->v.y = source->v.y + ((accum1->f.y + ((accum2->f.y + accum3->f.y) * 2.0f) + accum4->f.y) * sixthDeltaT);
		target->v.z = source->v.z + ((accum1->f.z + ((accum2->f.z + accum3->f.z) * 2.0f) + accum4->f.z) * sixthDeltaT);
		// DETERMINE THE NEW POSITION FOR THE PARTICLE USING RK4 FORMULA
		target->pos.x = source->pos.x + ((accum1->v.x + ((accum2->v.x + accum3->v.x) * 2.0f) + accum4->v.x) * sixthDeltaT);
		target->pos.y = source->pos.y + ((accum1->v.y + ((accum2->v.y + accum3->v.y) * 2.0f) + accum4->v.y) * sixthDeltaT);
		target->pos.z = source->pos.z + ((accum1->v.z + ((accum2->v.z + accum3->v.z) * 2.0f) + accum4->v.z) * sixthDeltaT);

		source++;
		target++;
		accum1++;
		accum2++;
		accum3++;
		accum4++;
	}

}

int CPhysEnv::CheckForCollisions( tParticle	*system )
{
    // be optimistic!
    int collisionState = NOT_COLLIDING;
    float const depthEpsilon = 0.001f;

	int loop;
	tParticle *curParticle;

	m_ContactCnt = 0;		// THERE ARE CURRENTLY NO CONTACTS

	curParticle = system;
	for (loop = 0; (loop < m_ParticleCnt) && (collisionState != PENETRATING); 
			loop++,curParticle++)
	{
		// CHECK THE MAIN BOUNDARY PLANES FIRST
        for(int planeIndex = 0;(planeIndex < m_CollisionPlaneCnt) &&
            (collisionState != PENETRATING);planeIndex++)
        {
			tCollisionPlane *plane = &m_CollisionPlane[planeIndex];

            float axbyczd = DotProduct(&curParticle->pos,&plane->normal) + plane->d;

            if(axbyczd < -depthEpsilon)
            {
				// ONCE ANY PARTICLE PENETRATES, QUIT THE LOOP
				collisionState = PENETRATING;
            }
            else
            if(axbyczd < depthEpsilon)
            {
                float relativeVelocity = DotProduct(&plane->normal,&curParticle->v);

                if(relativeVelocity < 0.0f)
                {
                    collisionState = COLLIDING;
					m_Contact[m_ContactCnt].particle = loop; 
					memcpy(&m_Contact[m_ContactCnt].normal,&plane->normal,sizeof(tVector));
					m_ContactCnt++;
                }
            }
        }
		if (m_CollisionActive)
		{
			// THIS IS NEW FROM MARCH - ADDED SPHERE BOUNDARIES
			// CHECK ANY SPHERE BOUNDARIES
			for(int sphereIndex = 0;(sphereIndex < m_SphereCnt) &&
				(collisionState != PENETRATING);sphereIndex++)
			{
				tCollisionSphere *sphere = &m_Sphere[sphereIndex];

				tVector	distVect;

				VectorDifference(&curParticle->pos, &sphere->pos, &distVect);

				float radius = VectorSquaredLength(&distVect);
				// SINCE IT IS TESTING THE SQUARED DISTANCE, SQUARE THE RADIUS ALSO
				float dist = radius - (sphere->radius * sphere->radius);

				if(dist < -depthEpsilon)
				{
					// ONCE ANY PARTICLE PENETRATES, QUIT THE LOOP
					collisionState = PENETRATING;
				}
				else
				if(dist < depthEpsilon)
				{
					// NORMALIZE THE VECTOR
					NormalizeVector(&distVect);

					float relativeVelocity = DotProduct(&distVect,&curParticle->v);

					if(relativeVelocity < 0.0f)
					{
						collisionState = COLLIDING;
						m_Contact[m_ContactCnt].particle = loop; 
						memcpy(&m_Contact[m_ContactCnt].normal,&distVect,sizeof(tVector));
						m_ContactCnt++;
					}
				}
			}
		}

	}

    return collisionState;
}

void CPhysEnv::ResolveCollisions( tParticle	*system )
{
	tContact	*contact;
	tParticle	*particle;		// THE PARTICLE COLLIDING
	float		VdotN;		
	tVector		Vn,Vt;				// CONTACT RESOLUTION IMPULSE
	contact = m_Contact;
	for (int loop = 0; loop < m_ContactCnt; loop++,contact++)
	{
		particle = &system[contact->particle];
		// CALCULATE Vn
		VdotN = DotProduct(&contact->normal,&particle->v);
		ScaleVector(&contact->normal, VdotN, &Vn);
		// CALCULATE Vt
		VectorDifference(&particle->v, &Vn, &Vt);
		// SCALE Vn BY COEFFICIENT OF RESTITUTION
		ScaleVector(&Vn, m_Kr, &Vn);
		// SET THE VELOCITY TO BE THE NEW IMPULSE
		VectorDifference(&Vt, &Vn, &particle->v);
	}
}

void CPhysEnv::Simulate(float DeltaTime, BOOL running)
{
    float		CurrentTime = 0.0f;
    float		TargetTime = DeltaTime;
	tParticle	*tempSys;
	int			collisionState;

    while(CurrentTime < DeltaTime)
    {
		if (running)
		{
			ComputeForces(m_CurrentSys);

			// IN ORDER TO MAKE THINGS RUN FASTER, I HAVE THIS LITTLE TRICK
			// IF THE SYSTEM IS DOING A BINARY SEARCH FOR THE COLLISION POINT,
			// I FORCE EULER'S METHOD ON IT. OTHERWISE, LET THE USER CHOOSE.
			// THIS DOESN'T SEEM TO EFFECT STABILITY EITHER WAY
			if (m_CollisionRootFinding)
			{
				EulerIntegrate(TargetTime-CurrentTime);
			}
			else
			{
				switch (m_IntegratorType)
				{
				case EULER_INTEGRATOR:
					EulerIntegrate(TargetTime-CurrentTime);
					break;
				case MIDPOINT_INTEGRATOR:
					MidPointIntegrate(TargetTime-CurrentTime);
					break;
				case RK4_INTEGRATOR:
					RK4Integrate(TargetTime-CurrentTime);
					break;
				}
			}
		}

		collisionState = CheckForCollisions(m_TargetSys);

        if(collisionState == PENETRATING)
        {
			// TELL THE SYSTEM I AM LOOKING FOR A COLLISION SO IT WILL USE EULER
			m_CollisionRootFinding = TRUE;
            // we simulated too far, so subdivide time and try again
            TargetTime = (CurrentTime + TargetTime) / 2.0f;

            // blow up if we aren't moving forward each step, which is
            // probably caused by interpenetration at the frame start
            assert(fabs(TargetTime - CurrentTime) > EPSILON);
        }
        else
        {
            // either colliding or clear
            if(collisionState == COLLIDING)
            {
                int Counter = 0;
                do
                {
                    ResolveCollisions(m_TargetSys);
                    Counter++;
                } while((CheckForCollisions(m_TargetSys) ==
                            COLLIDING) && (Counter < 100));

                assert(Counter < 100);
				m_CollisionRootFinding = FALSE;  // FOUND THE COLLISION POINT
            }

            // we made a successful step, so swap configurations
            // to "save" the data for the next step
            
			CurrentTime = TargetTime;
			TargetTime = DeltaTime;

			// SWAP MY TWO PARTICLE SYSTEM BUFFERS SO I CAN DO IT AGAIN
			tempSys = m_CurrentSys;
			m_CurrentSys = m_TargetSys;
			m_TargetSys = tempSys;
        }
    }
}

///////////////////////////////////////////////////////////////////////////////
// Function:	AddCollisionSphere 
// Purpose:		Add a collision sphere to the system
///////////////////////////////////////////////////////////////////////////////
void CPhysEnv::AddCollisionSphere()
{
/// Local Variables ///////////////////////////////////////////////////////////
	tCollisionSphere *temparray;
	CDlgAddSphere		dialog;
///////////////////////////////////////////////////////////////////////////////
	dialog.m_Radius = 2.0f;
	dialog.m_XPos = 0.0f;
	dialog.m_YPos = -3.0f;
	dialog.m_ZPos = 0.0f;
	if (dialog.DoModal())
	{
		temparray = (tCollisionSphere *)malloc(sizeof(tCollisionSphere) * (m_SphereCnt+1)); 	
		if (m_SphereCnt > 0)
		{
			memcpy(temparray,m_Sphere,sizeof(tCollisionSphere) * m_SphereCnt);
			free(m_Sphere);
		}
		
		MAKEVECTOR(temparray[m_SphereCnt].pos,dialog.m_XPos,dialog.m_YPos, dialog.m_ZPos)
		temparray[m_SphereCnt].radius = dialog.m_Radius;
	
		m_Sphere = temparray;
		m_SphereCnt++;
	}
}