@Test public void interpolate() { Pose pose1 = Pose.from(Vector3.fromXYZ(2, 10, 0)); Pose pose2 = Pose.from(Vector3.fromXYZ(4, 12, 2)); assertThat(pose1.interpolate(pose2, 0.0)).isEqualTo(pose1); assertThat(pose2.interpolate(pose1, 0.0)).isEqualTo(pose2); assertThat(pose1.interpolate(pose2, 1.0)).isEqualTo(pose2); assertThat(pose2.interpolate(pose1, 1.0)).isEqualTo(pose1); assertThat(pose1.interpolate(pose2, 0.5)).isEqualTo(Pose.from(Vector3.fromXYZ(3, 11, 1))); }
public Pose interpolate(Pose pose, double percentage) { double angleDiff = AngleMath.difference(pose.orientation, orientation) * percentage; double intpOrientation = AngleMath.normalizeAngle(orientation + angleDiff); IVector2 posDiff = pose.pos.subtractNew(pos).multiply(percentage); IVector2 intpPos = pos.addNew(posDiff); return Pose.from(intpPos, intpOrientation); }
Pose implements IMirrorable<Pose> { public Pose interpolate(Pose pose, double percentage) { double angleDiff = AngleMath.difference(pose.orientation, orientation) * percentage; double intpOrientation = AngleMath.normalizeAngle(orientation + angleDiff); IVector2 posDiff = pose.pos.subtractNew(pos).multiply(percentage); IVector2 intpPos = pos.addNew(posDiff); return Pose.from(intpPos, intpOrientation); } }
Pose implements IMirrorable<Pose> { public Pose interpolate(Pose pose, double percentage) { double angleDiff = AngleMath.difference(pose.orientation, orientation) * percentage; double intpOrientation = AngleMath.normalizeAngle(orientation + angleDiff); IVector2 posDiff = pose.pos.subtractNew(pos).multiply(percentage); IVector2 intpPos = pos.addNew(posDiff); return Pose.from(intpPos, intpOrientation); } @SuppressWarnings("unused") // berkeley private  Pose(); private  Pose(final IVector2 pos, final double orientation); }
Pose implements IMirrorable<Pose> { public Pose interpolate(Pose pose, double percentage) { double angleDiff = AngleMath.difference(pose.orientation, orientation) * percentage; double intpOrientation = AngleMath.normalizeAngle(orientation + angleDiff); IVector2 posDiff = pose.pos.subtractNew(pos).multiply(percentage); IVector2 intpPos = pos.addNew(posDiff); return Pose.from(intpPos, intpOrientation); } @SuppressWarnings("unused") // berkeley private  Pose(); private  Pose(final IVector2 pos, final double orientation); static Pose from(final IVector2 pos, final double orientation); static Pose from(final IVector3 pose); static Pose zero(); @Override Pose mirrored(); Pose interpolate(Pose pose, double percentage); IVector2 getPos(); double getOrientation(); @Override boolean equals(final Object o); @Override int hashCode(); @Override String toString(); }
Pose implements IMirrorable<Pose> { public Pose interpolate(Pose pose, double percentage) { double angleDiff = AngleMath.difference(pose.orientation, orientation) * percentage; double intpOrientation = AngleMath.normalizeAngle(orientation + angleDiff); IVector2 posDiff = pose.pos.subtractNew(pos).multiply(percentage); IVector2 intpPos = pos.addNew(posDiff); return Pose.from(intpPos, intpOrientation); } @SuppressWarnings("unused") // berkeley private  Pose(); private  Pose(final IVector2 pos, final double orientation); static Pose from(final IVector2 pos, final double orientation); static Pose from(final IVector3 pose); static Pose zero(); @Override Pose mirrored(); Pose interpolate(Pose pose, double percentage); IVector2 getPos(); double getOrientation(); @Override boolean equals(final Object o); @Override int hashCode(); @Override String toString();  }