void PAAvoid::Execute(ParticleGroup *group)
{
float magdt = magnitude * dt;
switch(position.type)
{
case PDPlane:
{
if(look_ahead < P_MAXFLOAT)
{
for(int i = 0; i < group->p_count; i++)
{
Particle &m = group->list[i];
// p2 stores the plane normal (the a,b,c of the plane eqn).
// Old and new distances: dist(p,plane) = n * p + d
// radius1 stores -n*p, which is d.
float dist = m.pos * position.p2 + position.radius1;
if(dist < look_ahead)
{
float vm = m.vel.length();
pVector Vn = m.vel / vm;
// float dot = Vn * position.p2;
pVector tmp = (position.p2 * (magdt / (dist*dist+epsilon))) + Vn;
m.vel = tmp * (vm / tmp.length());
}
}
}
else
{
for(int i = 0; i < group->p_count; i++)
{
Particle &m = group->list[i];
// p2 stores the plane normal (the a,b,c of the plane eqn).
// Old and new distances: dist(p,plane) = n * p + d
// radius1 stores -n*p, which is d.
float dist = m.pos * position.p2 + position.radius1;
float vm = m.vel.length();
pVector Vn = m.vel / vm;
// float dot = Vn * position.p2;
pVector tmp = (position.p2 * (magdt / (dist*dist+epsilon))) + Vn;
m.vel = tmp * (vm / tmp.length());
}
}
}
break;
case PDRectangle:
{
// Compute the inverse matrix of the plane basis.
pVector &u = position.u;
pVector &v = position.v;
// The normalized bases are needed inside the loop.
pVector un = u / position.radius1Sqr;
pVector vn = v / position.radius2Sqr;
// w = u cross v
float wx = u.y*v.z-u.z*v.y;
float wy = u.z*v.x-u.x*v.z;
float wz = u.x*v.y-u.y*v.x;
float det = 1/(wz*u.x*v.y-wz*u.y*v.x-u.z*wx*v.y-u.x*v.z*wy+v.z*wx*u.y+u.z*v.x*wy);
pVector s1((v.y*wz-v.z*wy), (v.z*wx-v.x*wz), (v.x*wy-v.y*wx));
s1 *= det;
pVector s2((u.y*wz-u.z*wy), (u.z*wx-u.x*wz), (u.x*wy-u.y*wx));
s2 *= -det;
// See which particles bounce.
for(int i = 0; i < group->p_count; i++)
{
Particle &m = group->list[i];
// See if particle's current and next positions cross plane.
// If not, couldn't bounce, so keep going.
pVector pnext(m.pos + m.vel * dt * look_ahead);
// p2 stores the plane normal (the a,b,c of the plane eqn).
// Old and new distances: dist(p,plane) = n * p + d
// radius1 stores -n*p, which is d.
float distold = m.pos * position.p2 + position.radius1;
float distnew = pnext * position.p2 + position.radius1;
// Opposite signs if product < 0
// There is no faster way to do this.
if(distold * distnew >= 0)
continue;
float nv = position.p2 * m.vel;
float t = -distold / nv;
// Actual intersection point p(t) = pos + vel t
pVector phit(m.pos + m.vel * t);
// Offset from origin in plane, p - origin
pVector offset(phit - position.p1);
// Dot product with basis vectors of old frame
// in terms of new frame gives position in uv frame.
float upos = offset * s1;
float vpos = offset * s2;
// Did it cross plane outside triangle?
if(upos < 0 || vpos < 0 || upos > 1 || vpos > 1)
continue;
// A hit! A most palpable hit!
// Compute distance to the three edges.
pVector uofs = (un * (un * offset)) - offset;
float udistSqr = uofs.length2();
pVector vofs = (vn * (vn * offset)) - offset;
float vdistSqr = vofs.length2();
pVector foffset((u + v) - offset);
pVector fofs = (un * (un * foffset)) - foffset;
float fdistSqr = fofs.length2();
pVector gofs = (un * (un * foffset)) - foffset;
float gdistSqr = gofs.length2();
pVector S;
if(udistSqr <= vdistSqr && udistSqr <= fdistSqr
&& udistSqr <= gdistSqr) S = uofs;
else if(vdistSqr <= fdistSqr && vdistSqr <= gdistSqr) S = vofs;
else if(fdistSqr <= gdistSqr) S = fofs;
else S = gofs;
S.normalize();
// We now have a vector to safety.
float vm = m.vel.length();
pVector Vn = m.vel / vm;
// Blend S into V.
pVector tmp = (S * (magdt / (t*t+epsilon))) + Vn;
m.vel = tmp * (vm / tmp.length());
}
}
break;
case PDTriangle:
{
// Compute the inverse matrix of the plane basis.
pVector &u = position.u;
pVector &v = position.v;
// The normalized bases are needed inside the loop.
pVector un = u / position.radius1Sqr;
pVector vn = v / position.radius2Sqr;
// f is the third (non-basis) triangle edge.
pVector f = v - u;
pVector fn(f);
fn.normalize();
// w = u cross v
float wx = u.y*v.z-u.z*v.y;
float wy = u.z*v.x-u.x*v.z;
float wz = u.x*v.y-u.y*v.x;
float det = 1/(wz*u.x*v.y-wz*u.y*v.x-u.z*wx*v.y-u.x*v.z*wy+v.z*wx*u.y+u.z*v.x*wy);
pVector s1((v.y*wz-v.z*wy), (v.z*wx-v.x*wz), (v.x*wy-v.y*wx));
s1 *= det;
pVector s2((u.y*wz-u.z*wy), (u.z*wx-u.x*wz), (u.x*wy-u.y*wx));
s2 *= -det;
// See which particles bounce.
for(int i = 0; i < group->p_count; i++)
{
Particle &m = group->list[i];
// See if particle's current and next positions cross plane.
// If not, couldn't bounce, so keep going.
pVector pnext(m.pos + m.vel * dt * look_ahead);
// p2 stores the plane normal (the a,b,c of the plane eqn).
// Old and new distances: dist(p,plane) = n * p + d
// radius1 stores -n*p, which is d.
float distold = m.pos * position.p2 + position.radius1;
float distnew = pnext * position.p2 + position.radius1;
// Opposite signs if product < 0
// Is there a faster way to do this?
if(distold * distnew >= 0)
continue;
float nv = position.p2 * m.vel;
float t = -distold / nv;
// Actual intersection point p(t) = pos + vel t
pVector phit(m.pos + m.vel * t);
// Offset from origin in plane, p - origin
pVector offset(phit - position.p1);
// Dot product with basis vectors of old frame
// in terms of new frame gives position in uv frame.
float upos = offset * s1;
float vpos = offset * s2;
// Did it cross plane outside triangle?
if(upos < 0 || vpos < 0 || (upos + vpos) > 1)
continue;
// A hit! A most palpable hit!
// Compute distance to the three edges.
pVector uofs = (un * (un * offset)) - offset;
float udistSqr = uofs.length2();
pVector vofs = (vn * (vn * offset)) - offset;
float vdistSqr = vofs.length2();
pVector foffset(offset - u);
pVector fofs = (fn * (fn * foffset)) - foffset;
float fdistSqr = fofs.length2();
pVector S;
if(udistSqr <= vdistSqr && udistSqr <= fdistSqr) S = uofs;
else if(vdistSqr <= fdistSqr) S = vofs;
else S = fofs;
S.normalize();
// We now have a vector to safety.
float vm = m.vel.length();
pVector Vn = m.vel / vm;
// Blend S into V.
pVector tmp = (S * (magdt / (t*t+epsilon))) + Vn;
m.vel = tmp * (vm / tmp.length());
}
}
break;
case PDDisc:
{
float r1Sqr = fsqr(position.radius1);
float r2Sqr = fsqr(position.radius2);
// See which particles bounce.
for(int i = 0; i < group->p_count; i++)
{
Particle &m = group->list[i];
// See if particle's current and next positions cross plane.
// If not, couldn't bounce, so keep going.
pVector pnext(m.pos + m.vel * dt * look_ahead);
// p2 stores the plane normal (the a,b,c of the plane eqn).
// Old and new distances: dist(p,plane) = n * p + d
// radius1 stores -n*p, which is d. radius1Sqr stores d.
float distold = m.pos * position.p2 + position.radius1Sqr;
float distnew = pnext * position.p2 + position.radius1Sqr;
// Opposite signs if product < 0
// Is there a faster way to do this?
if(distold * distnew >= 0)
continue;
// Find position at the crossing point by parameterizing
// p(t) = pos + vel * t
// Solve dist(p(t),plane) = 0 e.g.
// n * p(t) + D = 0 ->
// n * p + t (n * v) + D = 0 ->
// t = -(n * p + D) / (n * v)
// Could factor n*v into distnew = distold + n*v and save a bit.
// Safe since n*v != 0 assured by quick rejection test.
// This calc is indep. of dt because we have established that it
// will hit before dt. We just want to know when.
float nv = position.p2 * m.vel;
float t = -distold / nv;
// Actual intersection point p(t) = pos + vel t
pVector phit(m.pos + m.vel * t);
// Offset from origin in plane, phit - origin
pVector offset(phit - position.p1);
float rad = offset.length2();
if(rad > r1Sqr || rad < r2Sqr)
continue;
// A hit! A most palpable hit!
pVector S = offset;
S.normalize();
// We now have a vector to safety.
float vm = m.vel.length();
pVector Vn = m.vel / vm;
// Blend S into V.
pVector tmp = (S * (magdt / (t*t+epsilon))) + Vn;
m.vel = tmp * (vm / tmp.length());
}
}
break;
case PDSphere:
{
float rSqr = position.radius1 * position.radius1;
// See which particles are aimed toward the sphere.
for(int i = 0; i < group->p_count; i++)
{
Particle &m = group->list[i];
// First do a ray-sphere intersection test and
// see if it's soon enough.
// Can I do this faster without t?
float vm = m.vel.length();
pVector Vn = m.vel / vm;
pVector L = position.p1 - m.pos;
float v = L * Vn;
float disc = rSqr - (L * L) + v * v;
if(disc < 0)
continue; // I'm not heading toward it.
// Compute length for second rejection test.
float t = v - sqrtf(disc);
if(t < 0 || t > (vm * look_ahead))
continue;
// Get a vector to safety.
pVector C = Vn ^ L;
C.normalize();
pVector S = Vn ^ C;
// Blend S into V.
pVector tmp = (S * (magdt / (t*t+epsilon))) + Vn;
m.vel = tmp * (vm / tmp.length());
}
}
break;
default:
break;
}
}
void prcsreq(int fd)
{
struct student s;
struct course c;
struct grade g;
int from = -1, to = -1;
if (
REQUEST("ADD STUDENT %d %s", &s.index, s.name) == 2
) {
astud(s);
sprintf(buff, "OK\n");
} else if (
REQUEST("ADD COURSE %d %s", &c.id, c.name) == 2
) {
acourse(c);
sprintf(buff, "OK\n");
} else if (
REQUEST("ADD GRADE %d %d %d", &g.index, &g.id, &g.value) == 3
) {
agrade(g);
sprintf(buff, "OK\n");
} else if (
REQUEST("FIND %d %d", &g.index, &g.id) == 2
) {
int grade = fgrade(g);
if (grade > 0) {
sprintf(buff, "GRADE %d\n", grade);
} else {
sprintf(buff, "NONE\n");
}
} else if (
REQUEST("MATCH OFFSET %s %s %d %d", s.name, c.name, &from, &to)
) {
foffset(s.name, c.name, fd, from, to);
} else if (
REQUEST("MATCH %s %s", s.name, c.name) == 2
) {
int grade = fgnames(s.name, c.name);
if (grade > 0) {
sprintf(buff, "GRADE %d\n", grade);
} else {
sprintf(buff, "NONE\n");
}
} else if (
REQUEST("MODIFY %d %d %d", &g.index, &g.id, &g.value) == 3
) {
mgrade(g);
sprintf(buff, "OK\n");
} else if (
REQUEST("REMOVE %d %d", &g.index, &g.id)
) {
rgrade(g);
sprintf(buff, "OK\n");
} else {
printf("Invalid request.\n");
}
/* Send reply for request. */
if (from == -1 && to == -1) {
write(fd, buff, strlen(buff));
}
}
