public static boolean containsNaN(Quat4d quat4d) { return (Double.isNaN(quat4d.getW()) || Double.isNaN(quat4d.getX()) || Double.isNaN(quat4d.getY()) || Double.isNaN(quat4d.getZ())); }
public void setVectorFromPureQuaternion(Quat4d pureQuaternion, Vector3d vectorToPack) { vectorToPack.set(pureQuaternion.getX(), pureQuaternion.getY(), pureQuaternion.getZ()); } }
/** * Rotates the given {@code tupleOriginal} tuple by a quaternion (qx, qy, qz, qs) and stores the result in the tuple {@code tupleTransformed}. * * @param quaternion the quaternion used to rotate the tuple. * @param tupleOriginal the original tuple. Not modified. * @param tupleTransformed the tuple in which the transformed {@code original} is stored. Modified. */ public static void rotateTuple3d(Quat4d quaternion, Tuple3d tupleOriginal, Tuple3d tupleTransformed) { rotateTuple3d(quaternion.getX(), quaternion.getY(), quaternion.getZ(), quaternion.getW(), tupleOriginal, tupleTransformed); }
/** * Convert quaternion to rotation matrix and store as rotational * component of this transform. * * @param quat */ public void setRotation(Quat4d quat) { setRotationWithQuaternion(quat.getX(), quat.getY(), quat.getZ(), quat.getW()); }
public static void convertQuaternionToYawPitchRoll(Quat4d quaternion, double[] yawPitchRollToPack) { convertQuaternionToYawPitchRoll(quaternion.getX(), quaternion.getY(), quaternion.getZ(), quaternion.getW(), yawPitchRollToPack); }
public static void packVectMathQuat4dInJMEQuaternion(Quat4d original, Quaternion target) { target.set((float) original.getX(), (float) original.getY(), (float) original.getZ(), (float) original.getW()); }
public MocapRigidBody(int id, Vector3d position, Quat4d orientation, ArrayList<MocapMarker> listOfAssociatedMarkers, boolean isTracked) { this.id = id; this.xPosition = (float) position.getX(); this.yPosition = (float) position.getY(); this.zPosition = (float) position.getZ(); this.qw = (float) orientation.getW(); this.qx = (float) orientation.getX(); this.qy = (float) orientation.getY(); this.qz = (float) orientation.getZ(); this.listOfAssociatedMarkers = listOfAssociatedMarkers; this.dataValid = isTracked; }
public void update(Quat4d newInputValue) { validityCheckers[0].update(newInputValue.getX()); validityCheckers[1].update(newInputValue.getY()); validityCheckers[2].update(newInputValue.getZ()); validityCheckers[3].update(newInputValue.getW()); }
public void get(double[] array) { array[0] = rotation.getW(); array[1] = rotation.getX(); array[2] = rotation.getY(); array[3] = rotation.getZ(); array[4] = translation.getX(); array[5] = translation.getY(); array[6] = translation.getZ(); twist.getArray(array, 7); }
private static void packQuat4dToGeometry_msgsQuaternion(Quat4d quat, Quaternion orientation) { orientation.setW(quat.getW()); orientation.setX(quat.getX()); orientation.setY(quat.getY()); orientation.setZ(quat.getZ()); } }
public static void insertQuat4dIntoEJMLVector(DenseMatrix64F matrix, Quat4d quaternion, int rowStart) { int index = rowStart; matrix.set(index++, 0, quaternion.getX()); matrix.set(index++, 0, quaternion.getY()); matrix.set(index++, 0, quaternion.getZ()); matrix.set(index++, 0, quaternion.getW()); }
public double dot(Quat4d quaternion) { double dot = this.quaternion.getX() * quaternion.getX(); dot += this.quaternion.getY() * quaternion.getY(); dot += this.quaternion.getZ() * quaternion.getZ(); dot += this.quaternion.getW() * quaternion.getW(); return dot; }
public void get(double[] array) { array[0] = rotation.getW(); array[1] = rotation.getX(); array[2] = rotation.getY(); array[3] = rotation.getZ(); array[4] = translation.getX(); array[5] = translation.getY(); array[6] = translation.getZ(); twist.getArray(array, 7); }
public static double computeQuaternionNormSquared(Quat4d quaternion) { return square(quaternion.getW()) + square(quaternion.getX()) + square(quaternion.getY()) + square(quaternion.getZ()); }
private void printFootstepConstructor(Footstep footstep) { RobotSide robotSide = footstep.getRobotSide(); Point3d position = new Point3d(); footstep.getPositionInWorldFrame(position); Quat4d orientation = new Quat4d(); footstep.getOrientationInWorldFrame(orientation); System.out.println("footsteps.add(footstepProviderTestHelper.createFootstep(RobotSide." + robotSide + ", new Point3d(" + position.getX() + ", " + position.getY() + ", " + position.getZ() + "), new Quat4d(" + orientation.getW() + ", " + orientation.getX() + ", " + orientation.getY() + ", " + orientation.getZ() + ")));"); } }
public void queueQuaternion(Quat4d quaternion) { quaternions.reshape(quaternions.getNumRows() + 1, 4, true); int lastIndex = quaternions.getNumRows() - 1; quaternions.set(lastIndex, 0, quaternion.getX()); quaternions.set(lastIndex, 1, quaternion.getY()); quaternions.set(lastIndex, 2, quaternion.getZ()); quaternions.set(lastIndex, 3, quaternion.getW()); }
public void get(double[] buffer, int offset) { inverseDynamicsJoint.getRotation(rotation); inverseDynamicsJoint.getTranslation(translation); inverseDynamicsJoint.getJointTwist(twist); buffer[offset + 0] = rotation.getW(); buffer[offset + 1] = rotation.getX(); buffer[offset + 2] = rotation.getY(); buffer[offset + 3] = rotation.getZ(); buffer[offset + 4] = translation.getX(); buffer[offset + 5] = translation.getY(); buffer[offset + 6] = translation.getZ(); twist.getArray(buffer, offset + 7); } }
private void setYawPitchRoll() { Quat4d q = new Quat4d(); //This code has a singularity when yaw and roll line up (e.g. pitch is 90, can't rotate in one direction any more). RotationTools.convertYawPitchRollToQuaternion(q_yaw.getDoubleValue(), q_pitch.getDoubleValue(), q_roll.getDoubleValue(), q); //This code compounds the rotations so that on subsequent frames the ability to rotate in lost rotation directions is regained //This affectively uses global yaw pitch and roll each time. q.mul(qprev); q_qs.set(q.getW()); q_qx.set(q.getX()); q_qy.set(q.getY()); q_qz.set(q.getZ()); }
private void setYawPitchRoll() { Quat4d q = new Quat4d(); //This code has a singularity when yaw and roll line up (e.g. pitch is 90, can't rotate in one direction any more). RotationTools.convertYawPitchRollToQuaternion(q_yaw.getDoubleValue(), q_pitch.getDoubleValue(), q_roll.getDoubleValue(), q); //This code compounds the rotations so that on subsequent frames the ability to rotate in lost rotation directions is regained //This affectively uses global yaw pitch and roll each time. q.mul(qprev); q_qs.set(q.getW()); q_qx.set(q.getX()); q_qy.set(q.getY()); q_qz.set(q.getZ()); }