// A function to rotate a vector
 public void rotateVector(PVector v, float theta) {
   float m = v.mag();
   float a = v.heading2D();
   a += theta;
   v.x = m * PApplet.cos(a);
   v.y = m * PApplet.sin(a);
 }
예제 #2
0
  public void draw(PApplet canvas, float scale) {
    PVector orient = PVector.fromAngle(orientation);
    orient.mult(scale * getRadius());
    float x = (float) position.x * scale;
    float y = (float) position.y * scale;
    float diameter = getRadius() * 2 * scale;

    canvas.fill(teamColor);
    canvas.stroke(0);
    canvas.ellipse(x, y, diameter, diameter);
    canvas.line(x, y, x + (float) orient.x, y + (float) orient.y);

    // Delegate Decoration to Robot
    float heading = orient.heading();
    float drawScale = 100f / scale * getRadius();

    canvas.translate(x, y);
    canvas.rotate(heading);
    canvas.scale(drawScale);
    // TODO: How to resolve scale, so that teams don't have to mind it also...
    decorateRobot(canvas);
    canvas.scale(1f / drawScale);
    canvas.rotate(-heading);
    canvas.translate(-x, -y);
  }
예제 #3
0
 // tällä funktiolla on huono nimi ja toteutuskin o ruma.
 // tämä on se juttu mikä tekee työt endpoints()ille.
 // jos tilepisteen reunalla toinen tile vain yhdessä suunnassa,
 // niin piste on yksinään. tähän tarttis jonkun kuvan selostamaan.
 PVector head(int x, int y, int z) {
   PVector pp = new PVector(x, y, z);
   PVector[] dirs = {
     new PVector(-1, 0, 0),
     new PVector(1, 0, 0),
     new PVector(0, -1, 0),
     new PVector(0, 1, 0),
     new PVector(0, 0, -1),
     new PVector(0, 0, 1),
   };
   int blkfound = -1;
   for (int i = 0; i < dirs.length; i++) {
     PVector p = dirs[i];
     PVector next = PVector.add(pp, p);
     if (next.x >= 0
         && next.x < size
         && next.y >= 0
         && next.y < size
         && next.z >= 0
         && next.z < size) {
       if (map[at(next)] != 0) {
         if (blkfound != -1) return null;
         blkfound = i;
       }
     }
   }
   if (blkfound != -1) return PVector.sub(pp, dirs[blkfound]);
   return null;
 }
예제 #4
0
    public void checkCollision(Paddle[] pads) {

      // check against walls
      if (y >= height || y < 0) {
        speed.y *= -1;
      }
      if (x >= width) {
        restart();
        paddles[1].addPoint();
        timer = 40;
      }
      if (x < 0) {
        restart();
        paddles[0].addPoint();
        timer = 40;
      }
      // check against paddles
      for (int i = 0; i < pads.length; i++) {
        if (x > pads[i].x - (pads[i].w / 2)
            && x < pads[i].x + (pads[i].w / 2)
            && y > pads[i].y - (pads[i].h / 2)
            && y < pads[i].y + (pads[i].h / 2)) {
          speed.x *= -1;

          float friction = 0.5f;
          println(pads[i].vel.y);
          speed.y += friction * pads[i].vel.y;
        }
      }
    }
예제 #5
0
    public void update(float newY) {
      y = newY;
      vel.x = x - px;
      vel.y = y - py;

      px = x;
      py = y;
    }
예제 #6
0
 // Change robot's front/back left/right speed
 public void setSpeed(float x, float y) {
   if (Float.isNaN(x) || Float.isNaN(y)) {
     // System.out.println("Excep: setSpeed "+x+" "+y);
     return;
   }
   targetSpeed.set(x, y);
   targetSpeed.limit(maxSpeed);
 }
 // Method to update location
 public void update() {
   // Update velocity
   vel.add(acc);
   // Limit speed
   vel.limit(maxspeed);
   loc.add(vel);
   // Reset accelertion to 0 each cycle
   acc.mult(0);
 }
예제 #8
0
    public PVector getSmoothedVelocity() {

      PVector total = new PVector(0, 0, 0);
      for (int i = 0; i < buffer.length; i++) {
        total.x += buffer[i].x;
        total.y += buffer[i].y;
        total.z += buffer[i].z;
      }

      PVector smthd =
          new PVector(total.x / buffer.length, total.y / buffer.length, total.z / buffer.length);
      return smthd;
    }
