RNScalar R3MeshSearchTree::
DistanceSquared(const R3Point& query_position, const R3Box& box, RNScalar max_distance_squared) const
{
// Find and check axial distances from face to node box
RNScalar dx, dy, dz;
if (query_position.X() > box.XMax()) dx = query_position.X() - box.XMax();
else if (query_position.X() < box.XMin()) dx = box.XMin()- query_position.X();
else dx = 0.0;
RNScalar dx_squared = dx * dx;
if (dx_squared >= max_distance_squared) return dx_squared;
if (query_position.Y() > box.YMax()) dy = query_position.Y() - box.YMax();
else if (query_position.Y() < box.YMin()) dy = box.YMin()- query_position.Y();
else dy = 0.0;
RNScalar dy_squared = dy * dy;
if (dy_squared >= max_distance_squared) return dy_squared;
if (query_position.Z() > box.ZMax()) dz = query_position.Z() - box.ZMax();
else if (query_position.Z() < box.ZMin()) dz = box.ZMin()- query_position.Z();
else dz = 0.0;
RNScalar dz_squared = dz * dz;
if (dz_squared >= max_distance_squared) return dz_squared;
// Find and check actual distance from face to node box
RNScalar distance_squared = 0;
if ((dy == 0.0) && (dz == 0.0)) distance_squared = dx_squared;
else if ((dx == 0.0) && (dz == 0.0)) distance_squared = dy_squared;
else if ((dx == 0.0) && (dy == 0.0)) distance_squared = dz_squared;
else distance_squared = dx_squared + dy_squared + dz_squared;
// Return distance squared
return distance_squared;
}
static R3Vector calculate_sink_force(R3Scene* scene, R3Particle* particle)
{
R3ParticleSink* sink;
R3Vector force = R3Vector(0,0,0);
for (int i = 0; i < scene->NParticleSinks(); i++)
{
sink = scene->ParticleSink(i);
if (sink->shape->type == R3_SPHERE_SHAPE)
{
R3Sphere* sphere = sink->shape->sphere;
R3Point center = sphere->Center();
R3Vector f = -(particle->position - center);
double d = f.Length() - sphere->Radius();
f.Normalize();
double mag = sink->intensity / (sink->constant_attenuation +
sink->linear_attenuation*d +
sink->quadratic_attenuation*d*d);
force += f*mag;
}
else if (sink->shape->type == R3_MESH_SHAPE)
{
R3Mesh* mesh = sink->shape->mesh;
R3Point center = R3Point(0,0,0);
for (unsigned int j = 0; j < mesh->vertices.size(); j++)
{
center += mesh->vertices[i]->position / mesh->vertices.size();
}
R3Vector f = -(particle->position - center);
double d = f.Length();
f.Normalize();
double mag = sink->intensity / (sink->constant_attenuation +
sink->linear_attenuation*d +
sink->quadratic_attenuation*d*d);
force += f*mag;
}
else if (sink->shape->type == R3_BOX_SHAPE)
{
R3Box* box = sink->shape->box;
R3Point center = R3Point((box->XMax() - box->XMin())/2 + box->XMin(),
(box->YMax() - box->YMin())/2 + box->YMin(),
(box->ZMax() - box->ZMin())/2 + box->ZMin());
R3Vector f = -(particle->position - center);
double d = f.Length();
f.Normalize();
double mag = sink->intensity / (sink->constant_attenuation +
sink->linear_attenuation*d +
sink->quadratic_attenuation*d*d);
force += f*mag;
}
}
return force;
}
void R3Box::
Intersect (const R3Box& box)
{
// Intersect with box
if (minpt.X() < box.XMin()) minpt[0] = box.XMin();
if (minpt.Y() < box.YMin()) minpt[1] = box.YMin();
if (minpt.Z() < box.ZMin()) minpt[2] = box.ZMin();
if (maxpt.X() > box.XMax()) maxpt[0] = box.XMax();
if (maxpt.Y() > box.YMax()) maxpt[1] = box.YMax();
if (maxpt.Z() > box.ZMax()) maxpt[2] = box.ZMax();
}
void R3Box::
Union (const R3Box& box)
{
// Expand this to include box
if (minpt.X() > box.XMin()) minpt[0] = box.XMin();
if (minpt.Y() > box.YMin()) minpt[1] = box.YMin();
if (minpt.Z() > box.ZMin()) minpt[2] = box.ZMin();
if (maxpt.X() < box.XMax()) maxpt[0] = box.XMax();
if (maxpt.Y() < box.YMax()) maxpt[1] = box.YMax();
if (maxpt.Z() < box.ZMax()) maxpt[2] = box.ZMax();
}
RNBoolean R3Contains(const R3Box& box1, const R3Box& box2)
{
// Return whether box1 contains box2
if (box1.IsEmpty()) return FALSE;
if (box2.IsEmpty()) return TRUE;
if (RNIsLess(box2.XMin(), box1.XMin())) return FALSE;
if (RNIsLess(box2.YMin(), box1.YMin())) return FALSE;
if (RNIsLess(box2.ZMin(), box1.ZMin())) return FALSE;
if (RNIsGreater(box2.XMax(), box1.XMax())) return FALSE;
if (RNIsGreater(box2.YMax(), box1.YMax())) return FALSE;
if (RNIsGreater(box2.ZMax(), box1.ZMax())) return FALSE;
return TRUE;
}
double R3Distance(const R3Point& point, const R3Box& box)
{
// Find axial distances from point to box
double dx, dy, dz;
if (point.X() > box.XMax()) dx = point.X() - box.XMax();
else if (point.X() < box.XMin()) dx = box.XMin()- point.X();
else dx = 0.0;
if (point.Y() > box.YMax()) dy = point.Y() - box.YMax();
else if (point.Y() < box.YMin()) dy = box.YMin()- point.Y();
else dy = 0.0;
if (point.Z() > box.ZMax()) dz = point.Z() - box.ZMax();
else if (point.Z() < box.ZMin()) dz = box.ZMin()- point.Z();
else dz = 0.0;
// Return distance between point and closest point in box
if ((dy == 0.0) && (dz == 0.0)) return dx;
else if ((dx == 0.0) && (dz == 0.0)) return dy;
else if ((dx == 0.0) && (dy == 0.0)) return dz;
else return sqrt(dx*dx + dy*dy + dz*dz);
}
RNBoolean R3Contains(const R3Box& box, const R3Point& point)
{
// Return whether box contains point
if (box.IsEmpty()) return FALSE;
if (RNIsLess(point.X(), box.XMin())) return FALSE;
if (RNIsLess(point.Y(), box.YMin())) return FALSE;
if (RNIsLess(point.Z(), box.ZMin())) return FALSE;
if (RNIsGreater(point.X(), box.XMax())) return FALSE;
if (RNIsGreater(point.Y(), box.YMax())) return FALSE;
if (RNIsGreater(point.Z(), box.ZMax())) return FALSE;
return TRUE;
}
RNBoolean R3Contains(const R3Box& box, const R3Sphere& sphere)
{
// Return whether box contains sphere
if (box.IsEmpty()) return FALSE;
if (sphere.IsEmpty()) return TRUE;
if (RNIsLess(sphere.Center().X() - sphere.Radius(), box.XMin())) return FALSE;
if (RNIsLess(sphere.Center().Y() - sphere.Radius(), box.YMin())) return FALSE;
if (RNIsLess(sphere.Center().Z() - sphere.Radius(), box.ZMin())) return FALSE;
if (RNIsGreater(sphere.Center().X() + sphere.Radius(), box.XMax())) return FALSE;
if (RNIsGreater(sphere.Center().Y() + sphere.Radius(), box.YMax())) return FALSE;
if (RNIsGreater(sphere.Center().Z() + sphere.Radius(), box.ZMax())) return FALSE;
return TRUE;
}
////////////////////////////////////////////////////////////
// Check for collisions with rocks
////////////////////////////////////////////////////////////
void CheckCollisions(R3Scene *scene)
{
const double MOVEMENT_WEIGHT = 0.02;
// check each bobsled - //TODO CHANGE THIS WHEN WE HAVE MULTIPLE BOBSLEDS
for (unsigned int i = 0; i < 1; i++)
{
R3Bobsled *bobsled = scene->bobsleds[i];
bobsled->x_vibration = 0.0;
R3Box &bbox = bobsled->sleds[0]->mesh->bbox;
// printf("bobsled: %f %f %f %f %f %f\n", bbox.XMin(), bbox.XMax(), bbox.YMin(), bbox.YMax(), bbox.ZMin(), bbox.ZMax());
// check each obstacle for a collision
for (unsigned int j = 0; j < scene->obstacles.size(); j++)
{
R3Obstacle *obstacle = scene->obstacles[j];
R3Box intersection = bbox;
R3Box *obstacle_box = ObstacleBBox(obstacle);
intersection.Intersect(*obstacle_box);
if (intersection.XMin() < intersection.XMax() &&
intersection.YMin() < intersection.YMax() &&
intersection.ZMin() < intersection.ZMax())
{
double current_z = bobsled->velocity.Z();
// slow down
if (obstacle->hit_count == 0 && abs(current_z) > 40)
bobsled->velocity.SetZ(current_z * 0.6);
// add left-to-right vibration if this is a rock
if (obstacle->type == OBSTACLE_ROCK)
{
if (obstacle->hit_count - 2 * ((int) obstacle->hit_count / 2) == 0)
bobsled->x_vibration = MOVEMENT_WEIGHT * (1 + Rand());
else
bobsled->x_vibration = - MOVEMENT_WEIGHT * (1 + Rand());
}
obstacle->hit_count++;
}
delete obstacle_box;
}
}
}
static bool in_sink(R3Scene* scene, R3Particle* particle, double delta_time)
{
R3ParticleSink* sink;
for (int i = 0; i < scene->NParticleSinks(); i++)
{
sink = scene->ParticleSink(i);
if (sink->shape->type == R3_SPHERE_SHAPE)
{
R3Sphere* sphere = sink->shape->sphere;
R3Point center = sphere->Center();
double radius = sphere->Radius();
if ((particle->position - center).Length() < radius)
return true;
}
else if (sink->shape->type == R3_MESH_SHAPE)
{
R3Ray ray = R3Ray(particle->position, particle->velocity);
R3Node node;
node.shape = sink->shape;
R3Intersection inter = IntersectMesh(&node, &ray);
if (inter.hit == true
&& inter.t <= ((particle->velocity).Length() * (delta_time) * 2))
return true;
}
else if (sink->shape->type == R3_BOX_SHAPE)
{
R3Box* box = sink->shape->box;
if (particle->position.Z() < box->ZMax() && particle->position.Z() > box->ZMin()
&& particle->position.Y() < box->YMax() && particle->position.Y() > box->YMin()
&& particle->position.X() < box->XMax() && particle->position.X() > box->XMin())
return true;
}
}
return false;
}
static R3Particle* generate_random_box_particle(R3ParticleSource* source)
{
R3Box* box = source->shape->box;
int which_face = RandomNumber() * 6;
R3Point pos;
R3Vector dir;
if (which_face == 0)
{
pos = R3Point(box->XMin() + RandomNumber()*(box->XMax()-box->XMin()),
box->YMin() + RandomNumber()*(box->YMax()-box->YMin()),
box->ZMin());
dir = R3Vector(0, 0, -1);
}
else if (which_face == 1)
{
pos = R3Point(box->XMin() + RandomNumber()*(box->XMax()-box->XMin()),
box->YMin() + RandomNumber()*(box->YMax()-box->YMin()),
box->ZMax());
dir = R3Vector(0, 0, 1);
}
else if (which_face == 2)
{
pos = R3Point(box->XMin(),
box->YMin() + RandomNumber()*(box->YMax()-box->YMin()),
box->ZMin() + RandomNumber()*(box->ZMax()-box->ZMin()));
dir = R3Vector(-1, 0, 0);
}
else if (which_face == 3)
{
pos = R3Point(box->XMax(),
box->YMin() + RandomNumber()*(box->YMax()-box->YMin()),
box->ZMin() + RandomNumber()*(box->ZMax()-box->ZMin()));
dir = R3Vector(1, 0, 0);
}
else if (which_face == 4)
{
pos = R3Point(box->XMin() + RandomNumber()*(box->XMax()-box->XMin()),
box->YMin(),
box->ZMin() + RandomNumber()*(box->ZMax()-box->ZMin()));
dir = R3Vector(0, -1, 0);
}
else if (which_face == 5)
{
pos = R3Point(box->XMin() + RandomNumber()*(box->XMax()-box->XMin()),
box->YMax(),
box->ZMin() + RandomNumber()*(box->ZMax()-box->ZMin()));
dir = R3Vector(0, 1, 0);
}
R3Particle* particle = new R3Particle();
particle->position = pos;
particle->velocity = source->velocity * dir;
particle->mass = source->mass;
particle->fixed = source->fixed;
particle->drag = source->drag;
particle->elasticity = source->elasticity;
particle->lifetime = source->lifetime;
particle->material = source->material;
return particle;
}
// test if a point is inside a bounding box
bool InsideBBox(R3Box box, R3Point point)
{
return point.X() > box.XMin() && point.X() < box.XMax() && point.Y() > box.YMin() && point.Y() < box.YMax() && point.Z() > box.ZMin() && point.Z() < box.ZMax();
}