/** * Packs an existing Vector3d with the angular part */ public void getAngularPart(Vector3d vectorToPack) { vectorToPack.set(this.angularPart); }
/** * Packs an existing Vector3d with the angular velocity part */ public void getAngularPart(Vector3d vectorToPack) { vectorToPack.set(this.angularPart); }
@Override public void setLinearVelocityToZero() { linearVelocity.set(0.0, 0.0, 0.0); }
@Override public void setAngularVelocity(Vector3d angularVelocity) { this.angularVelocity.set(angularVelocity); }
@Override public void setAngularVelocityToNaN() { angularVelocity.set(Double.NaN, Double.NaN, Double.NaN); }
@Override public void getAngularVelocity(Vector3d angularVelocityToPack) { angularVelocityToPack.set(angularVelocity); }
@Override public void setToZero() { super.set(0.0, 0.0, 0.0); }
@Override public void setToNaN() { super.set(Double.NaN, Double.NaN, Double.NaN); }
/** * Get the axis along which the variance is the least. * @param thirdVectorToPack */ public void getThirdVector(Vector3d thirdVectorToPack) { thirdVectorToPack.set(thirdAxis); }
@Override public void set(SO3Waypoint other) { orientation.set(other.orientation); angularVelocity.set(other.angularVelocity); }
@Override public void set(EuclideanWaypoint other) { position.set(other.position); linearVelocity.set(other.linearVelocity); }
private void surfaceNormalAt(Point3d pointToCheck, Vector3d normalToPack) { normalToPack.set(pointToCheck); GeometryTools.normalizeSafelyZUp(normalToPack); }
public static void integrateAngularVelocity(Vector3d angularVelocityToIntegrate, double integrationTime, AxisAngle4d orientationResultToPack) { Vector3d angularVelocityIntegrated = angularVelocityForIntegrator.get(); angularVelocityIntegrated.set(angularVelocityToIntegrate); angularVelocityIntegrated.scale(integrationTime); convertRotationVectorToAxisAngle(angularVelocityIntegrated, orientationResultToPack); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { tempTranslation.set(axis); tempTranslation.scale(position); transformToParent.setTranslationAndIdentityRotation(tempTranslation); } }
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 checkIfInsideShapeFrame(Point3d pointInWorldToCheck, Point3d closestPointToPack, Vector3d normalToPack) { boolean isInside = isInsideOrOnSurfaceShapeFrame(pointInWorldToCheck, Epsilons.ONE_TRILLIONTH); surfaceNormalAt(pointInWorldToCheck, normalToPack); temporaryVector.set(normalToPack); temporaryVector.scale(radius); closestPointToPack.set(temporaryVector); return isInside; }
public void set(ReferenceFrame expressedInFrame, DenseMatrix64F matrix) { MathTools.checkIfEqual(matrix.getNumRows(), SIZE); MathTools.checkIfEqual(matrix.getNumCols(), 1); this.expressedInFrame = expressedInFrame; this.angularPart.set(matrix.get(0, 0), matrix.get(1, 0), matrix.get(2, 0)); this.linearPart.set(matrix.get(3, 0), matrix.get(4, 0), matrix.get(5, 0)); }
public void applyTransform(RigidBodyTransform transform, boolean requireTransformInPlane) { temporaryTransformedVector.set(this.getX(), this.getY(), 0.0); transform.transform(temporaryTransformedVector); if (requireTransformInPlane) checkIsTransformationInPlane(temporaryTransformedVector); this.set(temporaryTransformedVector.getX(), temporaryTransformedVector.getY()); }