예제 #9
0
    public void calculateForces(ArrayList objects) {
      for (int i = 0; i < objects.size(); i++) {
        CelestialObject obj = (CelestialObject) objects.get(i);
        ArrayList forces = obj.getForces();
        float totalForceX = 0;
        float totalForceY = 0;

        for (int j = 0; j < forces.size(); j++) {
          totalForceX += ((PVector) forces.get(j)).x;
          totalForceY += ((PVector) forces.get(j)).y;
        }

        PVector newAccel = new PVector(totalForceX / obj.getMass(), totalForceY / obj.getMass());

        obj.setAcceleration(newAccel);
      }

      for (int i = 0; i < objects.size(); i++) {
        CelestialObject obj1 = (CelestialObject) objects.get(i);
        float forceX = 0;
        float forceY = 0;
        obj1.clearForces();

        if (obj1.getClass() == Star.class) {
          println(obj1.getVelocity());
          continue;
        }

        for (int j = 0; j < objects.size(); j++) {
          CelestialObject obj2 = (CelestialObject) objects.get(j);

          if (i == j) continue;

          PVector pvDistance = PVector.sub(obj2.getPosition(), obj1.getPosition());
          //    println("distance: x:" + pvDistance.x + " y:" + pvDistance.y);
          float distance = sqrt(sq(pvDistance.y) + sq(pvDistance.x));
          float angle = degrees(atan2(pvDistance.y, pvDistance.x));

          float force = (G * obj1.getMass() * obj2.getMass()) / sq(distance);
          forceX = force * cos(radians(angle));
          forceY = force * sin(radians(angle));
          //        println("FORCES on " + obj1.getName() + ":" + forceX + "," + forceY);
          obj1.addForce(new PVector(forceX, forceY));

          println();
        }
      }
    }
 // Function to update location
 public void update() {
   // As long as we aren't dragging the pendulum, let it swing!
   if (!dragging) {
     float G = 0.4f; // Arbitrary universal gravitational constant
     theta_acc =
         (-1 * G / r)
             * sin(
                 theta); // Calculate acceleration (see:
                         // http://www.myphysicslab.com/pendulum1.html)
     theta_vel += theta_acc; // Increment velocity
     theta_vel *= damping; // Arbitrary damping
     theta += theta_vel; // Increment theta
   }
   loc.set(r * sin(theta), r * cos(theta), 0); // Polar to cartesian conversion
   loc.add(origin); // Make sure the location is relative to the pendulum's origin
 }
예제 #11
0
 public void draw() {
   stroke(0, 0, 0);
   change = round(random(1, 10));
   if (change == 10) {
     direction = round(random(0, 8));
     point(pointX, pointY);
     point(pointX + 1, pointY + 1);
     point(pointX - 1, pointY - 1);
     point(pointX - 1, pointY + 1);
     point(pointX + 1, pointY - 1);
     stroke(200, 200, 200);
     point(pointX + 1, pointY);
     point(pointX - 1, pointY);
     point(pointX, pointY + 1);
     point(pointX, pointY - 1);
     stroke(0, 0, 0);
   }
   if (direction == 1) {
     speed.set(0, -1, 0);
   }
   if (direction == 2) {
     speed.set(1, 0, 0);
   }
   if (direction == 3) {
     speed.set(0, 1, 0);
   }
   if (direction == 4) {
     speed.set(-1, 0, 0);
   }
   if (direction == 5) {
     speed.set(-1, -1, 0);
   }
   if (direction == 6) {
     speed.set(1, 1, 0);
   }
   if (direction == 7) {
     speed.set(-1, 1, 0);
   }
   if (direction == 8) {
     speed.set(1, -1, 0);
   }
   if (pointX == width) {
     pointX = 1;
   }
   if (pointX == 0) {
     pointX = width - 1;
   }
   if (pointY == height) {
     pointY = 1;
   }
   if (pointY == 0) {
     pointY = height - 1;
   }
   pointX = pointX + speed.x;
   pointY = pointY + speed.y;
   point(pointX, pointY);
 }
 // Constructor initialize all values
 Boid(PVector l, float ms, float mf) {
   loc = l.get();
   r = 4.0f;
   maxspeed = ms;
   maxforce = mf;
   acc = new PVector(0, 0);
   vel = new PVector(maxspeed, 0);
 }
