public void getOrientation(QuaternionBasics quaternionToPack) { if (isUsingYawPitchRoll()) yawPitchRoll.getQuaternion(quaternionToPack); else quaternionToPack.set(quaternion); }
public void setOrientation(FrameQuaternionReadOnly orientation) { if (isUsingYawPitchRoll()) yawPitchRoll.set(orientation); else quaternion.set(orientation); }
public void getYawPitchRoll(double[] yawPitchRollToPack) { if (isUsingYawPitchRoll()) yawPitchRoll.getYawPitchRoll(yawPitchRollToPack); else quaternion.getYawPitchRoll(yawPitchRollToPack); }
public void setOrientation(QuaternionReadOnly orientation) { if (isUsingYawPitchRoll()) yawPitchRoll.set(orientation); else quaternion.set(orientation); }
public void setYawPitchRoll(double[] yawPitchRoll) { if (isUsingYawPitchRoll()) this.yawPitchRoll.setYawPitchRoll(yawPitchRoll); else quaternion.setYawPitchRoll(yawPitchRoll); }
public void setYawPitchRoll(double yaw, double pitch, double roll) { if (isUsingYawPitchRoll()) yawPitchRoll.setYawPitchRoll(yaw, pitch, roll); else quaternion.setYawPitchRoll(yaw, pitch, roll); }
public void hide() { position.setToNaN(); if (isUsingYawPitchRoll()) yawPitchRoll.setToNaN(); else quaternion.setToNaN(); }
@Override public boolean containsNaN() { if (position.containsNaN()) return true; if (isUsingYawPitchRoll()) return yawPitchRoll.containsNaN(); else return quaternion.containsNaN(); }
public void setTransformToWorld(RigidBodyTransform transformToWorld) { position.set(transformToWorld.getTranslationVector()); if (isUsingYawPitchRoll()) yawPitchRoll.set(transformToWorld.getRotationMatrix()); else quaternion.set(transformToWorld.getRotationMatrix()); }
@Override public YoDouble[] getVariables() { YoDouble[] vars = new YoDouble[isUsingYawPitchRoll() ? 6 : 7]; int i = 0; vars[i++] = position.getYoX(); vars[i++] = position.getYoY(); vars[i++] = position.getYoZ(); if (isUsingYawPitchRoll()) { vars[i++] = yawPitchRoll.getYaw(); vars[i++] = yawPitchRoll.getPitch(); vars[i++] = yawPitchRoll.getRoll(); } else { vars[i++] = quaternion.getYoQx(); vars[i++] = quaternion.getYoQy(); vars[i++] = quaternion.getYoQz(); vars[i++] = quaternion.getYoQs(); } return vars; }
@Override public YoGraphicCoordinateSystem duplicate(YoVariableRegistry newRegistry) { if (isUsingYawPitchRoll()) return new YoGraphicCoordinateSystem(getName(), position.duplicate(newRegistry), yawPitchRoll.duplicate(newRegistry), scale, arrowColor); else return new YoGraphicCoordinateSystem(getName(), position.duplicate(newRegistry), quaternion.duplicate(newRegistry), scale, arrowColor); }
@Override protected void computeRotationTranslation(AffineTransform transform3D) { transform3D.setIdentity(); double globalScale = 1.0; if (globalScaleProvider != null) { globalScale = globalScaleProvider.getValue(); } transform3D.setScale(scale * globalScale); if (isUsingYawPitchRoll()) transform3D.setRotationYawPitchRoll(yawPitchRoll.getYaw().getValue(), yawPitchRoll.getPitch().getValue(), yawPitchRoll.getRoll().getValue()); else transform3D.setRotation(quaternion); transform3D.setTranslation(position); }