List of usage examples for org.apache.commons.math3.geometry.euclidean.twod Vector2D subtract
public Vector2D subtract(Vector<Euclidean2D> p)
From source file:edu.unc.cs.gamma.rvo.RVOMath.java
/** * Computes the signed distance from a line connecting the specified points * to a specified point.//from w w w.ja va 2 s .c o m * * @param point1 The first point on the line. * @param point2 The second point on the line. * @param point3 The point to which the signed distance is to be calculated. * @return Positive when the point3 lies to the left of the line passing * through point1 and point2. */ static double leftOf(Vector2D point1, Vector2D point2, Vector2D point3) { return det(point1.subtract(point3), point2.subtract(point1)); }
From source file:ch.zweivelo.renderer.simple.cameras.PinholeCamera.java
@Override public Ray createRayFor(final Vector2D uvPoint) { Vector2D transformedUV = uvPoint.subtract(new Vector2D(.5d, .5d)); return null;/*from ww w . j a v a 2s. c o m*/ }
From source file:edu.stanford.cfuller.imageanalysistools.filter.VoronoiFilter.java
protected Vector2D projectPointOntoVector(Vector2D origin, Vector2D pointToProject, Vector2D pointOnVector) { Vector2D onto = pointOnVector.subtract(origin).normalize(); Vector2D toProj = pointToProject.subtract(origin); Vector2D projected = origin.add(onto.scalarMultiply(onto.dotProduct(toProj))); return projected; }
From source file:edu.stanford.cfuller.imageanalysistools.util.VoronoiDiagram.java
protected Hyperplane<Euclidean2D> constructBisectingHyperplane(Vector2D firstPoint, Vector2D secondPoint) { Vector2D middlePoint = firstPoint.add(secondPoint).scalarMultiply(0.5); Vector2D difference = secondPoint.subtract(firstPoint); double angle = Math.atan2(difference.getY(), difference.getX()); angle -= Math.PI / 2.0; // rotate 90 degrees Line bisector = new Line(middlePoint, angle); return bisector; }
From source file:haxball.networking.ServerMainLoop.java
@Override public void run() { resetField();//w w w . j ava 2 s. co m byte score0 = 0, score1 = 0; while (!stopped) { // Move players for (Player player : players) { byte input = player.getLastInput(); float vx = 0; float vy = 0; if ((input & 0b00_00_00_01) != 0) { vy += speed; } if ((input & 0b00_00_00_10) != 0) { vx -= speed; } if ((input & 0b00_00_01_00) != 0) { vy -= speed; } if ((input & 0b00_00_10_00) != 0) { vx += speed; } if ((input & 0b00_01_00_00) != 0) { player.setShooting(true); } else { player.setShooting(false); } Vector2D v = new Vector2D(vx, vy); if (v.distance(Vector2D.ZERO) > 0) { v = v.normalize().scalarMultiply(speed); } player.velocity = player.velocity.add(v.subtract(player.velocity).scalarMultiply(acceleration)); player.position = player.position.add(player.velocity); } // Move ball ball.velocity = ball.velocity.scalarMultiply(1 - friction); ball.position = ball.position.add(ball.velocity); // Check for collisions for (Player p : players) { // collisions between players for (Player p0 : players) { if (!p.equals(p0)) { p.uncollide(p0); } } p.uncollide(ball); p.setInsideMap(true); if (p.isShooting() && p.position.distance(ball.position) < (p.getRadius() + ball.getRadius()) * 1.4f) { Vector2D norm = ball.position.subtract(p.position).normalize(); ball.velocity = ball.velocity.add(norm.scalarMultiply(shootingPower)); } } int result; if ((result = ball.setInsideMap(false)) >= 0) { if (result == 0) { score0++; } else { score1++; } resetField(); } // send position to every connection for (ConnectionHandler handler : connectionHandlers) { handler.writeState(ball, score0, score1, players); } // sleep try { Thread.sleep(10); } catch (InterruptedException e) { e.printStackTrace(); } } // while !stopped }
From source file:de.thkwalter.et.schlupfbezifferung.SchlupfbezifferungController.java
/** * Diese Methode berechnet den Drehpunkt der Schlupfgerade (in A). Der Drehpunkt liegt auf der gedrehten Ortskurve * senkrecht unter dem Mittelpunkt./*from w w w . java 2s . co m*/ * * @return Der Drehpunkt der Schlupfgerade (in A) */ private Vector2D drehpunktSchlupfgeradeBerechnen() { // Der Mittelpunkt der gedrehten Ortskurve (in A) wird gelesen. Vector2D mittelpunktOrtskurve = this.schlupfbezifferungModell.getOrtskurve().getMittelpunktOrtskurve(); // Der Radius der Ortskurve (in A) wird gelesen. double r = this.schlupfbezifferungModell.getOrtskurve().getRadiusOrtskurve(); // Der Drehpunkt der Schupfgerade (in A) wird berechnet. Vector2D drehpunkt = mittelpunktOrtskurve.subtract(new Vector2D(0.0, r)); // Der Drehpunkt der Schlupfgerade (in A) wird zurckgegeben. return drehpunkt; }
From source file:edu.unc.cs.gamma.rvo.Agent.java
/** * Computes the new velocity of this agent. */// ww w .ja va 2s . com void computeNewVelocity() { lines.clear(); final double invTimeHorizonObstacle = 1.0 / timeHorizonObstacles; // Create obstacle ORCA lines. for (final Pair<Double, Obstacle> obstacleNeighbor : obstacleNeighbors) { Obstacle obstacle1 = obstacleNeighbor.getSecond(); Obstacle obstacle2 = obstacle1.next; final Vector2D relativePosition1 = obstacle1.point.subtract(position); final Vector2D relativePosition2 = obstacle2.point.subtract(position); // Check if velocity obstacle of obstacle is already taken care of // by previously constructed obstacle ORCA lines. boolean alreadyCovered = false; for (final Line orcaLine : lines) { if (RVOMath.det(relativePosition1.scalarMultiply(invTimeHorizonObstacle).subtract(orcaLine.point), orcaLine.direction) - invTimeHorizonObstacle * radius >= -RVOMath.EPSILON && RVOMath .det(relativePosition2.scalarMultiply(invTimeHorizonObstacle) .subtract(orcaLine.point), orcaLine.direction) - invTimeHorizonObstacle * radius >= -RVOMath.EPSILON) { alreadyCovered = true; break; } } if (alreadyCovered) { continue; } // Not yet covered. Check for collisions. final double distanceSq1 = relativePosition1.getNormSq(); final double distanceSq2 = relativePosition2.getNormSq(); final double radiusSq = radius * radius; final Vector2D obstacleVector = obstacle2.point.subtract(obstacle1.point); final double s = -relativePosition1.dotProduct(obstacleVector) / obstacleVector.getNormSq(); final double distanceSqLine = relativePosition1.add(s, obstacleVector).getNormSq(); if (s < 0.0 && distanceSq1 <= radiusSq) { // Collision with left vertex. Ignore if non-convex. if (obstacle1.convex) { final Vector2D direction = new Vector2D(-relativePosition1.getY(), relativePosition1.getX()) .normalize(); lines.add(new Line(Vector2D.ZERO, direction)); } continue; } if (s > 1.0 && distanceSq2 <= radiusSq) { // Collision with right vertex. Ignore if non-convex or if it // will be taken care of by neighboring obstacle. if (obstacle2.convex && RVOMath.det(relativePosition2, obstacle2.direction) >= 0.0) { final Vector2D direction = new Vector2D(-relativePosition2.getY(), relativePosition2.getX()) .normalize(); lines.add(new Line(Vector2D.ZERO, direction)); } continue; } if (s >= 0.0 && s < 1.0 && distanceSqLine <= radiusSq) { // Collision with obstacle segment. final Vector2D direction = obstacle1.direction.negate(); lines.add(new Line(Vector2D.ZERO, direction)); continue; } // No collision. Compute legs. When obliquely viewed, both legs can // come from a single vertex. Legs extend cut-off line when // non-convex vertex. Vector2D leftLegDirection; Vector2D rightLegDirection; if (s < 0.0 && distanceSqLine <= radiusSq) { // Obstacle viewed obliquely so that left vertex defines // velocity obstacle. if (!obstacle1.convex) { // Ignore obstacle. continue; } obstacle2 = obstacle1; final double leg1 = FastMath.sqrt(distanceSq1 - radiusSq); leftLegDirection = new Vector2D(relativePosition1.getX() * leg1 - relativePosition1.getY() * radius, relativePosition1.getX() * radius + relativePosition1.getY() * leg1) .scalarMultiply(1.0 / distanceSq1); rightLegDirection = new Vector2D( relativePosition1.getX() * leg1 + relativePosition1.getY() * radius, -relativePosition1.getX() * radius + relativePosition1.getY() * leg1) .scalarMultiply(1.0 / distanceSq1); } else if (s > 1.0 && distanceSqLine <= radiusSq) { // Obstacle viewed obliquely so that right vertex defines // velocity obstacle. if (!obstacle2.convex) { // Ignore obstacle. continue; } obstacle1 = obstacle2; final double leg2 = FastMath.sqrt(distanceSq2 - radiusSq); leftLegDirection = new Vector2D(relativePosition2.getX() * leg2 - relativePosition2.getY() * radius, relativePosition2.getX() * radius + relativePosition2.getY() * leg2) .scalarMultiply(1.0 / distanceSq2); rightLegDirection = new Vector2D( relativePosition2.getX() * leg2 + relativePosition2.getY() * radius, -relativePosition2.getX() * radius + relativePosition2.getY() * leg2) .scalarMultiply(1.0 / distanceSq2); } else { // Usual situation. if (obstacle1.convex) { final double leg1 = FastMath.sqrt(distanceSq1 - radiusSq); leftLegDirection = new Vector2D( relativePosition1.getX() * leg1 - relativePosition1.getY() * radius, relativePosition1.getX() * radius + relativePosition1.getY() * leg1) .scalarMultiply(1.0 / distanceSq1); } else { // Left vertex non-convex; left leg extends cut-off line. leftLegDirection = obstacle1.direction.negate(); } if (obstacle2.convex) { final double leg2 = FastMath.sqrt(distanceSq2 - radiusSq); rightLegDirection = new Vector2D( relativePosition2.getX() * leg2 + relativePosition2.getY() * radius, -relativePosition2.getX() * radius + relativePosition2.getY() * leg2) .scalarMultiply(1.0 / distanceSq2); } else { // Right vertex non-convex; right leg extends cut-off line. rightLegDirection = obstacle1.direction; } } // Legs can never point into neighboring edge when convex vertex, // take cut-off line of neighboring edge instead. If velocity // projected on "foreign" leg, no constraint is added. boolean leftLegForeign = false; boolean rightLegForeign = false; if (obstacle1.convex && RVOMath.det(leftLegDirection, obstacle1.previous.direction.negate()) >= 0.0) { // Left leg points into obstacle. leftLegDirection = obstacle1.previous.direction.negate(); leftLegForeign = true; } if (obstacle2.convex && RVOMath.det(rightLegDirection, obstacle2.direction) <= 0.0) { // Right leg points into obstacle. rightLegDirection = obstacle2.direction; rightLegForeign = true; } // Compute cut-off centers. final Vector2D leftCutOff = obstacle1.point.subtract(position).scalarMultiply(invTimeHorizonObstacle); final Vector2D rightCutOff = obstacle2.point.subtract(position).scalarMultiply(invTimeHorizonObstacle); final Vector2D cutOffVector = rightCutOff.subtract(leftCutOff); // Project current velocity on velocity obstacle. // Check if current velocity is projected on cutoff circles. final double t = obstacle1 == obstacle2 ? 0.5 : velocity.subtract(leftCutOff).dotProduct(cutOffVector) / cutOffVector.getNormSq(); final double tLeft = velocity.subtract(leftCutOff).dotProduct(leftLegDirection); final double tRight = velocity.subtract(rightCutOff).dotProduct(rightLegDirection); if (t < 0.0 && tLeft < 0.0 || obstacle1 == obstacle2 && tLeft < 0.0 && tRight < 0.0) { // Project on left cut-off circle. final Vector2D unitW = velocity.subtract(leftCutOff).normalize(); final Vector2D direction = new Vector2D(unitW.getY(), -unitW.getX()); final Vector2D point = leftCutOff.add(radius * invTimeHorizonObstacle, unitW); lines.add(new Line(point, direction)); continue; } if (t > 1.0 && tRight < 0.0) { // Project on right cut-off circle. final Vector2D unitW = velocity.subtract(rightCutOff).normalize(); final Vector2D direction = new Vector2D(unitW.getY(), -unitW.getX()); final Vector2D point = rightCutOff.add(radius * invTimeHorizonObstacle, unitW); lines.add(new Line(point, direction)); continue; } // Project on left leg, right leg, or cut-off line, whichever is // closest to velocity. final double distanceSqCutOff = t < 0.0 || t > 1.0 || obstacle1 == obstacle2 ? Double.POSITIVE_INFINITY : velocity.distanceSq(leftCutOff.add(cutOffVector.scalarMultiply(t))); final double distanceSqLeft = tLeft < 0.0 ? Double.POSITIVE_INFINITY : velocity.distanceSq(leftCutOff.add(leftLegDirection.scalarMultiply(tLeft))); final double distanceSqRight = tRight < 0.0 ? Double.POSITIVE_INFINITY : velocity.distanceSq(rightCutOff.add(rightLegDirection.scalarMultiply(tRight))); if (distanceSqCutOff <= distanceSqLeft && distanceSqCutOff <= distanceSqRight) { // Project on cut-off line. final Vector2D direction = obstacle1.direction.negate(); final Vector2D point = leftCutOff.add(radius * invTimeHorizonObstacle, new Vector2D(-direction.getY(), direction.getX())); lines.add(new Line(point, direction)); continue; } if (distanceSqLeft <= distanceSqRight) { // Project on left leg. if (leftLegForeign) { continue; } final Vector2D point = leftCutOff.add(radius * invTimeHorizonObstacle, new Vector2D(-leftLegDirection.getY(), leftLegDirection.getX())); lines.add(new Line(point, leftLegDirection)); continue; } // Project on right leg. if (rightLegForeign) { continue; } final Vector2D direction = rightLegDirection.negate(); final Vector2D point = rightCutOff.add(radius * invTimeHorizonObstacle, new Vector2D(-direction.getY(), direction.getX())); lines.add(new Line(point, direction)); } final int numObstacleLines = lines.size(); final double invTimeHorizon = 1.0 / timeHorizonAgents; // Create agent ORCA lines. for (final Pair<Double, Agent> agentNeighbor : agentNeighbors) { final Agent other = agentNeighbor.getSecond(); final Vector2D relativePosition = other.position.subtract(position); final Vector2D relativeVelocity = velocity.subtract(other.velocity); final double distanceSq = relativePosition.getNormSq(); final double combinedRadius = radius + other.radius; final double combinedRadiusSq = combinedRadius * combinedRadius; final Vector2D direction; final Vector2D u; if (distanceSq > combinedRadiusSq) { // No collision. final Vector2D w = relativeVelocity.subtract(invTimeHorizon, relativePosition); // Vector from cutoff center to relative velocity. final double wLengthSq = w.getNormSq(); final double dotProduct1 = w.dotProduct(relativePosition); if (dotProduct1 < 0.0 && dotProduct1 * dotProduct1 > combinedRadiusSq * wLengthSq) { // Project on cut-off circle. final double wLength = FastMath.sqrt(wLengthSq); final Vector2D unitW = w.scalarMultiply(1.0 / wLength); direction = new Vector2D(unitW.getY(), -unitW.getX()); u = unitW.scalarMultiply(combinedRadius * invTimeHorizon - wLength); } else { // Project on legs. final double leg = FastMath.sqrt(distanceSq - combinedRadiusSq); if (RVOMath.det(relativePosition, w) > 0.0) { // Project on left leg. direction = new Vector2D( relativePosition.getX() * leg - relativePosition.getY() * combinedRadius, relativePosition.getX() * combinedRadius + relativePosition.getY() * leg) .scalarMultiply(1.0 / distanceSq); } else { // Project on right leg. direction = new Vector2D( relativePosition.getX() * leg + relativePosition.getY() * combinedRadius, -relativePosition.getX() * combinedRadius + relativePosition.getY() * leg) .scalarMultiply(-1.0 / distanceSq); } final double dotProduct2 = relativeVelocity.dotProduct(direction); u = direction.scalarMultiply(dotProduct2).subtract(relativeVelocity); } } else { // Collision. Project on cut-off circle of time timeStep. final double invTimeStep = 1.0 / Simulator.instance.timeStep; // Vector from cutoff center to relative velocity. final Vector2D w = relativeVelocity.subtract(invTimeStep, relativePosition); final double wLength = w.getNorm(); final Vector2D unitW = w.scalarMultiply(1.0 / wLength); direction = new Vector2D(unitW.getY(), -unitW.getX()); u = unitW.scalarMultiply(combinedRadius * invTimeStep - wLength); } final Vector2D point = velocity.add(0.5, u); lines.add(new Line(point, direction)); } final int lineFail = linearProgram2(lines, preferredVelocity, false); if (lineFail < lines.size()) { linearProgram3(numObstacleLines, lineFail); } }
From source file:edu.unc.cs.gamma.rvo.Agent.java
/** * Solves a one-dimensional linear program on a specified line subject to * linear constraints defined by lines and a circular constraint. * * @param lines Lines defining the linear constraints. * @param lineNo The specified line constraint. * @param optimizationVelocity The optimization velocity. * @param optimizeDirection True if the direction should be optimized. * @return True if successful./* w w w.ja v a 2 s. c om*/ */ private boolean linearProgram1(List<Line> lines, int lineNo, Vector2D optimizationVelocity, boolean optimizeDirection) { final double dotProduct = lines.get(lineNo).point.dotProduct(lines.get(lineNo).direction); final double discriminant = dotProduct * dotProduct + maxSpeed * maxSpeed - lines.get(lineNo).point.getNormSq(); if (discriminant < 0.0) { // Max speed circle fully invalidates line lineNo. return false; } final double sqrtDiscriminant = FastMath.sqrt(discriminant); double tLeft = -sqrtDiscriminant - dotProduct; double tRight = sqrtDiscriminant - dotProduct; for (int i = 0; i < lineNo; i++) { final double denominator = RVOMath.det(lines.get(lineNo).direction, lines.get(i).direction); final double numerator = RVOMath.det(lines.get(i).direction, lines.get(lineNo).point.subtract(lines.get(i).point)); if (FastMath.abs(denominator) <= RVOMath.EPSILON) { // Lines lineNo and i are (almost) parallel. if (numerator < 0.0) { return false; } continue; } final double t = numerator / denominator; if (denominator >= 0.0) { // Line i bounds line lineNo on the right. tRight = FastMath.min(tRight, t); } else { // Line i bounds line lineNo on the left. tLeft = FastMath.max(tLeft, t); } if (tLeft > tRight) { return false; } } if (optimizeDirection) { // Optimize direction. if (optimizationVelocity.dotProduct(lines.get(lineNo).direction) > 0.0) { // Take right extreme. newVelocity = lines.get(lineNo).point.add(tRight, lines.get(lineNo).direction); } else { // Take left extreme. newVelocity = lines.get(lineNo).point.add(tLeft, lines.get(lineNo).direction); } } else { // Optimize closest point. final double t = lines.get(lineNo).direction .dotProduct(optimizationVelocity.subtract(lines.get(lineNo).point)); if (t < tLeft) { newVelocity = lines.get(lineNo).point.add(tLeft, lines.get(lineNo).direction); } else if (t > tRight) { newVelocity = lines.get(lineNo).point.add(tRight, lines.get(lineNo).direction); } else { newVelocity = lines.get(lineNo).point.add(t, lines.get(lineNo).direction); } } return true; }
From source file:org.evors.rs.ui.sandpit.SandPitCamera.java
public Vector2D convertScreenToWorldCoords(Vector2D screen) { Vector2D halfWindow = getHalfWindowSize(); return new Vector2D(1 / getScale(), screen.subtract(halfWindow)).subtract(currentPosWorldCoord); }
From source file:org.evors.rs.ui.sandpit.SandPitCanvas.java
public SandPitCanvas() { camera = new SandPitCamera(Vector2D.ZERO, Vector2D.ZERO, 50); grid = new Grid(camera); MouseAdapter mouseAdapter = new MouseAdapter() { private Vector2D prevCoord = Vector2D.NaN; @Override// w w w. j a v a 2 s . co m public void mouseDragged(MouseEvent me) { Vector2D newCoord = getCamera().convertScreenToWorldCoords(new Vector2D(me.getX(), me.getY())); if (prevCoord == Vector2D.NaN) { prevCoord = newCoord; } else { Vector2D sub = newCoord.subtract(prevCoord); getCamera().move(sub); draw(); } } @Override public void mouseReleased(MouseEvent me) { prevCoord = Vector2D.NaN; } @Override public void mouseWheelMoved(MouseWheelEvent mwe) { getCamera().changeScale(-0.5 * mwe.getUnitsToScroll()); draw(); } }; addMouseListener(mouseAdapter); addMouseMotionListener(mouseAdapter); addMouseWheelListener(mouseAdapter); }