예제 #13
0
  /*
  	Perform Simulation
  */
  public void simulate(float dt) {
    // Accelerate angular speed
    float requiredSpeed = targetAngularSpeed - angularSpeed;
    float angularAccel = requiredSpeed / dt;
    angularAccel = PApplet.constrain(angularAccel, -maxAngularAccel, maxAngularAccel);

    // Limit Angular speed
    angularSpeed += angularAccel * dt;
    angularSpeed = PApplet.constrain(angularSpeed, -maxAngularSpeed, maxAngularSpeed);

    // Orientation Simulation
    orientation += angularSpeed * dt;

    // Position simulation
    PVector worldRequiredSpeed = targetSpeed.get();
    worldRequiredSpeed.rotate(orientation);
    worldRequiredSpeed.sub(speed);

    // PVector worldRequiredSpeed = worldTargetSpeed.get();
    float dSpeed = worldRequiredSpeed.mag();
    float dAcell = dSpeed / dt;
    float dForce = Math.min(dAcell * getMass(), motorForce);

    worldRequiredSpeed.normalize();
    worldRequiredSpeed.mult(dForce);
    force.add(worldRequiredSpeed);

    super.simulate(dt);
  }
예제 #14
0
    public void updateAll(PVector newPos) {
      // update current joint position
      currentPos = newPos;
      // map velocity to depth image size - I can't find how to get the depth of this context
      velocity.x = map(newPos.x - prevPos.x, -context.depthWidth(), context.depthWidth(), -1, 1);
      velocity.y = map(newPos.y - prevPos.y, -context.depthHeight(), context.depthHeight(), -1, 1);
      velocity.z = newPos.z - prevPos.z;

      // overwrite the circular buffer
      buffer[bufferIndex] = velocity;
      // and incrememnt the buffer index
      bufferIndex++;
      if (bufferIndex >= buffer.length) {
        bufferIndex = 0;
      }
      prevPos = newPos;
      smoothedVelocity = getSmoothedVelocity();
    }
예제 #15
0
파일: GUI.java 프로젝트: BNHeadrick/AFM-GUI
 /**
  * Given two points p1, p2 and a line passing through a,b check whether p1 and p2 are on the
  * same side of the line. In our case a,b are characters and p1, p2 are cameras
  */
 public boolean isInSameHalfPlane(PVector p1, PVector p2, PVector a, PVector b) {
   PVector copyP1 = new PVector(p1.x, p1.y, p1.z);
   PVector copyP2 = new PVector(p2.x, p2.y, p2.z);
   PVector copyA = new PVector(a.x, a.y, a.z);
   PVector copyB = new PVector(b.x, b.y, b.z);
   copyB.sub(copyA);
   copyP1.sub(copyA);
   copyP2.sub(copyA);
   PVector p1a = copyB.cross(copyP1);
   PVector p2a = copyB.cross(copyP2);
   if (p1a.dot(p2a) > 0) {
     return true;
   }
   return false;
 }
 public void drag() {
   // If we are draging the ball, we calculate the angle between the
   // pendulum origin and mouse location
   // we assign that angle to the pendulum
   if (dragging) {
     PVector diff =
         PVector.sub(origin, new PVector(mouseX, mouseY)); // Difference between 2 points
     theta = atan2(-1 * diff.y, diff.x) - radians(90); // Angle relative to vertical axis
   }
 }
예제 #17
0
파일: box.java 프로젝트: newshorts/pingpong
    public void wallCollisions() {

      // x
      if (x > (bounds.x / 2)) {
        velocity.x *= -1;
        //      velocity.x *= damping;
      }

      if (x < -(bounds.x / 2)) {
        velocity.x *= -1;
        //      velocity.x *= damping;
      }

      // y
      if (g) {
        velocity.y += gravity;
        if (y > bounds.y / 2) {
          velocity.y *= -0.95f;
          y = bounds.y / 2;
        }
      } else {
        if (y > (bounds.y / 2)) {
          velocity.y *= -1;
          velocity.y *= damping;
        }

        if (y < -(bounds.y / 2)) {
          velocity.y *= -1;
          velocity.y *= damping;
        }
      }

      // z
      if (z > (bounds.z / 2)) {
        velocity.z *= -1;
        //      velocity.z *= damping;
      }

      if (z < -(bounds.z / 2)) {
        velocity.z *= -1;
        //      velocity.z *= damping;
      }
    }
예제 #18
0
  public void setState(PVector position, float orientation, boolean resetAll) {
    this.position = position.get();
    this.orientation = orientation;

    if (resetAll) {
      this.force = new PVector();
      this.accel = new PVector();
      this.speed = new PVector();
    }
  }
