public YoFrameQuaternion findYoFrameQuaternion(String quaternionPrefix, String quaternionSuffix, ReferenceFrame quaternionFrame) { DoubleYoVariable qx = (DoubleYoVariable) scs.getVariable(createQxName(quaternionPrefix, quaternionSuffix)); DoubleYoVariable qy = (DoubleYoVariable) scs.getVariable(createQyName(quaternionPrefix, quaternionSuffix)); DoubleYoVariable qz = (DoubleYoVariable) scs.getVariable(createQzName(quaternionPrefix, quaternionSuffix)); DoubleYoVariable qs = (DoubleYoVariable) scs.getVariable(createQsName(quaternionPrefix, quaternionSuffix)); if (qx == null || qy == null || qz == null || qs == null) return null; else return new YoFrameQuaternion(qx, qy, qz, qs, quaternionFrame); } }
public YoFrameQuaternion(String namePrefix, String nameSuffix, ReferenceFrame referenceFrame, YoVariableRegistry registry) { this.namePrefix = namePrefix; this.nameSuffix = nameSuffix; this.qx = new DoubleYoVariable(createQxName(namePrefix, nameSuffix), registry); this.qy = new DoubleYoVariable(createQyName(namePrefix, nameSuffix), registry); this.qz = new DoubleYoVariable(createQzName(namePrefix, nameSuffix), registry); this.qs = new DoubleYoVariable(createQsName(namePrefix, nameSuffix), registry); this.referenceFrame = referenceFrame; qs.set(1.0); }
public YoFrameQuaternion findYoFrameQuaternion(String quaternionPrefix, String quaternionSuffix, ReferenceFrame quaternionFrame) { YoDouble qx = (YoDouble) scs.getVariable(createQxName(quaternionPrefix, quaternionSuffix)); YoDouble qy = (YoDouble) scs.getVariable(createQyName(quaternionPrefix, quaternionSuffix)); YoDouble qz = (YoDouble) scs.getVariable(createQzName(quaternionPrefix, quaternionSuffix)); YoDouble qs = (YoDouble) scs.getVariable(createQsName(quaternionPrefix, quaternionSuffix)); if (qx == null || qy == null || qz == null || qs == null) return null; else return new YoFrameQuaternion(qx, qy, qz, qs, quaternionFrame); } }
public YoFrameQuaternion findYoFrameQuaternion(String quaternionPrefix, String quaternionSuffix, ReferenceFrame quaternionFrame) { DoubleYoVariable qx = (DoubleYoVariable) scs.getVariable(createQxName(quaternionPrefix, quaternionSuffix)); DoubleYoVariable qy = (DoubleYoVariable) scs.getVariable(createQyName(quaternionPrefix, quaternionSuffix)); DoubleYoVariable qz = (DoubleYoVariable) scs.getVariable(createQzName(quaternionPrefix, quaternionSuffix)); DoubleYoVariable qs = (DoubleYoVariable) scs.getVariable(createQsName(quaternionPrefix, quaternionSuffix)); if (qx == null || qy == null || qz == null || qs == null) return null; else return new YoFrameQuaternion(qx, qy, qz, qs, quaternionFrame); } }
public static Quaternion findQuat4d(String nameSpace, String prefix, String suffix, SimulationConstructionSet scs) { double x = scs.getVariable(nameSpace, YoFrameVariableNameTools.createQxName(prefix, suffix)).getValueAsDouble(); double y = scs.getVariable(nameSpace, YoFrameVariableNameTools.createQyName(prefix, suffix)).getValueAsDouble(); double z = scs.getVariable(nameSpace, YoFrameVariableNameTools.createQzName(prefix, suffix)).getValueAsDouble(); double s = scs.getVariable(nameSpace, YoFrameVariableNameTools.createQsName(prefix, suffix)).getValueAsDouble(); return new Quaternion(x, y, z, s); }
/** * Finds the quaternion associated with the given namespace, prefix and suffix in scs. */ public static Quaternion findQuat4d(String nameSpace, String prefix, String suffix, SimulationConstructionSet scs) { double x = scs.getVariable(nameSpace, YoFrameVariableNameTools.createQxName(prefix, suffix)).getValueAsDouble(); double y = scs.getVariable(nameSpace, YoFrameVariableNameTools.createQyName(prefix, suffix)).getValueAsDouble(); double z = scs.getVariable(nameSpace, YoFrameVariableNameTools.createQzName(prefix, suffix)).getValueAsDouble(); double s = scs.getVariable(nameSpace, YoFrameVariableNameTools.createQsName(prefix, suffix)).getValueAsDouble(); return new Quaternion(x, y, z, s); }
private static YoFramePose3D findYoFramePose3D(String nameSpace, String namePrefix, String nameSuffix, SimulationConstructionSet scs) { YoDouble x = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createXName(namePrefix, nameSuffix)); YoDouble y = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createYName(namePrefix, nameSuffix)); YoDouble z = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createZName(namePrefix, nameSuffix)); YoFramePoint3D position = new YoFramePoint3D(x, y, z, ReferenceFrame.getWorldFrame()); YoDouble qx = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQxName(namePrefix, nameSuffix)); YoDouble qy = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQyName(namePrefix, nameSuffix)); YoDouble qz = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQzName(namePrefix, nameSuffix)); YoDouble qs = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQsName(namePrefix, nameSuffix)); YoFrameQuaternion orientation = new YoFrameQuaternion(qx, qy, qz, qs, ReferenceFrame.getWorldFrame()); return new YoFramePose3D(position, orientation); }