private void computeOther(double value) { k++; sum = sum + value; double mKLast = mK; double kDouble = (double)k; mK = mKLast + (value - mKLast) / kDouble; qK = qK + (kDouble - 1.0) * MathTools.square(value - mKLast) / kDouble; }
public static double getCosineAngleDotWithCosineLaw(double l_neighbour1, double l_neighbour2, double lDot_neighbour2, double l_opposite, double lDot_opposite) { checkIntervalContains(l_neighbour1, 0.0, Double.POSITIVE_INFINITY); checkIntervalContains(l_neighbour2, 0.0, Double.POSITIVE_INFINITY); checkIntervalContains(l_opposite, 0.0, Double.POSITIVE_INFINITY); double cosAngleDot = (square(l_neighbour2) * lDot_neighbour2 - 2.0 * l_neighbour2 * l_opposite * lDot_opposite - lDot_neighbour2 * square(l_neighbour1) + lDot_neighbour2 * square(l_opposite)) / (2.0 * square(l_neighbour2) * l_neighbour1); return cosAngleDot; }
public static double getCosineAngleDDotWithCosineLaw(double l_neighbour1, double l_neighbour2, double lDot_neighbour2, double lDDot_neighbour2, double l_opposite, double lDot_opposite, double lDDot_opposite) { checkIntervalContains(l_neighbour1, 0.0, Double.POSITIVE_INFINITY); checkIntervalContains(l_neighbour2, 0.0, Double.POSITIVE_INFINITY); checkIntervalContains(l_opposite, 0.0, Double.POSITIVE_INFINITY); double cosAngleDDot = (cube(l_neighbour2) * lDDot_neighbour2 - 2 * square(l_neighbour2 * lDot_opposite) - 2 * square(l_neighbour2) * l_opposite * lDDot_opposite + 4 * l_neighbour2 * lDot_neighbour2 * l_opposite * lDot_opposite + 2 * square(lDot_neighbour2 * l_neighbour1) - 2 * square( lDot_neighbour2 * l_opposite) - l_neighbour2 * lDDot_neighbour2 * square(l_neighbour1) + l_neighbour2 * lDDot_neighbour2 * square(l_opposite)) / (2.0 * cube(l_neighbour2) * l_neighbour1); return cosAngleDDot; }
public void printResultsForTest() { System.out.println(timeMatrix); System.out.println(time.get(0) + " : " + time.get(1) + " : " + time.get(2)); System.out.println(parametersX.get(1)[0] + " : " + parametersX.get(1)[1] + " : " + parametersX.get(1)[2]); System.out.println(parametersY.get(1)[0] + " : " + parametersY.get(1)[1] + " : " + parametersY.get(1)[2]); System.out.println(parametersZ.get(1)[0] + " : " + parametersZ.get(1)[1] + " : " + parametersZ.get(1)[2]); System.out.println(parametersX.get(1)[0] * MathTools.square(time.get(0)) + parametersX.get(1)[1] * time.get(0) + parametersX.get(1)[2]); System.out.println(parametersX.get(1)[0] * MathTools.square(time.get(1)) + parametersX.get(1)[1] * time.get(1) + parametersX.get(1)[2]); System.out.println(parametersX.get(1)[0] * MathTools.square(time.get(2)) + parametersX.get(1)[1] * time.get(2) + parametersX.get(1)[2]); System.out.println(2 * parametersX.get(1)[0] * time.get(0) + parametersX.get(1)[1]); System.out.println(2 * parametersX.get(1)[0] * time.get(1) + parametersX.get(1)[1]); System.out.println(2 * parametersX.get(1)[0] * time.get(2) + parametersX.get(1)[1]); System.out.println("final check"); System.out.println(xDot); }
private static double getCosineAngleWithCosineLaw(double l_neighbour1, double l_neighbour2, double l_opposite) { checkIntervalContains(l_neighbour1, 0.0, Double.POSITIVE_INFINITY); checkIntervalContains(l_neighbour2, 0.0, Double.POSITIVE_INFINITY); checkIntervalContains(l_opposite, 0.0, Double.POSITIVE_INFINITY); double cosAngle = MathTools.clamp((square(l_neighbour1) + square(l_neighbour2) - square(l_opposite)) / (2.0 * l_neighbour1 * l_neighbour2), -1.0, 1.0); return cosAngle; }
public static double getCosineAngleWithCosineLaw(double l_neighbour1, double l_neighbour2, double l_opposite) { checkIntervalContains(l_neighbour1, 0.0, Double.POSITIVE_INFINITY); checkIntervalContains(l_neighbour2, 0.0, Double.POSITIVE_INFINITY); checkIntervalContains(l_opposite, 0.0, Double.POSITIVE_INFINITY); double cosAngle = MathTools .clamp((square(l_neighbour1) + square(l_neighbour2) - square(l_opposite)) / (2.0 * l_neighbour1 * l_neighbour2), -1.0, 1.0); return cosAngle; }
public static void getAcceleration(FrameVector3D accelerationToPack, ReferenceFrame frame, double angle, double angleDot, double angleDDot, double radius, double radiusDot, double radiusDDot, double zDDot) { double cos = Math.cos(angle); double sin = Math.sin(angle); accelerationToPack.setToZero(frame); double xDDot = -cos * radius * MathTools.square(angleDot) - 2.0 * sin * angleDot * radiusDot - radius * sin * angleDDot + cos * radiusDDot; double yDDot = -sin * radius * MathTools.square(angleDot) + 2.0 * cos * angleDot * radiusDot + radius * cos * angleDDot + sin * radiusDDot; accelerationToPack.setX(xDDot); accelerationToPack.setY(yDDot); accelerationToPack.setZ(zDDot); }
public void update(double input) { int nUpdatesOld = nUpdates.getIntegerValue(); nUpdates.increment(); int nUpdatesNew = nUpdates.getIntegerValue(); double ratio = ((double) nUpdatesOld) / ((double) nUpdatesNew); rms.set(Math.sqrt(square(rms.getDoubleValue()) * ratio + square(input) / nUpdatesNew)); }
public static double getAngularVelocity(FramePoint3D position, FrameVector3D velocity) { position.checkReferenceFrameMatch(velocity); double x = position.getX(); double y = position.getY(); double xd = velocity.getX(); double yd = velocity.getY(); double radiusSquared = MathTools.square(x) + MathTools.square(y); return (x * yd - y * xd) / radiusSquared; } }
default InterRegionConnectionFilter getInterRegionConnectionFilter() { return new InterRegionConnectionFilter() { private final double maxLengthSquared = MathTools.square(getMaxInterRegionConnectionLength()); private final double maxDeltaHeight = getTooHighToStepDistance(); @Override public boolean isConnectionValid(ConnectionPoint3D source, ConnectionPoint3D target) { if (Math.abs(source.getZ() - target.getZ()) > maxDeltaHeight) return false; if (source.distanceSquared(target) > maxLengthSquared) return false; return true; } }; }
public void addRamp(double length, double deltaZ) { checkHasNotBeenGenerated(); generator.translate(length / 2.0, 0.0, deltaZ / 2.0); generator.rotate(-Math.atan2(deltaZ, length), Axis.Y); generator.addRectangle(Math.sqrt(MathTools.square(length) + MathTools.square(deltaZ)), rampWidth); generator.rotate(Math.atan2(deltaZ, length), Axis.Y); generator.translate(length / 2.0, 0.0, deltaZ / 2.0); addPlanarRegionsToTerrain(YoAppearance.Grey()); } }
@Override public void assertStateIsCloseToClosedFormCalculation(double epsilon) throws AssertionError { double q = pinJoint.getQ(); double qd = pinJoint.getQD(); double qdd = pinJoint.getQDD(); double qddLagrangian = (mass * getGravityZ() * (0.5 * length) * Math.sin(q) - damping * qd) / (Ixx + mass * MathTools.square(0.5 * length)); if(Math.abs(qdd - qddLagrangian) > epsilon) { throw new AssertionError("Joint accelerations from simulation and lagrangian don't match. At t=" + getTime() + ", simulated joint acceleration = " + qdd + ", lagrangian acceleration = " + qddLagrangian); } } }
public static double getAngleDDotWithCosineLaw(double l_neighbour1, double l_neighbour2, double lDot_neighbour2, double lDDot_neighbour2, double l_opposite, double lDot_opposite, double lDDot_opposite) { checkIntervalContains(l_neighbour1, 0.0, Double.POSITIVE_INFINITY); checkIntervalContains(l_neighbour2, 0.0, Double.POSITIVE_INFINITY); checkIntervalContains(l_opposite, 0.0, Double.POSITIVE_INFINITY); double cosAngle = getCosineAngleWithCosineLaw(l_neighbour1, l_neighbour2, l_opposite); double cosAngleDot = getCosineAngleDotWithCosineLaw(l_neighbour1, l_neighbour2, lDot_neighbour2, l_opposite, lDot_opposite); double cosAngleDDot = getCosineAngleDDotWithCosineLaw(l_neighbour1, l_neighbour2, lDot_neighbour2, lDDot_neighbour2, l_opposite, lDot_opposite, lDDot_opposite); double angleDDot = (-cosAngleDDot + cosAngleDDot * square(cosAngle) - cosAngleDot * cosAngleDot * cosAngle) / pow(1 - square(cosAngle), 3.0 / 2.0); return angleDDot; }
public AlternatingSlopesEnvironment(double rampWidth, double landingLength) { super(); this.rampWidth = rampWidth; generator.addRectangle(Math.sqrt(MathTools.square(landingLength)), rampWidth); generator.translate(landingLength / 2.0, 0.0, 0.0); addPlanarRegionsToTerrain(YoAppearance.Grey()); }
private void updateBezierCoefficients(double t) { double T = trajectoryTime.getDoubleValue(); double oneOverT = 1.0 / T; double tOverT = t * oneOverT; cumulativeBeziers[1].set(1.0 - cube(1.0 - tOverT)); cumulativeBeziers[2].set(3.0 * square(tOverT) - 2.0 * cube(tOverT)); cumulativeBeziers[3].set(cube(tOverT)); cumulativeBeziersDot[1].set(3.0 * oneOverT * square(1.0 - tOverT)); cumulativeBeziersDot[2].set(6.0 * tOverT * oneOverT * (1.0 - tOverT)); cumulativeBeziersDot[3].set(3.0 * square(tOverT) * oneOverT); cumulativeBeziersDDot[1].set(-6.0 * square(oneOverT) * (1.0 - tOverT)); cumulativeBeziersDDot[2].set(6.0 * square(oneOverT) * (1.0 - 2.0 * tOverT)); cumulativeBeziersDDot[3].set(6.0 * t * cube(oneOverT)); }
public void computeQDDotByFiniteDifferenceCentral(QuaternionReadOnly qPrevious, QuaternionReadOnly q, QuaternionReadOnly qNext, double dt, Vector4DBasics qDDotToPack) { qDDotToPack.set(qNext); qDDotToPack.sub(q); qDDotToPack.sub(q); qDDotToPack.add(qPrevious); qDDotToPack.scale(1.0 / MathTools.square(dt)); }
public FrameVector3D getCurrentDesiredAcceleration(double timeInMove) { FramePoint3D Xfh = getCurrentDesiredPoint(timeInMove + stepSizeforNumericalCalculation); FramePoint3D X = getCurrentDesiredPoint(timeInMove); FramePoint3D Xrh = getCurrentDesiredPoint(timeInMove - stepSizeforNumericalCalculation); FrameVector3D ret = new FrameVector3D(X.getReferenceFrame()); ret.setX((Xfh.getX() - 2.0 * X.getX() + Xrh.getX()) / (MathTools.square(stepSizeforNumericalCalculation))); ret.setY((Xfh.getY() - 2.0 * X.getY() + Xrh.getY()) / (MathTools.square(stepSizeforNumericalCalculation))); ret.setZ((Xfh.getZ() - 2.0 * X.getZ() + Xrh.getZ()) / (MathTools.square(stepSizeforNumericalCalculation))); return ret; }
/** * Division of ComplexNumbers (doesn't change this ComplexNumber). * <br>(x+i*y)/(s+i*t) = ((x*s+y*t) + i*(y*s-y*t)) / (s^2+t^2) * @param w is the number to divide by * @return new ComplexNumber z/w where z is this ComplexNumber */ public ComplexNumber dividedBy(ComplexNumber w) { double den = MathTools.square(w.magnitude()); return new ComplexNumber((real * w.real() + imag * w.imag()) / den, (imag * w.real() - real * w.imag()) / den); }
public void getPosition(FramePoint3D positionToPack, double parameter) { double q = parameter; MathTools.checkIntervalContains(q, 0.0, 1.0); positionToPack.setToZero(referenceFrame); // c2 * q^2 positionToPack.set(c2); positionToPack.scale(MathTools.square(q)); // c1 * q tempPackPosition.set(c1); tempPackPosition.scale(q); positionToPack.add(tempPackPosition); // c0 positionToPack.add(c0); }
public void initialize(FramePoint3D initialPosition, FramePoint3D intermediatePosition, FramePoint3D finalPosition, double intermediateParameter) { initialPosition.changeFrame(referenceFrame); intermediatePosition.changeFrame(referenceFrame); finalPosition.changeFrame(referenceFrame); final double q = intermediateParameter; MathTools.checkIntervalContains(q, 0.0, 1.0); c0.set(initialPosition); c2.set(intermediatePosition); c2.sub(initialPosition); tempInitialize.set(finalPosition); tempInitialize.sub(initialPosition); tempInitialize.scale(q); c2.sub(tempInitialize); c2.scale(1.0 / (MathTools.square(q) - q)); c1.set(finalPosition); c1.sub(initialPosition); c1.sub(c2); }