예제 #19
0
파일: GUI.java 프로젝트: BNHeadrick/AFM-GUI
 public boolean isInView(PVector vect) {
   PVector z = getZAxis();
   PVector v1 = PVector.sub(vect, getTranslation());
   float dotP = PVector.dot(v1, z) / (z.mag() * v1.mag());
   if ((acos(dotP) * 180 / PI) <= fov) {
     return true;
   }
   return false;
 }
예제 #20
0
 // maailmassa on välttämättä myös pisteitä joissa ei ole haaroja, laitetaan niiden päihin
 // kolikkoja
 // akseli ulospäin siitä suorasta osasta
 // paluuarvot paikka1, akseli1, ...
 ArrayList<PVector> endpoints() {
   ArrayList<PVector> pts = new ArrayList<PVector>();
   for (int z = 0; z < size; z += space + 1) {
     for (int y = 0; y < size; y += space + 1) {
       for (int x = 0; x < size; x += space + 1) {
         PVector pos = head(x, y, z);
         if (pos != null) {
           pts.add(pos);
           pts.add(PVector.sub(pos, new PVector(x, y, z)));
         }
       }
     }
   }
   return pts;
 }
    // This constructor could be improved to allow a greater variety of pendulums
    Pendulum(PVector origin_, float r_) {
      // Fill all variables
      origin = origin_.get();
      r = r_;
      theta = 0.0f;

      // calculate the location of the ball using polar to cartesian conversion
      float x = r * sin(theta);
      float y = r * cos(theta);
      loc = new PVector(origin.x + x, origin.y + y);
      theta_vel = 0.0f;
      theta_acc = 0.0f;
      damping = 0.995f; // Arbitrary damping
      ballr = 16.0f; // Arbitrary ball radius
    }
    public void render() {
      // Draw a triangle rotated in the direction of velocity
      float theta = vel.heading2D() + PApplet.radians(90);
      fill(100);
      stroke(0);
      pushMatrix();
      translate(loc.x, loc.y);
      rotate(theta);
      beginShape(PConstants.TRIANGLES);
      vertex(0, -r * 2);
      vertex(-r, r * 2);
      vertex(r, r * 2);
      endShape();

      if (debug) {
        stroke(50);
        line(0, 0, 0, -sight);
      }
      popMatrix();
    }
예제 #23
0
  public void mouseDragged() {
    if (paused) {
      ArrayList objects = timeline.getStatefulObjects();
      for (int i = 0; i < objects.size(); i++) {
        CelestialObject obj = (CelestialObject) objects.get(i);
        if (obj.isMouseOver()) {
          dragging = true;
          PVector pos = obj.getPosition();
          pos.x = mouseX;
          pos.y = mouseY;

          timeline.reset();
          timeline.setCurrentState(objects);
          sliderTimeline.setValue(0);
          break;
        }
      }
    }
  }
예제 #24
0
 public void setAcceleration(PVector acceleration) {
   this.acceleration = acceleration;
   velocity.add(acceleration);
   position.add(velocity);
 }
