/* Look for the nearest source to the target
node -- Place that we're searching
target -- source we're looking for
interest -- the area of interest
nearest -- the current nearest match
dist -- distance to current nearest match */
static void nearer_source(node_t *tree, node_t *node, double tx, double ty, box_t *interest, source_t **nearest, double *dist) {
source_t *s;
double s_dist;
if (debug) {
printf("nearer_source: (%0.4f, %0.4f) (%0.4f, %0.4f)\n",
node->box.xmin, node->box.ymin, node->box.xmax, node->box.ymax);
}
// Check if the node interesects with the area of interest
if (intersecting(&node->box, interest)) {
// If the node has no children run through the compares
if (node->q1 == NULL) {
if (debug)
printf(" intersection with leaf\n");
for (s = (source_t *)node->contents.first; s != NULL; s = (source_t *)s->links.next) {
s_dist = norm2(s->x, s->y, tx, ty);
if (debug)
printf(" comparing (%0.2f, %0.2f) dist %0.4f", s->x, s->y, s_dist);
if (s_dist < *dist) {
*nearest = s;
*dist = s_dist;
s_dist = sqrt(s_dist); // actual distance
interest->xmin = tx - s_dist;
interest->ymin = ty - s_dist;
interest->xmax = tx + s_dist;
interest->ymax = ty + s_dist;
clip_box(interest, &tree->box);
if (debug) {
printf(" -- new nearest: dist %0.4f box (%0.4f, %0.4f) (%0.4f, %0.4f)",
s_dist, interest->xmin, interest->ymin, interest->xmax, interest->ymax);
}
}
if (debug)
printf("\n");
}
} else {
if (debug)
printf(" intersection, checking children\n");
// If the node has children, recurse through the
// tree by calling nearer_source on each of the children
nearer_source(tree, node->q1, tx, ty, interest, nearest, dist);
nearer_source(tree, node->q2, tx, ty, interest, nearest, dist);
nearer_source(tree, node->q3, tx, ty, interest, nearest, dist);
nearer_source(tree, node->q4, tx, ty, interest, nearest, dist);
}
} else {
if (debug)
printf(" no intersection\n");
}
}
// Test loading of volume
TEST(MAIN, RangeBasic){
Range one(V3f(0,0,0), V3f(1,1,1)); //start
Range inside(V3f(0.1,0.1,0.1), V3f(0.9,0.9,0.9));
Range intersecting(V3f(0.1,0.1,0.1), V3f(0.9,5,0.9));
Range outside(V3f(1.1,-2,3), V3f(1.2,-1,3.8));
EXPECT_TRUE(ContainsPoint(one, V3f(0.5, 0.5, 0.5))); // Some common cases
EXPECT_FALSE(ContainsPoint(one, V3f(1.1, 0.5, 0.5)));
EXPECT_FALSE(ContainsPoint(one, V3f(-0.1, 0.5, 0.5)));
EXPECT_FALSE(ContainsPoint(one, V3f(0.5, 1.1, 0.5)));
EXPECT_FALSE(ContainsPoint(one, V3f(0.5, -0.1, 0.5)));
EXPECT_FALSE(ContainsPoint(one, V3f(0.5, 0.5, 1.1)));
EXPECT_FALSE(ContainsPoint(one, V3f(0.5, 0.5, -0.1)));
EXPECT_TRUE(ContainsRange(one, inside));
EXPECT_TRUE(IntersectsRange(one, intersecting));
EXPECT_FALSE(ContainsRange(one, intersecting));
EXPECT_FALSE(ContainsRange(one, outside));
};
void PhysicsObject::update(vector<PhysicsObject*> & otherObjects, float dTime)
{
for (int i=0; i<otherObjects.size(); i++){
PhysicsObject *otherObject = otherObjects[i];
// handle collisions
// A-posteriori collision detection
if (intersecting(otherObject) || passedThrough(otherObject) || otherObject->passedThrough(this)){
collide(otherObject, dTime);
// if still intersecting, split the difference
if (intersecting(otherObject))
{
ofVec2f difference = otherObject->position - position;
if (difference.length() == 0) difference = ofVec2f(1,0);
float overlap = (boundingRadius + otherObject->boundingRadius - position.distance(otherObject->position));
difference.normalize();
if (isAnchored){
otherObject->position += overlap * difference;
}
else if (otherObject->isAnchored){
position -= overlap * difference;
}
else{
position -= overlap * (difference/2.0f);
otherObject->position += overlap * (difference/2.0f);
}
}
}
// Calculate and update forces
ofVec2f forceToMe = otherObject->forceAppliedTo(this, dTime);
ofVec2f forceToOther = forceAppliedTo(otherObject, dTime);
force += forceToMe;
otherObject->force += forceToOther;
}
// Collide against walls
if (position.x - boundingRadius < 0){
velocity.x *= -1;
position.x = boundingRadius;
}
else if (position.x + boundingRadius > ofGetWidth())
{
velocity.x *= -1;
position.x = ofGetWidth() - boundingRadius;
}
if (position.y - boundingRadius < 0){
velocity.y *= -1;
position.y = boundingRadius;
}
else if (position.y + boundingRadius > ofGetHeight()){
velocity.y *= -1;
position.y = ofGetHeight() - boundingRadius;
}
}
