@Override public boolean geometricallyEquals(EuclideanWaypoint other, double epsilon) { if (!position.geometricallyEquals(other.position, epsilon)) return false; if (!linearVelocity.geometricallyEquals(other.linearVelocity, epsilon)) return false; return true; }
private void updatePointsAndVectors(Point3DReadOnly initialPosition, Point3DReadOnly finalPosition, double velocityMagnitude) { this.finalPosition.set(finalPosition); directionVector.set(finalPosition); directionVector.sub(initialPosition); directionVector.normalize(); velocityVector.set(directionVector); velocityVector.scale(velocityMagnitude); }
@Override public boolean epsilonEquals(EuclideanWaypoint other, double epsilon) { if (!position.epsilonEquals(other.position, epsilon)) return false; if (!linearVelocity.epsilonEquals(other.linearVelocity, epsilon)) return false; return true; }
/** * Sets the point and vector of this plane to zero. After calling this method, this plane becomes * invalid. A new valid point and valid normal will have to be set so this plane is again usable. */ @Override public void setToZero() { point.setToZero(); normal.setToZero(); hasPointBeenSet = false; hasNormalBeenSet = false; }
@Override public boolean equals(Object other) { if(other == null) return false; if(other == this) return true; if(!(other instanceof QuadrupedGroundPlaneMessage)) return false; QuadrupedGroundPlaneMessage otherMyClass = (QuadrupedGroundPlaneMessage) other; if(this.sequence_id_ != otherMyClass.sequence_id_) return false; if (!this.region_origin_.equals(otherMyClass.region_origin_)) return false; if (!this.region_normal_.equals(otherMyClass.region_normal_)) return false; return true; }
/** * Tests if this plane contains {@link Double#NaN}. * * @return {@code true} if {@code point} and/or {@code normal} contains {@link Double#NaN}, * {@code false} otherwise. */ @Override public boolean containsNaN() { return point.containsNaN() || normal.containsNaN(); }
@Override public void applyInverseTransform(Transform transform) { for (int i = 0; i < vertices.length; i++) vertices[i].applyInverseTransform(transform); for (int i = 0; i < vertexNormals.length; i++) vertexNormals[i].applyInverseTransform(transform); }
@Override public void applyTransform(Transform transform) { for (int i = 0; i < vertices.length; i++) vertices[i].applyTransform(transform); for (int i = 0; i < vertexNormals.length; i++) vertexNormals[i].applyTransform(transform); }
public EuclideanTrajectoryPointMessage() { position_ = new us.ihmc.euclid.tuple3D.Point3D(); linear_velocity_ = new us.ihmc.euclid.tuple3D.Vector3D(); }
@Override public boolean equals(Object other) { if(other == null) return false; if(other == this) return true; if(!(other instanceof EuclideanTrajectoryPointMessage)) return false; EuclideanTrajectoryPointMessage otherMyClass = (EuclideanTrajectoryPointMessage) other; if(this.sequence_id_ != otherMyClass.sequence_id_) return false; if(this.time_ != otherMyClass.time_) return false; if (!this.position_.equals(otherMyClass.position_)) return false; if (!this.linear_velocity_.equals(otherMyClass.linear_velocity_)) return false; return true; }
public QuadrupedGroundPlaneMessage() { region_origin_ = new us.ihmc.euclid.tuple3D.Point3D(); region_normal_ = new us.ihmc.euclid.tuple3D.Vector3D(); }
/** * Tests on a per component basis, if this plane 3D is exactly equal to {@code other}. * * @param other the other plane 3D to compare against this. Not modified. * @return {@code true} if the two planes are exactly equal component-wise, {@code false} otherwise. */ public boolean equals(Plane3D other) { if (other == null) return false; else return point.equals(other.point) && normal.equals(other.normal); }