예제 #25
0
파일: GUI.java 프로젝트: BNHeadrick/AFM-GUI
    public void checkThirtyDegreeRule(
        ArrayList<Cam> cameras, ArrayList<Character> characters, int selectedIdx) {
      if (cameras == null || cameras.isEmpty()) {
        println("No cameras in the scene!");
      }

      if (characters.size() != 2) {
        println("Only two characters supported for now");
        // TODO (sanjeet): Hack! Fix this once more characters are allowed
      }

      Cam selectedCamera = cameras.get(selectedIdx);

      // TODO The characters.get results in a runtime error because there aren't currently any
      // characters allocated in the input file.
      Character ch1 = characters.get(0);
      Character ch2 = characters.get(1);

      // Obtaining (x,y,z) for characters and selected camera
      PVector ch1Location = ch1.getTranslation();
      PVector ch2Location = ch2.getTranslation();
      PVector selectedCameraLocation = selectedCamera.getTranslation();

      PVector cameraPoint = new PVector();
      cameraPoint.add(selectedCameraLocation);
      for (int i = 0; i < 100; i++) {
        cameraPoint.add(selectedCamera.getZAxis());
      }
      PVector intersection =
          getTwoLinesIntersection(
              new PVector(ch1Location.x, ch1Location.z),
              new PVector(ch2Location.x, ch2Location.z),
              new PVector(selectedCameraLocation.x, selectedCameraLocation.z),
              new PVector(cameraPoint.x, cameraPoint.z));

      PVector diff = PVector.sub(selectedCameraLocation, intersection);
      diff.normalize();
      FloatBuffer fb = selectedCamera.modelViewMatrix;
      float[] mat = fb.array();
      float[] fbMatrix = new float[mat.length];
      for (int i = 0; i < fbMatrix.length; i++) {
        fbMatrix[i] = mat[i];
      }
      fbMatrix[0] = -diff.x;
      fbMatrix[1] = diff.y;
      fbMatrix[2] = -diff.z;
      fbMatrix[9] = diff.x;
      fbMatrix[10] = diff.y;
      fbMatrix[11] = diff.z;
      fbMatrix[13] = intersection.x;
      fbMatrix[14] = intersection.y;
      fbMatrix[15] = intersection.z;
      PMatrix3D matrix = new PMatrix3D();
      matrix.set(fbMatrix);
      matrix.transpose();
      pushMatrix();
      applyMatrix(matrix);
      rotateY(radians(30));
      line(0, 0, 0, 0, 0, 1000);
      rotateY(radians(-2 * 30));
      line(0, 0, 0, 0, 0, 1000);
      popMatrix();

      for (int i = 0; i < cameras.size(); i++) {
        if (i == selectedIdx) {
          continue;
        }

        if (!cameras.get(i).isInView(ch1Location) && !cameras.get(i).isInView(ch2Location)) {
          continue;
        }
        PVector currCamLocation = cameras.get(i).getTranslation();
        PVector vect1 = PVector.sub(currCamLocation, intersection);
        PVector vect2 = PVector.sub(selectedCameraLocation, intersection);
        float dotP = vect1.dot(vect2) / (vect1.mag() * vect2.mag());
        if (acos(dotP) <= PI / 6) {
          cameras.get(i).setColor(255, 0, 0);
        } else {
          cameras.get(i).setColor(0, 0, 255);
        }
      }
    }
    public void avoid(ArrayList obstacles) {

      // Make a vector that will be the position of the object
      // relative to the Boid rotated in the direction of boid's velocity
      PVector closestRotated = new PVector(sight + 1, sight + 1);
      float closestDistance = 99999;
      Obstacle avoid = null;

      // Let's look at each obstacle
      for (int i = 0; i < obstacles.size(); i++) {
        Obstacle o = (Obstacle) obstacles.get(i);

        float d = PVector.dist(loc, o.loc);
        PVector dir = vel.get();
        dir.normalize();
        PVector diff = PVector.sub(o.loc, loc);

        // Now we use the dot product to rotate the vector that points from boid to obstacle
        // Velocity is the new x-axis
        PVector rotated = new PVector(diff.dot(dir), diff.dot(getNormal(dir)));

        // Is the obstacle in our path?
        if (PApplet.abs(rotated.y) < (o.radius + r)) {
          // Is it the closest obstacle?
          if ((rotated.x > 0) && (rotated.x < closestRotated.x)) {
            closestRotated = rotated;
            avoid = o;
          }
        }
      }

      // Can we actually see the closest one?
      if (PApplet.abs(closestRotated.x) < sight) {

        // The desired vector should point away from the obstacle
        // The closer to the obstacle, the more it should steer
        PVector desired =
            new PVector(closestRotated.x, -closestRotated.y * sight / closestRotated.x);
        desired.normalize();
        desired.mult(closestDistance);
        desired.limit(maxspeed);
        // Rotate back to the regular coordinate system
        rotateVector(desired, vel.heading2D());

        // Draw some debugging stuff
        if (debug) {
          stroke(0);
          line(loc.x, loc.y, loc.x + desired.x * 10, loc.y + desired.y * 10);
          avoid.highlight(true);
        }

        // Apply Reynolds steering rules
        desired.sub(vel);
        desired.limit(maxforce);
        acc.add(desired);
      }
    }
 // Wraparound
 public void borders() {
   if (loc.x < -r) loc.x = width + r;
   if (loc.y < -r) loc.y = height + r;
   if (loc.x > width + r) loc.x = -r;
   if (loc.y > height + r) loc.y = -r;
 }
예제 #28
0
 // deep copy
 public PositionRotation clone() {
   return new PositionRotation(position.get(), rotation.get());
 }
예제 #29
0
 // Change robot forward/backward speed only
 public void setSpeed(float x) {
   targetSpeed.set(x, 0);
   targetSpeed.limit(maxSpeed);
 }