vec.sub(new Vector3d(item.posX, item.posY, item.posZ));
/** * Subtracts another spatial force vector from this one, after performing some reference frame checks. */ public void sub(SpatialForceVector other) { this.expressedInFrame.checkReferenceFrameMatch(other.expressedInFrame); this.linearPart.sub(other.linearPart); this.angularPart.sub(other.angularPart); }
/** * Computes the vector going from the first to the second endpoint of this line segment. * * @param normalize whether the direction vector is to be normalized. * @param directionToPack vector in which the direction is stored. Modified. */ public void getDirection(boolean normalize, Vector3d directionToPack) { directionToPack.sub(secondEndpoint, firstEndpoint); if (normalize) directionToPack.normalize(); }
public void subAngularPart(FrameVector angularPart) { expressedInFrame.checkReferenceFrameMatch(angularPart); this.angularPart.sub(angularPart.getVector()); }
public void getRay(Vector3d rayToPack, int rayIndex) { MathTools.checkIfInRange(rayIndex, 0, numberOfRays - 1); rayToPack.sub(sphereOrigin, pointsOnSphere[rayIndex]); rayToPack.normalize(); }
/** * Compute the vector from this coord to the input coord * * @param loc * @return */ public Vector3d getVector(final EarthVector loc) { final Vector3d vecTemp = new Vector3d(loc.getVector()); vecTemp.sub(ecfVector); return vecTemp; }
private double calculateAngleBetweenTwoLines(Vector3d a, Vector3d b, Vector3d c, Vector3d d) { Vector3d firstLine = new Vector3d(); firstLine.sub(a, b); Vector3d secondLine = new Vector3d(); secondLine.sub(c, d); Vector3d firstVec = new Vector3d(firstLine); Vector3d secondVec = new Vector3d(secondLine); return firstVec.angle(secondVec); }
private double calculateAngleBetweenTwoLines(Vector3d a, Vector3d b, Vector3d c, Vector3d d) { Vector3d firstLine = new Vector3d(); firstLine.sub(a, b); Vector3d secondLine = new Vector3d(); secondLine.sub(c, d); Vector3d firstVec = new Vector3d(firstLine); Vector3d secondVec = new Vector3d(secondLine); return firstVec.angle(secondVec); }
// Surface 1/3 (x, y, z - one surface for each) Vector3d edge1 = new Vector3d(); Vector3d edge2 = new Vector3d(); Vector3d normal1 = new Vector3d(); // Get the edges, the two edges must be perpendicular to one another. edge1.sub( point0, point1 ); edge2.sub( point0, point4 ); normal1.cross( edge1, edge2 ); normal1.normalize();
@Override public Vector3d getDimension() { run(); Vector3d dimension = new Vector3d(); dimension.sub(maxBoundary, minBoundary); dimension.scale(0.5); return dimension; }
public Vector3d getNormalizedEarthTangentVector(final double azimuth) { // TODO: rewrite this to use real math instead of this silly difference final EarthVector nextEV = findPoint(1, azimuth); final Vector3d deltaVec = new Vector3d(); deltaVec.sub(nextEV.getVector(), getVector()); deltaVec.normalize(); return deltaVec; }
private void updateMaxTranslationAlphaVariationSpeed() { //Translation distanceToTravelVector.sub(updatedGoalOffsetWithDeadzone_Translation, updatedStartOffset_Translation); distanceToTravel.set(distanceToTravelVector.length()); translationalSpeedForGivenDistanceToTravel.set(distanceToTravel.getDoubleValue() / dt.getDoubleValue()); }
private void computeAngularVelocityError() { orientationEstimator.getEstimatedAngularVelocity(estimatedAngularVelocityFrameVector); Vector3d estimatedAngularVelocity = estimatedAngularVelocityFrameVector.getVectorCopy(); Vector3d actualAngularVelocity = new Vector3d(); estimationJoint.physics.getAngularVelocityInBody(actualAngularVelocity); perfectAngularVelocity.set(actualAngularVelocity); actualAngularVelocity.sub(estimatedAngularVelocity); angularVelocityError.set(actualAngularVelocity.length()); }
public void orthogonalProjection(Vector3d vectorToProject) { temporaryVector.set( 0.0, 0.0, 0.0); temporaryVector.sub(this.point); double distA = temporaryVector.dot(this.normal); temporaryVector.set( vectorToProject ); temporaryVector.sub(this.point); double distB = temporaryVector.dot(this.normal); temporaryVector.set(this.normal); temporaryVector.scale(distB - distA); vectorToProject.sub( temporaryVector ); }
private void setNodesCostsAndRememberIfClosestYet(BipedalFootstepPlannerNode nodeToSetCostOf) { Point3d currentPosition = nodeToSetCostOf.getSolePosition(); Point3d goalPosition = planarRegionPotentialNextStepCalculator.getGoalPosition(nodeToSetCostOf.getRobotSide()); Vector3d currentToGoalVector = new Vector3d(); currentToGoalVector.sub(goalPosition, currentPosition); double euclideanDistanceToGoal = currentToGoalVector.length(); nodeToSetCostOf.setEstimatedCostToGoal(euclideanDistanceToGoal); if (closestNodeToGoal == null || euclideanDistanceToGoal + 0.01 < closestNodeToGoal.getEstimatedCostToGoal()) replaceBestPlan(nodeToSetCostOf); }
public Point3d get3DCoordinatesForSPLigands(IAtom refAtom, IAtomContainer withCoords, double length, double angle) { //logger.debug(" SP Ligands start "+refAtom.getPoint3d()+" "+(withCoords.getAtomAt(0)).getPoint3d()); Vector3d ca = new Vector3d(refAtom.getPoint3d()); ca.sub((withCoords.getAtom(0)).getPoint3d()); ca.normalize(); ca.scale(length); Point3d newPoint = new Point3d(refAtom.getPoint3d()); newPoint.add(ca); return newPoint; }
public DenseMatrix64F computeResidual() { PointPositionDataObject data = pointPositionMeasurementInputPort.getData(); ReferenceFrame referenceFrame = referenceFrameMap.getFrameByName(pointPositionMeasurementInputPort.getData().getBodyFixedReferenceFrameName()); tempFramePoint.setIncludingFrame(referenceFrame, data.getMeasurementPointInBodyFrame()); tempFramePoint.changeFrame(ReferenceFrame.getWorldFrame()); residualVector.set(data.getMeasurementPointInWorldFrame()); residualVector.sub(tempFramePoint.getPoint()); MatrixTools.insertTuple3dIntoEJMLVector(residualVector, residual, 0); return residual; }
public void setBasedOnOriginAcceleration(FrameVector angularAcceleration, FrameVector originAcceleration, Twist twistOfBodyWithRespectToBase) { bodyFrame.checkReferenceFrameMatch(expressedInFrame); twistOfBodyWithRespectToBase.getBodyFrame().checkReferenceFrameMatch(bodyFrame); twistOfBodyWithRespectToBase.getBaseFrame().checkReferenceFrameMatch(baseFrame); angularAcceleration.changeFrame(bodyFrame); angularPart.set(angularAcceleration.getVector()); originAcceleration.changeFrame(bodyFrame); twistOfBodyWithRespectToBase.changeFrame(bodyFrame); linearPart.cross(twistOfBodyWithRespectToBase.getAngularPart(), twistOfBodyWithRespectToBase.getLinearPart()); linearPart.sub(originAcceleration.getVector(), linearPart); }