public FrameOrientation(FrameOrientation orientation) { super(orientation.getReferenceFrame(), new TransformableQuat4d(orientation.quaternion)); this.quaternion = this.getGeometryObject(); }
public OrientationFrame(FrameOrientation orientation) { super("Orientation_" + orientationNumber, orientation.getReferenceFrame(), false, false, false); orientationNumber++; frameOrientation = new FrameOrientation(orientation); this.update(); }
public void interpolate(FrameOrientation orientationOne, FrameOrientation orientationTwo, double alpha) { orientationOne.checkReferenceFrameMatch(orientationTwo); alpha = MathTools.clipToMinMax(alpha, 0.0, 1.0); quaternion.interpolate(orientationOne.quaternion, orientationTwo.quaternion, alpha); referenceFrame = orientationOne.getReferenceFrame(); }
public void setOrientationAndUpdate(FrameOrientation orientation) { this.parentFrame.checkReferenceFrameMatch(orientation.getReferenceFrame()); this.frameOrientation.set(orientation); this.update(); }
public FramePose(FramePoint position, FrameOrientation orientation) { this(position.getReferenceFrame(), new Pose(position.getGeometryObject(), orientation.getGeometryObject())); if (position.getReferenceFrame() != orientation.getReferenceFrame()) { throw new ReferenceFrameMismatchException("FramePose: The position frame (" + position.getReferenceFrame() + ") does not match the orientation frame (" + orientation.getReferenceFrame() + ")"); } }
public void setIncludingFrame(double time, FrameOrientation orientation, FrameVector angularVelocity) { orientation.checkReferenceFrameMatch(angularVelocity); setToZero(orientation.getReferenceFrame()); geometryObject.set(time, orientation.getQuaternion(), angularVelocity.getVector()); }
public void setIncludingFrame(FrameOrientation orientation, FrameVector angularVelocity) { orientation.checkReferenceFrameMatch(angularVelocity); setToZero(orientation.getReferenceFrame()); geometryObject.set(orientation.getQuaternion(), angularVelocity.getVector()); }
/** * Computes the angular velocity for an interpolation between two orientations using the SLERP method. * @param startOrientation the starting orientation * @param endOrientation the final orientation * @param alphaDot the interpolation rate * @return the angular velocity of the interpolated frame, w.r.t. the startOrientation, expressed in the frame in which the orientations were expressed */ public void computeAngularVelocity(FrameVector angularVelocityToPack, FrameOrientation startOrientation, FrameOrientation endOrientation, double alphaDot) { startOrientation.checkReferenceFrameMatch(endOrientation); ReferenceFrame frame = startOrientation.getReferenceFrame(); startOrientation.getQuaternion(startRotationQuaternion); endOrientation.getQuaternion(endRotationQuaternion); computeAngularVelocity(angularVelocity, startRotationQuaternion, endRotationQuaternion, alphaDot); angularVelocityToPack.setIncludingFrame(frame, angularVelocity); }