Unreliable collision detection between Sensor and other object in jBox2D (Processing)

121 views Asked by At

I am attempting to make a Neuroevolution program to create cars that can avoid other cars and park themselves. I have been able to implement the neural network, and the math needed to support it. Each car has a neural network that takes in inputs (from sensors) and produces output (steering and throttle). A car receives this input from its 12 sensors: 8 collision aversion sensors tell the car if it is about to collide with something, and 4 other senses tell it how far it is from its parking spot. The 8 collisions aversion (C.A.) sensors have fd.isSensor = true. I took into account that sensors collide with their own car (since they originate from it's center). The issue is that sometimes the collision listener beginContact/endContact does not trigger when it overlaps with a Boundary and only triggers in response to a Car when it collides with the center of the car. Sometimes, the sensors collisions do not register until the car collides with something. This defeats the purpose of the sensor. If anyone is familiar with collision listening in Box2D I would really appreciate you help so I can get on to the fun part-the genetic algorithm. Thank you!

Here is my collision listener:

/*
  Evolving Neural Network
 Author: Aniekan Umoren
 Created: Sun Sep, 10 2017
 */

// Imports
import controlP5.*;
import shiffman.box2d.*;
import org.jbox2d.common.*;
import org.jbox2d.dynamics.joints.*;
import org.jbox2d.collision.shapes.*;
import org.jbox2d.collision.shapes.Shape;
import org.jbox2d.common.*;
import org.jbox2d.dynamics.*;
import org.jbox2d.dynamics.contacts.*;
import org.jbox2d.callbacks.ContactImpulse;
import org.jbox2d.callbacks.ContactListener;
import org.jbox2d.collision.Manifold;
import org.jbox2d.dynamics.contacts.Contact;



Box2DProcessing box2d;
ControlP5 control;
int populationSize, CURR_CAR;
ArrayList <Car> Population;
ArrayList<Boundary> boundaries;


void setup() {
  size(1000, 600);
  box2d = new Box2DProcessing(this);

  // Initializing a BOX2D world
  box2d.createWorld();
  box2d.setGravity(0, 0);
  box2d.listenForCollisions();


  // Creating World Boundaries
  boundaries = new ArrayList<Boundary>();
  boundaries.add(new Boundary(width/2, 0, width, 5));
  boundaries.add(new Boundary(0, height/2, 5, height));
  boundaries.add(new Boundary(width/2, height, width, 5));
  boundaries.add(new Boundary(width, height/2, 5, height));

  // Creating Initial Population
  populationSize = 10;
  Population = new ArrayList<Car>();
  for (int i = 0; i < populationSize; i++){
    Population.add(new Car(new float[0]));
  }

  // Creating User Control Interface
  control = new ControlP5(this);
  Slider currCar_slider = control.addSlider("CURR_CAR",  0, populationSize-1, 0, width/2 - 250, 25, 500, 10); 
  currCar_slider.setColorBackground(color(255, 255, 255));

  CURR_CAR = 0;
}

void draw() {
  background(0);
  fill(255);

  // Draw the Parking Spaces
  rectMode(CENTER);
  rect(width/2-300, height/2, 10, 300);
  rect(width/2, height/2, 10, 300);
  rect(width/2+300, height/2, 10, 300);
  for (int j = 0; j < 4; j++){
    rect(width/2, height/2 + 50*j, 200, 10);
    rect(width/2, height/2 + -50*j, 200, 10);
    rect(width/2-300, height/2 + 50*j, 200, 10);
    rect(width/2-300, height/2 + -50*j, 200, 10);
    rect(width/2+300, height/2 + 50*j, 200, 10);
    rect(width/2+300, height/2 + -50*j, 200, 10);
  }

  box2d.step();

  for (Boundary b : boundaries){
    b.render();
  }

  for (Car c : Population){
    if (Population.indexOf(c) == CURR_CAR){
      c.displayNN(true);
    } else{
      c.displayNN(false);
    }
    c.update();
    c.render();
  }
}


// ---------------------------- PROBLEMATIC AREA --------------------------------

void preSolve(Contact cp, Manifold oldManifold) {
    Fixture f1 = cp.getFixtureA();
    Fixture f2 = cp.getFixtureB();

    Body b1 = f1.getBody();
    Body b2 = f2.getBody();

    Object o1 = b1.getUserData();
    Object o2 = b2.getUserData();

    /*print("PRE SOLVE:\n");
     print(o1.getClass());
     print("\n");
     print(o2.getClass());
     print("\n");*/

    if (o1.getClass() == Boundary.class && o2.getClass() == Boundary.class) { // Stop boundaries from colliding since it's unecessary
      cp.setEnabled(false);
      //print("stop\n\n");
    } else if (o1.getClass() == Sensor.class && o2.getClass() == Sensor.class) {
      cp.setEnabled(false);
    }
  }

  void beginContact(Contact cp) {
    //print("collided\n");
    Fixture f1 = cp.getFixtureA();
    Fixture f2 = cp.getFixtureB();

    Body b1 = f1.getBody();
    Body b2 = f2.getBody();

    Object o1 = b1.getUserData();
    Object o2 = b2.getUserData();

    print("BEGIN CONTACT:\n");
    print(o1.getClass());
    print("\n");
    print(o2.getClass());
    print("\n");

    if (o1.getClass() == Sensor.class ^ o2.getClass() == Sensor.class) {

      print("Collision b/w Sensor and non-Sensor\n");

      if (o1.getClass() == Sensor.class) {
        Sensor s = (Sensor) o1;
        if (o2.getClass() == Car.class) {
          Car c = (Car) o2;
          boolean flag = true;
          print("SENSOR ID: " + str(s.id) + " CAR ID: " + str(c.id) + "\n");
          if (s.id == c.id) {
            flag = false;
            cp.setEnabled(false);
          }
          s.changeState(flag);
        } else { // It is a boundary
          s.changeState(true);
        }
      } else if (o2.getClass() == Sensor.class) {
        Sensor s = (Sensor) o2;
        if (o1.getClass() == Car.class) {
          Car c = (Car) o1;
          boolean flag = true;
          print("SENSOR ID: " + str(s.id) + " CAR ID: " + str(c.id) + "\n");
          if (s.id == c.id) {
            flag = false;
            cp.setEnabled(false);
          }
          s.changeState(flag);
        } else { // It is a boundary
          s.changeState(true);
        }
      }
    }
  }

  void endContact(Contact cp) {
    //print("no collision\n");
    Fixture f1 = cp.getFixtureA();
    Fixture f2 = cp.getFixtureB();

    Body b1 = f1.getBody();
    Body b2 = f2.getBody();

    Object o1 = b1.getUserData();
    Object o2 = b2.getUserData();

    print("END CONTACT: \n");
    print(o1.getClass());
    print("\n");
    print(o2.getClass());
    print("\n");

    if (o1.getClass() == Sensor.class || o2.getClass() == Sensor.class ) {
      print("Collision b/w Sensor and Object ended\n");
      if (o1.getClass() == Sensor.class) {
        Sensor s = (Sensor) o1;
        s.changeState(false);
        print("state changed\n");
      } else if (o2.getClass() == Sensor.class) {
        Sensor s = (Sensor) o2;
        s.changeState(false);
        print("state changed\n");
      }
    }
  }

Here is my Sensor Class:

class Sensor {

  // Variables to keep track of
  Body body;
  int state;
  float id; 
  float x, y, w, h, angle_offset;

  Sensor(float x_, float y_, float w_, float h_, float a_, float id_) {
    /*
      Sensor(float x_, y_, w_, h_, a_, id_) - Sensor constructor
      float x, y - position of Sensor (and corresponding Car)
      float w, h - dimensions of the Sensor
      float a - angle relative to the car
      float id - ID# shared with its corresponding car
    */
    x = x_;
    y = y_;
    w = w_;
    h = h_;
    state = 0;
    id = id_;
    angle_offset = a_;
    makeBody();
  }

  void makeBody() {
    /*
      void makeBody() - define and create body
    */

    BodyDef bd = new BodyDef();
    bd.position.set(box2d.coordPixelsToWorld(x, y));
    bd.type = BodyType.DYNAMIC;
    bd.setAngle(angle_offset);


    body = box2d.createBody(bd);

    PolygonShape rect = new PolygonShape();
    float box2dW = box2d.scalarPixelsToWorld(w/2);
    float box2dH = box2d.scalarPixelsToWorld(h/2);
    Vec2 origin = new Vec2(0, -box2dH/2);
    rect.setAsBox(box2dW, box2dH, origin, 0);

    FixtureDef fd = new FixtureDef();
    fd.isSensor = true;
    fd.shape = rect;
    fd.friction = 0;
    fd.restitution = 0;
    body.createFixture(fd);
    body.setUserData(this);
  }

  void changeState(boolean flag){
    /*
      void changeState(boolean flag) - switches the Sensor on/off when contact has begun/ended
      boolean flag - on/off
    */

    if (flag){
      //print("collided\n");
      state = 1;
    } else{
      //print("no collision\n");
      state = 0;
    }
  }

  void update(Vec2 carPixelPos_, float angle){
    /*
      void update(Vec2 carPizelPos, float angle) - update the position of Sensor
      Vec2 carPixelPos - position of Car on the screen in pixels
      float angle - angle of the Car (in Processing CW direction is positive)
    */

    Vec2 pos = box2d.coordPixelsToWorld(carPixelPos_);
    body.setTransform(pos, angle + angle_offset);
  }

  void render() {
    /*
      void render() - display the Sensor
    */

    stroke(255, 255, 255, 50);
    Vec2 pos = box2d.getBodyPixelCoord(body);
    float angle = body.getAngle();
    pushMatrix();
    translate(pos.x, pos.y);
    rotate(-angle +PI/2); //front facing Sensor should be first
    rectMode(CORNER);
    rect(0, 0, w, h);
    popMatrix();
  }
}

Here is my Car class:

class Boundary{
  // Variables to keep track of
  Body body;
  float x, y, w, h;

  Boundary(float x_, float y_, float w_, float h_){
    /*
      Boundary(float x_, y_, w_, h_) - Boundary Constructor
      float x_, y_ - position in pixel coord
      float w_, y_ - width and height
    */
    x = x_;
    y = y_;
    w = w_;
    h = h_;
    makeBody();
  }

  void makeBody(){
    /*
      void makeBody() - define and create body
    */

    BodyDef bd = new BodyDef();
    bd.position.set(box2d.coordPixelsToWorld(x,y));
    bd.type = BodyType.DYNAMIC; // It should be STATIC but then the sensors wouldn't collide with it

    body = box2d.createBody(bd);

    PolygonShape rect = new PolygonShape();
    float box2dW = box2d.scalarPixelsToWorld(w/2);
    float box2dH = box2d.scalarPixelsToWorld(h/2);
    rect.setAsBox(box2dW, box2dH);

    FixtureDef fd = new FixtureDef();
    fd.shape = rect;
    fd.friction = 0;
    fd.restitution = 0;
    fd.density = 1000000000000.0; // High mass to make it approx STATIC
    body.createFixture(fd);
    body.setUserData(this);
  }

  void render(){
    /*
      void render() - display the Boundary
    */
    Vec2 pos = box2d.getBodyPixelCoord(body);
    fill(255);
    stroke(255);
    pushMatrix();
    translate(pos.x, pos.y);
    rectMode(CENTER);
    rect(0,0,w,h);
    popMatrix();
  }


}

I did not include my Neural Network Class and Matrix Class because I do not think it is necessary. The Car isn't colliding with anything but the Sensors are still active. I think endContact() isn't activating

0

There are 0 answers