List of usage examples for org.apache.commons.math3.geometry.euclidean.twod Vector2D dotProduct
public double dotProduct(final Vector<Euclidean2D> v)
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 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. j av a2s . co m */ 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:edu.unc.cs.gamma.rvo.Agent.java
/** * Computes the new velocity of this agent. *///from ww w.j ava 2 s. c o 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.orekit.bodies.Ellipse.java
/** Project position-velocity-acceleration on an ellipse. * @param pv position-velocity-acceleration to project, in the reference frame * @return projected position-velocity-acceleration *//* w w w.j a v a2s .c om*/ public TimeStampedPVCoordinates projectToEllipse(final TimeStampedPVCoordinates pv) { // find the closest point in the meridian plane final Vector2D p2D = toPlane(pv.getPosition()); final Vector2D e2D = projectToEllipse(p2D); // tangent to the ellipse final double fx = -a2 * e2D.getY(); final double fy = b2 * e2D.getX(); final double f2 = fx * fx + fy * fy; final double f = FastMath.sqrt(f2); final Vector2D tangent = new Vector2D(fx / f, fy / f); // normal to the ellipse (towards interior) final Vector2D normal = new Vector2D(-tangent.getY(), tangent.getX()); // center of curvature final double x2 = e2D.getX() * e2D.getX(); final double y2 = e2D.getY() * e2D.getY(); final double eX = evoluteFactorX * x2; final double eY = evoluteFactorY * y2; final double omegaX = eX * e2D.getX(); final double omegaY = eY * e2D.getY(); // velocity projection ratio final double rho = FastMath.hypot(e2D.getX() - omegaX, e2D.getY() - omegaY); final double d = FastMath.hypot(p2D.getX() - omegaX, p2D.getY() - omegaY); final double projectionRatio = rho / d; // tangential velocity final Vector2D pDot2D = new Vector2D(Vector3D.dotProduct(pv.getVelocity(), u), Vector3D.dotProduct(pv.getVelocity(), v)); final double pDotTangent = pDot2D.dotProduct(tangent); final double pDotNormal = pDot2D.dotProduct(normal); final double eDotTangent = projectionRatio * pDotTangent; final Vector2D eDot2D = new Vector2D(eDotTangent, tangent); final Vector2D tangentDot = new Vector2D( a2 * b2 * (e2D.getX() * eDot2D.getY() - e2D.getY() * eDot2D.getX()) / f2, normal); // velocity of the center of curvature in the meridian plane final double omegaXDot = 3 * eX * eDotTangent * tangent.getX(); final double omegaYDot = 3 * eY * eDotTangent * tangent.getY(); // derivative of the projection ratio final double voz = omegaXDot * tangent.getY() - omegaYDot * tangent.getX(); final double vsz = -pDotNormal; final double projectionRatioDot = ((rho - d) * voz - rho * vsz) / (d * d); // acceleration final Vector2D pDotDot2D = new Vector2D(Vector3D.dotProduct(pv.getAcceleration(), u), Vector3D.dotProduct(pv.getAcceleration(), v)); final double pDotDotTangent = pDotDot2D.dotProduct(tangent); final double pDotTangentDot = pDot2D.dotProduct(tangentDot); final double eDotDotTangent = projectionRatio * (pDotDotTangent + pDotTangentDot) + projectionRatioDot * pDotTangent; final Vector2D eDotDot2D = new Vector2D(eDotDotTangent, tangent, eDotTangent, tangentDot); // back to 3D final Vector3D e3D = toSpace(e2D); final Vector3D eDot3D = new Vector3D(eDot2D.getX(), u, eDot2D.getY(), v); final Vector3D eDotDot3D = new Vector3D(eDotDot2D.getX(), u, eDotDot2D.getY(), v); return new TimeStampedPVCoordinates(pv.getDate(), e3D, eDot3D, eDotDot3D); }