public void interpolate(FrameOrientation frameOrientation1, FrameOrientation frameOrientation2, double alpha) { checkReferenceFrameMatch(frameOrientation1); checkReferenceFrameMatch(frameOrientation2); frameOrientation.interpolate(frameOrientation1, frameOrientation2, alpha); frameOrientation.checkQuaternionIsUnitMagnitude(); getYoValuesFromFrameOrientation(); }
public void set(YoFrameQuaternion yoFrameQuaternion) { checkReferenceFrameMatch(yoFrameQuaternion); yoFrameQuaternion.getFrameOrientationIncludingFrame(frameOrientation); getYoValuesFromFrameOrientation(); }
public void set(FrameOrientation frameOrientation, boolean notifyListeners) { checkReferenceFrameMatch(frameOrientation); this.frameOrientation.setIncludingFrame(frameOrientation); getYoValuesFromFrameOrientation(notifyListeners); }
public void set(YoFrameQuaternion orientation) { orientation.checkReferenceFrameMatch(getReferenceFrame()); orientation.getFrameOrientationIncludingFrame(tempFrameOrientation); set(tempFrameOrientation); }
private void updateViz() { yoRootJointFrameQuaternion.checkReferenceFrameMatch(worldFrame); yoRootJointFrameQuaternion.set(rotationFromRootJointFrameToWorld); yoRootJointAngularVelocityFromFD.update(); yoRootJointFrameOrientation.checkReferenceFrameMatch(worldFrame); yoRootJointFrameOrientation.set(rotationFromRootJointFrameToWorld); }
/** * 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(YoFrameVector angularVelocityToPack, YoFrameQuaternion startOrientation, YoFrameQuaternion endOrientation, double alphaDot) { angularVelocityToPack.checkReferenceFrameMatch(startOrientation); startOrientation.checkReferenceFrameMatch(endOrientation); startOrientation.get(startRotationQuaternion); endOrientation.get(endRotationQuaternion); computeAngularVelocity(angularVelocity, startRotationQuaternion, endRotationQuaternion, alphaDot); angularVelocityToPack.set(angularVelocity); }