public double getZRadius() { return radius.getZ(); }
/** * Gets the Z coordinate of the angular velocity part of the spatial motion vector */ public double getAngularPartZ() { return angularPart.getZ(); }
private void checkIsTransformationInPlane(Vector3d transformedVector) { if (Math.abs(transformedVector.getZ()) > epsilon) throw new RuntimeException("Cannot transform FramePoint2d to a plane with a different surface normal"); }
private void checkIsTransformationInPlane(RigidBodyTransform transformToNewFrame, Point3d transformedPoint) { transformToNewFrame.getTranslation(temporaryTranslation); if (Math.abs(temporaryTranslation.getZ() - transformedPoint.getZ()) > epsilon) throw new RuntimeException("Cannot transform FramePoint2d to a plane with a different surface normal"); }
@Override public boolean containsNaN() { if (Double.isNaN(time)) return true; if (Double.isNaN(position.getX()) || Double.isNaN(position.getY()) || Double.isNaN(position.getZ())) return true; if (Double.isNaN(linearVelocity.getX()) || Double.isNaN(linearVelocity.getY()) || Double.isNaN(linearVelocity.getZ())) return true; return false; }
@Override public boolean containsNaN() { if (Double.isNaN(orientation.getX()) || Double.isNaN(orientation.getY()) || Double.isNaN(orientation.getZ()) || Double.isNaN(orientation.getW())) return true; if (Double.isNaN(angularVelocity.getX()) || Double.isNaN(angularVelocity.getY()) || Double.isNaN(angularVelocity.getZ())) return true; return false; }
public void getTranslation(Tuple3d translationToPack) { translationToPack.setX(jointTranslation.getX()); translationToPack.setZ(jointTranslation.getZ()); }
public void setWeights(Vector3d angular, Vector3d linear) { weightVector.set(0, 0, angular.getX()); weightVector.set(1, 0, angular.getY()); weightVector.set(2, 0, angular.getZ()); weightVector.set(3, 0, linear.getX()); weightVector.set(4, 0, linear.getY()); weightVector.set(5, 0, linear.getZ()); hasWeight = angular.getX() != HARD_CONSTRAINT && angular.getY() != HARD_CONSTRAINT && angular.getZ() != HARD_CONSTRAINT; hasWeight = linear.getX() != HARD_CONSTRAINT && linear.getY() != HARD_CONSTRAINT && linear.getZ() != HARD_CONSTRAINT && hasWeight; }
public void update(Vector3d vectorUnfiltered) { x.update(vectorUnfiltered.getX()); y.update(vectorUnfiltered.getY()); z.update(vectorUnfiltered.getZ()); }
@Override public void getConfigurationMatrix(DenseMatrix64F matrix, int rowStart) { RotationTools.convertQuaternionToYawPitchRoll(jointRotation, yawPitchRoll); int index = rowStart; matrix.set(index++, 0, yawPitchRoll[1]); matrix.set(index++, 0, jointTranslation.getX()); matrix.set(index++, 0, jointTranslation.getZ()); }
public void update(Vector3d vectorSource) { x.update(vectorSource.getX()); y.update(vectorSource.getY()); z.update(vectorSource.getZ()); }
public void update(Vector3d vectorUnfiltered) { x.update(vectorUnfiltered.getX()); y.update(vectorUnfiltered.getY()); z.update(vectorUnfiltered.getZ()); }
/** * 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()); }
@Override protected void doScaleInstruction(Graphics3DScaleInstruction graphics3dObjectScale) { Node scale = new Node(); Vector3d scaleFactor = graphics3dObjectScale.getScaleFactor(); scale.setLocalScale((float) scaleFactor.getX(), (float) scaleFactor.getY(), (float) scaleFactor.getZ()); currentNode.attachChild(scale); currentNode = scale; addChangedScaleListener(scale, graphics3dObjectScale); }
public void get(double[] array) { array[0] = rotation.getW(); array[1] = rotation.getX(); array[2] = rotation.getY(); array[3] = rotation.getZ(); array[4] = translation.getX(); array[5] = translation.getY(); array[6] = translation.getZ(); twist.getArray(array, 7); }
public void setTransformToWorld(RigidBodyTransform transformToWorld) { transformToWorld.getTranslation(translationToWorld); x.set(translationToWorld.getX()); y.set(translationToWorld.getY()); z.set(translationToWorld.getZ()); orientation.setIncludingFrame(ReferenceFrame.getWorldFrame(), transformToWorld); setOrientation(orientation); }
private CombinedTerrainObject3D setUpGround(String name, Vector3d tableCenter, double tableLength) { CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name); YoAppearanceTexture texture = new YoAppearanceTexture("Textures/gridGroundProfile.png"); combinedTerrainObject.addBox(-10.0, -10.0, 10.0, 10.0, -0.05, 0.0, texture); combinedTerrainObject.addBox(tableCenter.getX()-tableLength , tableCenter.getY()-tableLength, tableCenter.getX()+tableLength, tableCenter.getY()+tableLength , tableCenter.getZ()); combinedTerrainObject.addBox(wallPosition.getX() - 1.0, wallPosition.getY() - 0.05, wallPosition.getX() + 1.0, wallPosition.getY() + 0.05, 2.0); return combinedTerrainObject; }
private void setColumn(Twist twist, FramePoint comPositionScaledByMass, double subTreeMass, int column) { tempVector.set(comPositionScaledByMass.getPoint()); tempVector.cross(twist.getAngularPart(), tempVector); tempJacobianColumn.set(tempVector); tempVector.set(twist.getLinearPart()); tempVector.scale(subTreeMass); tempJacobianColumn.add(tempVector); jacobianMatrix.set(0, column, tempJacobianColumn.getX()); jacobianMatrix.set(1, column, tempJacobianColumn.getY()); jacobianMatrix.set(2, column, tempJacobianColumn.getZ()); } }