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);
}
}
/*
* Replica of STEP function:
* FUNCTION first_proj_axis()
*/
void
Axis2Placement3D::FirstProjAxis(double *proj,double *zaxis, double *refdir) {
double z[3] = VINIT_ZERO;
double v[3] = VINIT_ZERO;
double TOL = 1e-9;
if (zaxis == NULL)
return;
VMOVE(z,zaxis);
VUNITIZE(z);
if (refdir == NULL) {
double xplus[3]= {1.0,0.0,0.0};
double xminus[3]= {-1.0,0.0,0.0};
if (!VNEAR_EQUAL(z, xplus, TOL) &&
!VNEAR_EQUAL(z, xminus, TOL)) {
VSET(v,1.0,0.0,0.0);
} else {
VSET(v,0.0,1.0,0.0);
}
} else {
double cross[3];
double mag;
VCROSS(cross, refdir, z);
mag = MAGNITUDE(cross);
if (NEAR_ZERO(mag,TOL)) {
return;
} else {
VMOVE(v,refdir);
VUNITIZE(v);
}
}
double x_vec[3];
double aproj[3];
double dot = VDOT(v,z);
ScalarTimesVector(x_vec, dot, z);
VectorDifference(aproj,v,x_vec);
VSCALE(x_vec,z,dot);
VSUB2(aproj,v, x_vec);
VUNITIZE(aproj);
VMOVE(proj,aproj);
return;
}
void CPhysEnv::ComputeForces( tParticle *system )
{
int loop;
tParticle *curParticle,*p1, *p2;
tSpring *spring;
float dist, Hterm, Dterm;
tVector springForce,deltaV,deltaP;
curParticle = system;
for (loop = 0; loop < m_ParticleCnt; loop++)
{
MAKEVECTOR(curParticle->f,0.0f,0.0f,0.0f) // CLEAR FORCE VECTOR
if (m_UseGravity && curParticle->oneOverM != 0)
{
curParticle->f.x += (m_Gravity.x / curParticle->oneOverM);
curParticle->f.y += (m_Gravity.y / curParticle->oneOverM);
curParticle->f.z += (m_Gravity.z / curParticle->oneOverM);
}
if (m_UseDamping)
{
curParticle->f.x += (-m_Kd * curParticle->v.x);
curParticle->f.y += (-m_Kd * curParticle->v.y);
curParticle->f.z += (-m_Kd * curParticle->v.z);
}
else
{
curParticle->f.x += (-DEFAULT_DAMPING * curParticle->v.x);
curParticle->f.y += (-DEFAULT_DAMPING * curParticle->v.y);
curParticle->f.z += (-DEFAULT_DAMPING * curParticle->v.z);
}
curParticle++;
}
// CHECK IF THERE IS A USER FORCE BEING APPLIED
if (m_UserForceActive)
{
if (m_Pick[0] != -1)
{
VectorSum(&system[m_Pick[0]].f,&m_UserForce,&system[m_Pick[0]].f);
}
if (m_Pick[1] != -1)
{
VectorSum(&system[m_Pick[1]].f,&m_UserForce,&system[m_Pick[1]].f);
}
MAKEVECTOR(m_UserForce,0.0f,0.0f,0.0f); // CLEAR USER FORCE
}
// NOW DO ALL THE SPRINGS
spring = m_Spring;
for (loop = 0; loop < m_SpringCnt; loop++)
{
p1 = &system[spring->p1];
p2 = &system[spring->p2];
VectorDifference(&p1->pos,&p2->pos,&deltaP); // Vector distance
dist = VectorLength(&deltaP); // Magnitude of deltaP
Hterm = (dist - spring->restLen) * spring->Ks; // Ks * (dist - rest)
VectorDifference(&p1->v,&p2->v,&deltaV); // Delta Velocity Vector
Dterm = (DotProduct(&deltaV,&deltaP) * spring->Kd) / dist; // Damping Term
ScaleVector(&deltaP,1.0f / dist, &springForce); // Normalize Distance Vector
ScaleVector(&springForce,-(Hterm + Dterm),&springForce); // Calc Force
VectorSum(&p1->f,&springForce,&p1->f); // Apply to Particle 1
VectorDifference(&p2->f,&springForce,&p2->f); // - Force on Particle 2
spring++; // DO THE NEXT SPRING
}
// APPLY THE MOUSE DRAG FORCES IF THEY ARE ACTIVE
if (m_MouseForceActive)
{
// APPLY TO EACH PICKED PARTICLE
if (m_Pick[0] > -1)
{
p1 = &system[m_Pick[0]];
VectorDifference(&p1->pos,&m_MouseDragPos[0],&deltaP); // Vector distance
dist = VectorLength(&deltaP); // Magnitude of deltaP
if (dist != 0.0f)
{
Hterm = (dist) * m_MouseForceKs; // Ks * dist
ScaleVector(&deltaP,1.0f / dist, &springForce); // Normalize Distance Vector
ScaleVector(&springForce,-(Hterm),&springForce); // Calc Force
VectorSum(&p1->f,&springForce,&p1->f); // Apply to Particle 1
}
}
if (m_Pick[1] > -1)
{
p1 = &system[m_Pick[1]];
VectorDifference(&p1->pos,&m_MouseDragPos[1],&deltaP); // Vector distance
dist = VectorLength(&deltaP); // Magnitude of deltaP
if (dist != 0.0f)
{
Hterm = (dist) * m_MouseForceKs; // Ks * dist
ScaleVector(&deltaP,1.0f / dist, &springForce); // Normalize Distance Vector
ScaleVector(&springForce,-(Hterm),&springForce); // Calc Force
VectorSum(&p1->f,&springForce,&p1->f); // Apply to Particle 1
}
}
}
}
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;
}
