@Override protected void setZ(Vector3D data, double z) { data.setZ(z); }
@Override public double heightAndNormalAt(double x, double y, double z, Vector3D normalToPack) { normalToPack.setZ(1.0); return 0.0; }
/** * Sets the z-component of this vector. * * @param z the z-component. */ @Override public void setZ(double z) { vector.setZ(z); }
/** * Sets the radius along the z-axis for this ellipsoid. * * @param radiusZ radius of the ellipsoid along the z-axis. * @throws IllegalArgumentException if the argument is negative. */ public void setRadiusZ(double radiusZ) { if (radiusZ < 0.0) throw new IllegalArgumentException("The z-radius of an Ellipsoid3D cannot be negative: " + radiusZ); radii.setZ(radiusZ); }
private void randomizeVector(Random random, Vector3D vector) { vector.setX(random.nextDouble()); vector.setY(random.nextDouble()); vector.setZ(random.nextDouble()); } }
public void bounceAwayAfterCollision() { this.launched = false; velocityVector.scale(-1.0); final double zVelocityAfterBounce = -10.0; velocityVector.setZ(zVelocityAfterBounce); setVelocity(velocityVector); }
public void packPosition(Vector3D position) { position.setX(this.position[0]); position.setY(this.position[1]); position.setZ(this.position[2]); }
public void surfaceNormalAt(double x, double y, double z, Vector3D normal) { normal.setX(0.0); normal.setY(0.0); normal.setZ(1.0); }
/** * add a trajectory point to this message */ public void addTrajectoryPoint(double time, double zHeight, double velocity) { tempPoint.setToZero(); tempPoint.setZ(zHeight); tempVector.setToZero(); tempVector.setZ(velocity); euclideanTrajectory.addTrajectoryPoint(time, tempPoint, tempVector); }
/** * Pack the variance along each principal axis in the given Vector3d. * * @param principalVarianceToPack The variance is stored in the Vector3d as follows: x is the * variance on the principal axis, y on the secondary axis, and z on the third axis. */ public void getVariance(Vector3D principalVarianceToPack) { principalVarianceToPack.setX(variance.getX()); principalVarianceToPack.setY(variance.getY()); principalVarianceToPack.setZ(variance.getZ()); }
public void getMagnetometer(Vector3D magToPack) { magToPack.setX(magX.getDoubleValue()); magToPack.setY(magY.getDoubleValue()); magToPack.setZ(magZ.getDoubleValue()); }
public void packLinearAcceleration(Vector3D accelerationToPack) { accelerationToPack.setX(this.xdd.getDoubleValue()); accelerationToPack.setY(this.ydd.getDoubleValue()); accelerationToPack.setZ(this.zdd.getDoubleValue()); }
public void getAccel(Vector3D accelToPack) { accelToPack.setX(accelX.getDoubleValue()); accelToPack.setY(accelY.getDoubleValue()); accelToPack.setZ(accelZ.getDoubleValue()); }
public void packAngularVelocity(Vector3D angularVelocityToPack) { angularVelocityToPack.setX(this.theta_x.getDoubleValue()); angularVelocityToPack.setY(this.theta_y.getDoubleValue()); angularVelocityToPack.setZ(this.theta_z.getDoubleValue()); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { xAxis.set(positionToPointAt); xAxis.setZ(0.0); xAxis.normalize(); yAxis.cross(zAxis, xAxis); rotationMatrix.setColumns(xAxis, yAxis, zAxis); transformToParent.setRotation(rotationMatrix); } }
public void getGyro(Vector3D gyroToPack) { gyroToPack.setX(gyroX.getDoubleValue()); gyroToPack.setY(gyroY.getDoubleValue()); gyroToPack.setZ(gyroZ.getDoubleValue()); }
private void extractTandR(RigidBodyTransform tran, Vector3D T, Vector3D R) { tran.getTranslation(T); tran.getRotation(rotationMatrix); rotationMatrix.get(m); ConvertRotation3D_F64.matrixToEuler(m, EulerType.XYZ, euler); R.setX(euler[0]); R.setY(euler[1]); R.setZ(euler[2]); }
protected void setPositionInWorld(HumanoidFloatingRootJointRobot robot) { robot.update(); robot.getRootJointToWorldTransform(rootToWorld); rootToWorld.getTranslation(offset); Vector3D positionInWorld = new Vector3D(); rootToWorld.getTranslation(positionInWorld); GroundContactPoint gc1 = robot.getFootGroundContactPoints(RobotSide.LEFT).get(0); double pelvisToFoot = positionInWorld.getZ() - gc1.getPositionPoint().getZ(); offset.setZ(groundZ + pelvisToFoot); robot.setPositionInWorld(offset); }
private Vector3DReadOnly convertToAngularVelocity(QuaternionReadOnly q, Vector4DReadOnly qDot) {// w = qDot * q^-1 QuaternionTools.multiplyConjugateRight(qDot, q, tempConvertVector4D); tempConvertVector3D.setX(tempConvertVector4D.getX()); tempConvertVector3D.setY(tempConvertVector4D.getY()); tempConvertVector3D.setZ(tempConvertVector4D.getZ()); tempConvertVector3D.scale(2.0); return tempConvertVector3D; }
public static Vector3D nextVector3D(Random random, Tuple3DReadOnly boundary1, Tuple3DReadOnly boundary2) { Vector3D ret = new Vector3D(); ret.setX(RandomNumbers.nextDouble(random, boundary1.getX(), boundary2.getX())); ret.setY(RandomNumbers.nextDouble(random, boundary1.getY(), boundary2.getY())); ret.setZ(RandomNumbers.nextDouble(random, boundary1.getZ(), boundary2.getZ())); return ret; }