public void setYawPitchRoll(double yaw, double pitch, double roll) { Quat4d q = new Quat4d(); RotationTools.convertYawPitchRollToQuaternion(yaw, pitch, roll, q); RotationTools.checkQuaternionNormalized(q); q_qs.set(q.getW()); q_qx.set(q.getX()); q_qy.set(q.getY()); q_qz.set(q.getZ()); }
public static Vector3d quatRotate(Quat4d rotation, Vector3d v, Vector3d out) { Quat4d q = new Quat4d(rotation); QuaternionUtil.mul(q, v); Quat4d tmp = new Quat4d(); inverse(tmp, rotation); q.mul(tmp); out.set(q.x, q.y, q.z); return out; }
public static void setEuler(Quat4d q, double yaw, double pitch, double roll) { double halfYaw = yaw * 0.5f; double halfPitch = pitch * 0.5f; double halfRoll = roll * 0.5f; double cosYaw = (double) Math.cos(halfYaw); double sinYaw = (double) Math.sin(halfYaw); double cosPitch = (double) Math.cos(halfPitch); double sinPitch = (double) Math.sin(halfPitch); double cosRoll = (double) Math.cos(halfRoll); double sinRoll = (double) Math.sin(halfRoll); q.x = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw; q.y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw; q.z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw; q.w = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw; }
// private Matrix3d tempRotationMatrix = new Matrix3d(); protected void setFloatingTransform3D(RigidBodyTransform t1) { // position.set(q_x.val + offset.x, q_y.val + offset.y, q_z.val + offset.z); tempPosition1.set(q_x.getDoubleValue(), q_y.getDoubleValue(), q_z.getDoubleValue()); tempOrientation1.set( q_qx.getDoubleValue(), q_qy.getDoubleValue(), q_qz.getDoubleValue(), q_qs.getDoubleValue()); t1.set(tempOrientation1, tempPosition1); // // An alternate way is to hardcode from // http://www.genesis3d.com/~kdtop/Quaternions-UsingToRepresentRotation.htm (except do the // transpose): // double w2 = q_qs.val * q_qs.val; // double x2 = q_qx.val * q_qx.val; // double y2 = q_qy.val * q_qy.val; // double z2 = q_qz.val * q_qz.val; // // double wx = q_qs.val * q_qx.val; // double wy = q_qs.val * q_qy.val; // double wz = q_qs.val * q_qz.val; // // double xy = q_qx.val * q_qy.val; // double xz = q_qx.val * q_qz.val; // double yz = q_qy.val * q_qz.val; // // //// Matrix3d rotationMatrix = new Matrix3d // // tempRotationMatrix = new Matrix3d( // w2+x2-y2-z2, 2.0*(xy - wz), 2.0*(xz + wy), // 2.0*(xy+wz), w2-x2+y2-z2, 2.0*(yz-wx), // 2.0*(xz-wy), 2.0*(yz+wx), w2-x2-y2+z2 // ); // // t1.set(tempRotationMatrix, tempPosition1, 1.0); }
public static void mul(Quat4d q, Vector3d w) { double rx = q.w * w.x + q.y * w.z - q.z * w.y; double ry = q.w * w.y + q.z * w.x - q.x * w.z; double rz = q.w * w.z + q.x * w.y - q.y * w.x; double rw = -q.x * w.x - q.y * w.y - q.z * w.z; q.set(rx, ry, rz, rw); }
// Game Programming Gems 2.10. make sure v0,v1 are normalized public static Quat4d shortestArcQuat(Vector3d v0, Vector3d v1, Quat4d out) { Vector3d c = new Vector3d(); c.cross(v0, v1); double d = v0.dot(v1); if (d < -1.0 + BulletGlobals.FLT_EPSILON) { // just pick any vector out.set(0.0f, 1.0f, 0.0f, 0.0f); return out; } double s = (double) Math.sqrt((1.0f + d) * 2.0f); double rs = 1.0f / s; out.set(c.x * rs, c.y * rs, c.z * rs, s * 0.5f); return out; }
protected void jointDependentSetAndGetRotation(Matrix3d Rh_i) { tempOrientation2.set( owner.q_qx.getDoubleValue(), owner.q_qy.getDoubleValue(), owner.q_qz.getDoubleValue(), owner.q_qs.getDoubleValue()); Rh_i.set(tempOrientation2); }
public void getQuaternion(Quat4d quaternionToPack) { quaternionToPack.set( q_qx.getDoubleValue(), q_qy.getDoubleValue(), q_qz.getDoubleValue(), q_qs.getDoubleValue()); }
public void setQuaternion(Quat4d q) { q_qs.set(q.getW()); q_qx.set(q.getX()); q_qy.set(q.getY()); q_qz.set(q.getZ()); }
public static void inverse(Quat4d q, Quat4d src) { q.x = -src.x; q.y = -src.y; q.z = -src.z; q.w = src.w; }
public static void inverse(Quat4d q) { q.x = -q.x; q.y = -q.y; q.z = -q.z; }
public static void setRotation(Quat4d q, Vector3d axis, double angle) { double d = axis.length(); assert (d != 0f); double s = (double) Math.sin(angle * 0.5f) / d; q.set(axis.x * s, axis.y * s, axis.z * s, (double) Math.cos(angle * 0.5f)); }
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 packRosQuaternionToQuat4d(Quaternion rosQuat, Quat4d quat) { quat.setX(rosQuat.getX()); quat.setY(rosQuat.getY()); quat.setZ(rosQuat.getZ()); quat.setW(rosQuat.getW()); }
@Override @SuppressWarnings("rawtypes") public void processStimulus(Enumeration criteria) { // examiner criteria � while (criteria.hasMoreElements()) { WakeupOnAWTEvent w = (WakeupOnAWTEvent) criteria.nextElement(); AWTEvent events[] = w.getAWTEvent(); synchronized (deltaT) { synchronized (deltaR) { for (int i = 0; i < events.length; i++) { if (events[i].getID() == KeyEvent.KEY_PRESSED) { int k = ((KeyEvent) events[i]).getKeyCode(); if (k == KeyEvent.VK_UP) { deltaT.z = -0.1; } else if (k == KeyEvent.VK_DOWN) { deltaT.z = 0.1; } else if (k == KeyEvent.VK_PAGE_UP) { deltaT.y = 0.1; } else if (k == KeyEvent.VK_PAGE_DOWN) { deltaT.y = -0.1; } else if (k == KeyEvent.VK_LEFT) { deltaR.set(new AxisAngle4d(new Vector3d(0, 1, 0), 0.02)); } else if (k == KeyEvent.VK_RIGHT) { deltaR.set(new AxisAngle4d(new Vector3d(0, 1, 0), -0.02)); } } else if (events[i].getID() == KeyEvent.KEY_RELEASED) { int k = ((KeyEvent) events[i]).getKeyCode(); if (k == KeyEvent.VK_UP) { deltaT.z = 0; } else if (k == KeyEvent.VK_DOWN) { deltaT.z = 0; } else if (k == KeyEvent.VK_PAGE_UP) { deltaT.y = 0; } else if (k == KeyEvent.VK_PAGE_DOWN) { deltaT.y = 0; } else if (k == KeyEvent.VK_LEFT) { deltaR.set(new AxisAngle4d(new Vector3d(0, 1, 0), 0.0)); } else if (k == KeyEvent.VK_RIGHT) { deltaR.set(new AxisAngle4d(new Vector3d(0, 1, 0), 0.0)); } } else if (events[i].getID() == MouseEvent.MOUSE_WHEEL) { MouseWheelEvent mwe = ((MouseWheelEvent) events[i]); // deltaT.y = deltaT.y + mwe.getPreciseWheelRotation () / 10 ; // } else if ((events [i].getID () == MouseEvent.MOUSE_DRAGGED) || // (events [i].getID () == MouseEvent.MOUSE_PRESSED)) { // MouseEvent me = ((MouseEvent)events [i]) ; // int width = me.getComponent ().getWidth () ; // int height = me.getComponent ().getHeight () ; // int x = me.getX () ; // int y = me.getY () ; // double azimuth = Math.atan2 (width / 2.0 - x, 500.0) ; // double elevation = Math.atan2 (height / 2.0 - y, 500.0) ; // Quat4d azimuthR = new Quat4d () ; // azimuthR.set (new AxisAngle4d (0, 1, 0, azimuth)) ; // Quat4d elevationR = new Quat4d () ; // elevationR.set (new AxisAngle4d (1, 0, 0, elevation)) ; // absoluteR.mul (azimuthR, elevationR) ; // navigator.setHeadOrientationInSupportFrame (absoluteR.x, // absoluteR.y, absoluteR.z, absoluteR.w); } else if (events[i].getID() == MouseEvent.MOUSE_PRESSED) { initHeadR = navigator.getHeadRotationInSupportFrame(); MouseEvent me = ((MouseEvent) events[i]); xInit = me.getX(); yInit = me.getY(); } else if (events[i].getID() == MouseEvent.MOUSE_DRAGGED) { MouseEvent me = ((MouseEvent) events[i]); int dx = me.getX() - xInit; int dy = me.getY() - yInit; double azimuth = Math.atan2(-dx, 500.0); double elevation = Math.atan2(-dy, 500.0); Quat4d azimuthR = new Quat4d(); azimuthR.set(new AxisAngle4d(0, 1, 0, azimuth)); Quat4d elevationR = new Quat4d(); elevationR.set(new AxisAngle4d(1, 0, 0, elevation)); Quat4d relativeR = new Quat4d(); relativeR.mul(azimuthR, elevationR); absoluteR.mul(initHeadR, relativeR); navigator.setHeadOrientationInSupportFrame( absoluteR.x, absoluteR.y, absoluteR.z, absoluteR.w); } } } } wakeupOn(wEvents); } }