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());
 }
Ejemplo n.º 2
0
  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;
  }
Ejemplo n.º 3
0
 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);
  }
Ejemplo n.º 5
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);
 }
Ejemplo n.º 6
0
  // 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());
 }
Ejemplo n.º 10
0
 public static void inverse(Quat4d q, Quat4d src) {
   q.x = -src.x;
   q.y = -src.y;
   q.z = -src.z;
   q.w = src.w;
 }
Ejemplo n.º 11
0
 public static void inverse(Quat4d q) {
   q.x = -q.x;
   q.y = -q.y;
   q.z = -q.z;
 }
Ejemplo n.º 12
0
 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());
 }
Ejemplo n.º 15
0
 @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);
   }
 }