public void limitAngularPartMagnitude(double maximumMagnitude) { if (maximumMagnitude < 1e-7) angularPart.set(0.0, 0.0, 0.0); if (angularPart.lengthSquared() > maximumMagnitude * maximumMagnitude) { angularPart.scale(maximumMagnitude/angularPart.length()); } }
@Override protected boolean isInsideOrOnSurfaceShapeFrame(Point3d pointToCheck, double epsilon) { temporaryVector.set(pointToCheck); return temporaryVector.length() <= radius + epsilon; }
@Override protected double distanceShapeFrame(Point3d point) { temporaryVector.set(point); return temporaryVector.length() - radius; }
public void limitLinearPartMagnitude(double maximumMagnitude) { if (maximumMagnitude < 1e-7) linearPart.set(0.0, 0.0, 0.0); if (linearPart.lengthSquared() > maximumMagnitude * maximumMagnitude) { linearPart.scale(maximumMagnitude/linearPart.length()); } }
private static Vector3d getNonColinearVector(Vector3d ab) { Vector3d cr = new Vector3d(); cr.cross(ab, XV); if (cr.length() > 0.00001) { return XV; } else { return YV; } } }
@Override protected void orthogonalProjectionShapeFrame(Point3d pointToCheckAndPack) { computeCompositeVectorsForPoint(originToRadiusTemporaryVector, tubeCenterToPointTemporaryVector, pointToCheckAndPack); if (tubeCenterToPointTemporaryVector.length() > tubeRadius) tubeCenterToPointTemporaryVector.scale(tubeRadius / tubeCenterToPointTemporaryVector.length()); tubeCenterToPointTemporaryVector.add(originToRadiusTemporaryVector); pointToCheckAndPack.set(tubeCenterToPointTemporaryVector); }
public boolean isSolvable(double trajectoryTime, Vector3d omegaA, Vector3d omegaB) { omegaA.scale(trajectoryTime / 3); boolean omegaALength = omegaA.length() < 2 * Math.PI; omegaB.scale(trajectoryTime / 3); boolean omegaBLength = (omegaB.length() < 2 * Math.PI); omegaB.scale(-1); boolean opposite = omegaA.epsilonEquals(omegaB, 1e-2); return omegaALength && omegaBLength && !opposite; }
public static double getMagnitudeOfTranslation(RigidBodyTransform rigidBodyTransform) { Vector3d vector3d = new Vector3d(); rigidBodyTransform.getTranslation(vector3d); return vector3d.length(); }
private void sendCorrectionUpdatePacket() { totalRotationErrorFrame.get(totalRotationError); totalTranslationErrorFrame.get(totalTranslationError); totalError.set(totalRotationError, totalTranslationError); errorBetweenCurrentPositionAndCorrected.getTranslation(translationalResidualError); double absoluteResidualError = translationalResidualError.length(); totalError.getTranslation(translationalTotalError); double absoluteTotalError = translationalTotalError.length(); PelvisPoseErrorPacket pelvisPoseErrorPacket = new PelvisPoseErrorPacket((float) absoluteTotalError, (float) absoluteResidualError, false); pelvisPoseCorrectionCommunicator.sendPelvisPoseErrorPacket(pelvisPoseErrorPacket); }
@Override protected void orthogonalProjectionShapeFrame(Point3d point) { temporaryVector.set(point); double distance = temporaryVector.length(); if (distance >= Epsilons.ONE_TRILLIONTH) { temporaryVector.normalize(); temporaryVector.scale(radius); point.set(temporaryVector); } }
@Override public void update() { foreSensorData.getWrench(wrenchToPack); wrenchToPack.getLinearPart(vectorToPack); touchdownDetected.set(vectorToPack.length() > touchdownForceThreshold.getDoubleValue()); touchdownDetectedFiltered.update(); } }
private void updateMaxTranslationAlphaVariationSpeed() { //Translation distanceToTravelVector.sub(updatedGoalOffsetWithDeadzone_Translation, updatedStartOffset_Translation); distanceToTravel.set(distanceToTravelVector.length()); translationalSpeedForGivenDistanceToTravel.set(distanceToTravel.getDoubleValue() / dt.getDoubleValue()); }
public boolean isPointNearWheel(double x, double y, double z, double fudgeFactor) { tempVector.set(x, y, z); tempVector3.set(1.0, 0.0, 0.0); double dotProduct = tempVector.dot(tempVector3); tempVector2.set(tempVector3); tempVector2.scale(dotProduct); tempVector.sub(tempVector2); double radius = tempVector.length(); return (dotProduct > -BOUNDING_WHEEL_NEAR_X) && (dotProduct < BOUNDING_WHEEL_FAR_X) && (radius < BOUNDING_OUTER_WHEEL_RADIUS - fudgeFactor) && (radius > BOUNDING_INNER_WHEEL_RADIUS + fudgeFactor); }
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()); }
@Test() public void cod1100784CellLengths() throws IOException, CDKException { InputStream in = getClass().getResourceAsStream("1100784.cif"); CIFReader cifReader = new CIFReader(in); IChemFile chemFile = cifReader.read(new ChemFile()); ICrystal crystal = chemFile.getChemSequence(0).getChemModel(0).getCrystal(); Assert.assertTrue( java.lang.Math.abs(crystal.getA().length() - 10.9754) < 1E-5 ); Assert.assertTrue( java.lang.Math.abs(crystal.getB().length() - 11.4045) < 1E-5 ); Assert.assertTrue( java.lang.Math.abs(crystal.getC().length() - 12.9314) < 1E-5 ); cifReader.close(); }
@Override protected boolean isInsideOrOnSurfaceShapeFrame(Point3d pointToCheck, double epsilon) { temporaryVector.set(pointToCheck.getX(), pointToCheck.getY(), 0.0); if (temporaryVector.length() < Epsilons.ONE_TRILLIONTH) { return tubeRadius >= radius; } temporaryVector.normalize(); temporaryVector.scale(radius); temporaryPoint.set(temporaryVector); return temporaryPoint.distance(pointToCheck) <= tubeRadius + epsilon; }
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); }
private void computePelvisVelocityError() { Vector3d actualVelocity = new Vector3d(); Vector3d linearVelocityError = new Vector3d(); estimationJoint.physics.getLinearVelocityInWorld(actualVelocity, new Vector3d()); perfectPelvisLinearVelocity.set(actualVelocity); orientationEstimator.getEstimatedPelvisLinearVelocity(estimatedPelvisVelocityFrameVector); estimatedPelvisVelocityFrameVector.get(linearVelocityError); linearVelocityError.sub(actualVelocity); pelvisLinearVelocityError.set(linearVelocityError.length()); } }
public static RigidBodyTransform opencvTRtoRigidBodyTransform(Mat rvec, Mat tvec) { Vector3d translation = new Vector3d(tvec.get(0, 0)[0], tvec.get(1, 0)[0], tvec.get(2, 0)[0]); Vector3d axis = new Vector3d(rvec.get(0, 0)[0], rvec.get(1, 0)[0], rvec.get(2, 0)[0]); double angle = axis.length(); axis.normalize(); AxisAngle4d rotation = new AxisAngle4d(axis, angle); RigidBodyTransform transform = new RigidBodyTransform(rotation, translation); return transform; }
private void computePelvisPositionError() { Vector3d actualPosition = new Vector3d(); Vector3d positionError = new Vector3d(); estimationJoint.getTranslationToWorld(actualPosition); perfectPelvisPosition.set(actualPosition); orientationEstimator.getEstimatedPelvisPosition(estimatedPelvisPosition); estimatedPelvisPosition.get(positionError); positionError.sub(actualPosition); pelvisZPositionError.set(positionError.getZ()); positionError.setZ(0.0); pelvisXYPositionError.set(positionError.length()); }