List of usage examples for org.apache.commons.math3.geometry.euclidean.twod Vector2D scalarMultiply
public Vector2D scalarMultiply(double a)
From source file:edu.unc.cs.gamma.rvo.Circle.java
private void setPreferredVelocities() { // Set the preferred velocity to be a vector of unit magnitude (speed) // in the direction of the goal. for (int agentNo = 0; agentNo < Simulator.instance.getNumAgents(); agentNo++) { Vector2D goalVector = goals.get(agentNo).subtract(Simulator.instance.getAgentPosition(agentNo)); final double lengthSq = goalVector.getNormSq(); if (lengthSq > 1.0) { goalVector = goalVector.scalarMultiply(1.0 / FastMath.sqrt(lengthSq)); }/*from ww w .j a v a2 s . c o m*/ Simulator.instance.setAgentPreferredVelocity(agentNo, goalVector); } }
From source file:haxball.util.MapObject.java
public void uncollide(MapObject other) { if (!other.isMoveable()) { if (!isMoveable()) { return; }/*w w w .ja v a 2 s . c om*/ other.uncollide(this); return; } float dist = (float) position.distance(other.position); float mindist = getCollisionDistance(other); if (dist <= mindist) { float overlap = mindist - dist; Vector2D direction = other.position.subtract(position).normalize(); Vector2D sum = other.velocity.add(velocity); if (!isMoveable()) { other.position = other.position.add(direction.scalarMultiply(overlap)); other.velocity = other.velocity.add(sum.scalarMultiply(0.5)); } else { other.position = other.position.add(direction.scalarMultiply(overlap / 2)); other.velocity = other.velocity.add(sum.scalarMultiply(0.5)); direction = direction.negate(); sum = sum.negate(); position = position.add(direction.scalarMultiply(overlap / 2)); velocity = velocity.add(sum.scalarMultiply(0.5)); } } }
From source file:edu.unc.cs.gamma.rvo.Blocks.java
private void setPreferredVelocities() { // Set the preferred velocity to be a vector of unit magnitude (speed) // in the direction of the goal. for (int agentNo = 0; agentNo < Simulator.instance.getNumAgents(); agentNo++) { Vector2D goalVector = goals.get(agentNo).subtract(Simulator.instance.getAgentPosition(agentNo)); final double lengthSq = goalVector.getNormSq(); if (lengthSq > 1.0) { goalVector = goalVector.scalarMultiply(1.0 / FastMath.sqrt(lengthSq)); }/*from w w w .jav a2s . c o m*/ Simulator.instance.setAgentPreferredVelocity(agentNo, goalVector); // Perturb a little to avoid deadlocks due to perfect symmetry. final double angle = random.nextDouble() * 2.0 * FastMath.PI; final double distance = random.nextDouble() * 0.0001; Simulator.instance.setAgentPreferredVelocity(agentNo, Simulator.instance.getAgentPreferredVelocity(agentNo) .add(new Vector2D(FastMath.cos(angle), FastMath.sin(angle)).scalarMultiply(distance))); } }
From source file:haxball.networking.ServerMainLoop.java
@Override public void run() { resetField();/*from ww w. j av a 2 s .c o 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: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.unc.cs.gamma.rvo.Agent.java
/** * Solves a two-dimensional linear program subject to linear constraints * defined by lines and a circular constraint. * * @param lines Lines defining the linear constraints. * @param optimizationVelocity The optimization velocity. * @param optimizeDirection True if the direction should be optimized. * @return The number of the line on which it fails, or the number of lines * if successful.// w w w. j av a2s . c o m */ private int linearProgram2(List<Line> lines, Vector2D optimizationVelocity, boolean optimizeDirection) { if (optimizeDirection) { // Optimize direction. Note that the optimization velocity is of unit length in this case. newVelocity = optimizationVelocity.scalarMultiply(maxSpeed); } else if (optimizationVelocity.getNormSq() > maxSpeed * maxSpeed) { // Optimize closest point and outside circle. newVelocity = optimizationVelocity.normalize().scalarMultiply(maxSpeed); } else { // Optimize closest point and inside circle. newVelocity = optimizationVelocity; } for (int lineNo = 0; lineNo < lines.size(); lineNo++) { if (RVOMath.det(lines.get(lineNo).direction, lines.get(lineNo).point.subtract(newVelocity)) > 0.0) { // Result does not satisfy constraint i. Compute new optimal // result. final Vector2D tempResult = newVelocity; if (!linearProgram1(lines, lineNo, optimizationVelocity, optimizeDirection)) { newVelocity = tempResult; return lineNo; } } } return lines.size(); }
From source file:edu.unc.cs.gamma.rvo.Agent.java
/** * Computes the new velocity of this agent. *//*from ww w . j av a 2 s . co m*/ 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:org.kalypso.model.wspm.ewawi.utils.profiles.EwawiProfilePoint.java
/** * This function returns the point as shape. * //from w w w . j ava 2 s . com * @return The point as shape. */ public SHPPoint getShape() { /* g Rechtswert (Projektion auf Abszisse) von links nach rechts aufsteigend */ /* h Hochwert (Abweichung von der Achse) rechts positiv, links negativer Wert. */ final double rechtswert = m_proLine.getRechtswert().doubleValue(); final double hochwert = m_proLine.getHochwert().doubleValue(); /* Get the normalized vector of the fix points (length 1). */ final Vector2D normalizedProfileAxis = getNormalizedProfileAxis(); /* Get the projection vector. */ final Vector2D projectionVector = normalizedProfileAxis.scalarMultiply(rechtswert); /* Create the start point. */ final double xLeft = m_left.getRechtswert().doubleValue(); final double yLeft = m_left.getHochwert().doubleValue(); final Vector2D startPoint = new Vector2D(xLeft, yLeft); /* Create the projection point. */ final Vector2D projectionPoint = startPoint.add(projectionVector); /* Create the orthogonal vector from the normalized profile axis (has then also length 1). */ final Vector2D orthVector = new Vector2D(normalizedProfileAxis.getY(), -normalizedProfileAxis.getX()); /* Add hochwert * orthVector to the projection point. */ final Vector2D targetPoint = projectionPoint.add(hochwert, orthVector); return new SHPPoint(targetPoint.getX(), targetPoint.getY()); }
From source file:org.kalypso.model.wspm.ewawi.utils.profiles.EwawiProfilePoint.java
/** * This function returns the point as shape. * It ignores the hochwert, so that the point will be the projected point on the idealized line (left fixpoint to right fixpoint). * /*from w ww . j av a 2s. c o m*/ * @return The point as shape. */ public SHPPoint getShapeIdealized() { /* g Rechtswert (Projektion auf Abszisse) von links nach rechts aufsteigend */ final double rechtswert = m_proLine.getRechtswert().doubleValue(); /* Get the normalized vector of the fix points (length 1). */ final Vector2D normalizedProfileAxis = getNormalizedProfileAxis(); /* Get the projection vector. */ final Vector2D projectionVector = normalizedProfileAxis.scalarMultiply(rechtswert); /* Create the start point. */ final double xLeft = m_left.getRechtswert().doubleValue(); final double yLeft = m_left.getHochwert().doubleValue(); final Vector2D startPoint = new Vector2D(xLeft, yLeft); /* Create the projection point. */ final Vector2D projectionPoint = startPoint.add(projectionVector); return new SHPPoint(projectionPoint.getX(), projectionPoint.getY()); }