Java 类javax.vecmath.Quat4d 实例源码

public void copyData()
{
    Quat4d dat = panel.getRender().getRotationAttr().getValue();

    // Transform3D rot = new Transform3D();
    // rot.setRotation(dat);

    AxisAngle4d rot = new AxisAngle4d();
    rot.set(dat);

    int rotX = (int) (dat.x * steps);
    int rotY = (int) (dat.y * steps);
    int rotZ = (int) (dat.z * steps);
    int rotW = (int) (dat.w * steps);

    setAllowUpdate(false);
    xSlide.setValue(rotX);
    ySlide.setValue(rotY);
    zSlide.setValue(rotZ);
    wManualSlide.setValue(rotW);
    setAllowUpdate(true);
}

@Override
public void set(String stringValue)
{
    int index;
    String doubleString;
    index = stringValue.indexOf('(');
    stringValue = stringValue.substring(index + 1);
    index = stringValue.indexOf(',');
    doubleString = stringValue.substring(0, index);
    double x = Double.valueOf(doubleString).doubleValue();
    stringValue = stringValue.substring(index + 1);
    index = stringValue.indexOf(',');
    doubleString = stringValue.substring(0, index);
    double y = Double.valueOf(doubleString).doubleValue();
    stringValue = stringValue.substring(index + 1);
    index = stringValue.indexOf(',');
    doubleString = stringValue.substring(0, index);
    double z = Double.valueOf(doubleString).doubleValue();
    stringValue = stringValue.substring(index + 1);
    index = stringValue.indexOf(')');
    doubleString = stringValue.substring(0, index);
    double w = Double.valueOf(doubleString).doubleValue();
    set(new Quat4d(x, y, z, w));
}

private FrameTransform convertTransformToPb(Transform t) {

        long timeMSec = t.getTime();
        long timeUSec = timeMSec * 1000l;

        Quat4d quat = t.getRotationQuat();
        Vector3d vec = t.getTranslation();

        FrameTransform.Builder tBuilder = FrameTransform.newBuilder();
        tBuilder.getTimeBuilder().setTime(timeUSec);
        tBuilder.setFrameChild(t.getFrameChild());
        tBuilder.setFrameParent(t.getFrameParent());
        tBuilder.getTransformBuilder().getRotationBuilder().setQw(quat.w);
        tBuilder.getTransformBuilder().getRotationBuilder().setQx(quat.x);
        tBuilder.getTransformBuilder().getRotationBuilder().setQy(quat.y);
        tBuilder.getTransformBuilder().getRotationBuilder().setQz(quat.z);
        tBuilder.getTransformBuilder().getTranslationBuilder().setX(vec.x);
        tBuilder.getTransformBuilder().getTranslationBuilder().setY(vec.y);
        tBuilder.getTransformBuilder().getTranslationBuilder().setZ(vec.z);

        return tBuilder.build();
    }

private Transform convertPbToTransform(FrameTransform t) {

        Timestamp time = t.getTime();
        long timeUSec = time.getTime();
        long timeMSec = timeUSec / 1000l;

        Rotation rstRot = t.getTransform().getRotation();
        Translation rstTrans = t.getTransform().getTranslation();

        Quat4d quat = new Quat4d(rstRot.getQx(), rstRot.getQy(), rstRot.getQz(), rstRot.getQw());
        Vector3d vec = new Vector3d(rstTrans.getX(), rstTrans.getY(), rstTrans.getZ());

        Transform3D transform3d = new Transform3D(quat, vec, 1.0);

        Transform newTrans = new Transform(transform3d, t.getFrameParent(), t.getFrameChild(),
                timeMSec);
        return newTrans;
    }

public static Quat4d yrp2q_marian(double roll, double pitch, double yaw) {

        // Assuming the angles are in radians.
        double cosYawHalf = Math.cos(yaw / 2);
        double sinYawHalf = Math.sin(yaw / 2);
        double cosPitchHalf = Math.cos(pitch / 2);
        double sinPitchHalf = Math.sin(pitch / 2);
        double cosRollHalf = Math.cos(roll / 2);
        double sinRollHalf = Math.sin(roll / 2);
        double cosYawPitchHalf = cosYawHalf * cosPitchHalf;
        double sinYawPitchHalf = sinYawHalf * sinPitchHalf;

        return new Quat4d((cosYawPitchHalf * cosRollHalf - sinYawPitchHalf * sinRollHalf),
                (cosYawPitchHalf * sinRollHalf + sinYawPitchHalf * cosRollHalf),
                (sinYawHalf * cosPitchHalf * cosRollHalf + cosYawHalf * sinPitchHalf * sinRollHalf),
                (cosYawHalf * sinPitchHalf * cosRollHalf - sinYawHalf * cosPitchHalf * sinRollHalf));
    }

/**
     * Multiply this transform by t1. This transform will be update
     * and the result returned
     * 
     * @param transform
     * @return this
     */
    public CellTransform mul(CellTransform in) {
        // This does not work when scale!=1
//        this.scale *= in.scale;
//        this.translation.addLocal(rotation.mult(in.translation).multLocal(in.scale));
//        this.rotation.multLocal(in.rotation);

        // Correctly calculate the multiplication.
        Quat4d q = new Quat4d(rotation.x, rotation.y, rotation.z, rotation.w);
        Vector3d t = new Vector3d(translation.x, translation.y, translation.z);
        Matrix4d m = new Matrix4d(q,t,scale);

        Quat4d q1 = new Quat4d(in.rotation.x, in.rotation.y, in.rotation.z, in.rotation.w);
        Vector3d t1 = new Vector3d(in.translation.x, in.translation.y, in.translation.z);
        Matrix4d m1 = new Matrix4d(q1,t1,in.scale);

        m.mul(m1);

        m.get(q);
        m.get(t);
        scale = (float)m.getScale();
        rotation.set((float)q.x, (float)q.y, (float)q.z, (float)q.w);
        translation.set((float)t.x, (float)t.y, (float)t.z);

        return this;
    }

public void serialize(StringBuffer buf) {
    logger.info("");
    buf.append("\n<camera>");
    Quat4d quat = new Quat4d();
    quat.set(attitude);
    buf.append("\n<attitude>");
    buf.append(String.valueOf(quat.x));
    buf.append(' ');
    buf.append(String.valueOf(quat.y));
    buf.append(' ');
    buf.append(String.valueOf(quat.z));
    buf.append(' ');
    buf.append(String.valueOf(quat.w));
    buf.append("</attitude>");
    buf.append("\n</camera>");
}

public void serialize(StringBuffer buf) {
    logger.info("");
    buf.append("\n<camera>");
    Quat4d quat = new Quat4d();
    quat.set(attitude);
    buf.append("\n<attitude>");
    buf.append(String.valueOf(quat.x));
    buf.append(' ');
    buf.append(String.valueOf(quat.y));
    buf.append(' ');
    buf.append(String.valueOf(quat.z));
    buf.append(' ');
    buf.append(String.valueOf(quat.w));
    buf.append("</attitude>");
    buf.append("\n</camera>");
}

/**
   * Construct a default instance of this node. The defaults are set by the
   * VRML specification.
   */
  public BaseGeoTransform() {
      super("GeoTransform");

      hasChanged = new boolean[NUM_FIELDS];

      vfGeoCenter = new double[3];
      vfRotation = new float[] {0, 0, 1, 0};
      vfScale = new float[] {1, 1, 1};
      vfScaleOrientation = new float[] {0, 0, 1, 0};
      vfTranslation = new float[] {0, 0, 0};
      vfGeoSystem = new String[] {"GD","WE"};

      localCenter = new double[3];
locMtx = new Matrix4f();

      tmatrix = new Matrix4f();

      tempVec = new Vector3f();
      tempVec3d = new Vector3d();
      tempAxis = new AxisAngle4f();
      tempMtx1 = new Matrix4f();
      tempMtx2 = new Matrix4f();
origQuat = new Quat4d();
rotQuat = new Quat4d();
  }

@Override
public void readExternal(ObjectInput in) throws IOException,
        ClassNotFoundException
{
    // double[] transformData = (double[]) in.readObject();
    // Transform3D transform = new Transform3D(transformData);
    // render.setXForm(transform);

    Quat4d rot = (Quat4d) in.readObject();
    render.getRotationAttr().set(rot);

    double dat = in.readDouble();
    render.getScaleAttr().set(dat);

    Vector3d vec = (Vector3d) in.readObject();
    render.getTranslationAttr().set(vec);

    // Copy CMAP data
    render.getColorModeAttr().colormaps[0] = (UserChoiceColorMap) in
            .readObject();

    render.getAnnotationsAttr().setValue(in.readBoolean());
    // render.displayAxisAttr.value = in.readInt();
    render.perspectiveAttr.setValue(in.readBoolean());
    render.coordSysAttr.setValue(in.readBoolean());

    render.rendererAttr.value = in.readInt();
    render.texColorMapAttr.setValue(in.readBoolean());
    render.getColorModeAttr().value = in.readInt();

    reloadColorMap(null);
}

public void updateRotation()
{
    double xRot = xSlide.getValue() / (double) (steps);
    double yRot = ySlide.getValue() / (double) (steps);
    double zRot = zSlide.getValue() / (double) (steps);
    double wRot = wManualSlide.getValue() / (double) (steps);

    wRot *= 3.14;
    // System.out.println(steps);
    // System.out.println(xSlide.getValue() + " - " + xRot);
    // System.out.println(ySlide.getValue() + " - " + yRot);
    // System.out.println(zSlide.getValue() + " - " + zRot);
    // System.out.println(wManualSlide.getValue() + " - " + wRot);

    Transform3D tra = new Transform3D();
    AxisAngle4d rotation = new AxisAngle4d();
    rotation.setX(xRot);
    rotation.setY(yRot);
    rotation.setZ(zRot);
    rotation.setAngle(wRot);

    tra.set(rotation);

    Quat4d dat = new Quat4d();
    tra.get(dat);
    // System.out.println("\nSetting Values\n" + dat.x);
    // System.out.println(dat.y);
    // System.out.println(dat.z);
    // System.out.println(dat.w);
    panel.getRender().getRotationAttr().set(tra);
    panel.getRender().restoreXform();

}

public Vector3d quatRotate(Quat4d r, Vector3d v) {
    Matrix3d rotMat = new Matrix3d();
    rotMat.set(r);
    Vector3d result = new Vector3d();
    rotMat.transform(v, result);
    return result;
}

public TransformInternal(Vector3d translation, Quat4d rotation, int frameNumber,
        int childFrameNumber, long time) {
    this.translation = translation;
    this.rotation = rotation;
    this.frame_id = frameNumber;
    this.child_frame_id = childFrameNumber;
    this.stamp = time;
}

Transform generateDefaultTransform() {
    Quat4d q = new Quat4d(0, 1, 2, 1);
    Vector3d v = new Vector3d(0, 1, 2);
    Transform3D t = new Transform3D(q,v,1);
    Transform transform = new Transform(t,"foo", "bar", 0);
    transform.setAuthority(TransformerCoreDefaultTest.class.getSimpleName());
    return transform;
}

/**
     * Invert the transform, this object is inverted and returned.
     *
     * @return return this
     */
    public CellTransform invert() {
        // This invert for jme does not function when the scale != 1
//        Matrix3f rot = new Matrix3f();
//        rot.set(rotation);
//        float temp;
//        temp=rot.m01;
//        rot.m01=rot.m10;
//        rot.m10=temp;
//        temp=rot.m02;
//        rot.m02=rot.m20;
//        rot.m20=temp;
//        temp=rot.m21;
//        rot.m21=rot.m12;
//        rot.m12=temp;
//        rot.multLocal(1/scale);
//
//        rot.multLocal(translation);
//
//        translation.multLocal(-1);
//        scale = 1/scale;
//
//        rotation.fromRotationMatrix(rot);

        // Correctly compute the inversion, use Vecmath as the matrix invert
        // in JME does not function when scale!=1
        Quat4d q = new Quat4d(rotation.x, rotation.y, rotation.z, rotation.w);
        Vector3d t = new Vector3d(translation.x, translation.y, translation.z);
        Matrix4d m = new Matrix4d(q,t,scale);
        m.invert();

        m.get(q);
        m.get(t);
        scale = (float)m.getScale();
        rotation.set((float)q.x, (float)q.y, (float)q.z, (float)q.w);
        translation.set((float)t.x, (float)t.y, (float)t.z);

        return this;
    }

private  void trackballRolled( MouseEvent e )
  {
      // get the new coordinates
      int newX = e .getX();
      int newY = e .getY();
      // the angle in degrees is just the pixel differences
      int angleX = newX - oldX;
      int angleY = newY - oldY;
      // set the old values
      oldX = newX;
      oldY = newY;

      double radians = ((double) angleY) * mSpeed;
      AxisAngle4d yAngle = new AxisAngle4d( new Vector3d( 1d, 0d, 0d ), radians );

      radians = ((double) angleX) * mSpeed;
      AxisAngle4d xAngle = new AxisAngle4d( new Vector3d( 0d, 1d, 0d ), radians );

Matrix4d x = new Matrix4d();
      x.set( xAngle );
Matrix4d y = new Matrix4d();
      y.set( yAngle );
      x .mul( y );
      Quat4d q = new Quat4d();
      x .get( q );

      trackballRolled( q );
  }

public AnimationCaptureController( CameraController cameraController, File directory )
{       
    this .iteration = NUM_ITERATIONS;
    double rotationRadians = 2 * Math.PI / NUM_ITERATIONS;

    this.cameraController = cameraController;
    Vector3f viewRotAxis = new Vector3f( 0f, 1.618f, 1f );
    this .cameraController .mapViewToWorld( viewRotAxis );
    this .rotation = new Quat4d();
    this .rotation .set( new AxisAngle4f( viewRotAxis, (float) rotationRadians ) );

    this .parentDir = directory .isDirectory()? directory : directory .getParentFile();
}

@Override
public MouseToolDefault getMouseTool()
{
    return new Trackball()
       {
           @Override
           protected void trackballRolled( Quat4d roll )
           {
               // TODO remove this?
           }

       };
}

@Override
public MouseToolDefault getMouseTool()
{
    return new Trackball()
       {
           @Override
           protected void trackballRolled( Quat4d roll )
           {
               // TODO remove this?
           }

       };
}

public void trackballRolled( Quat4d roll )
{
    mViewPlatform .getWorldRotation( roll );
    Matrix4d rollM = new Matrix4d();
    rollM.set( roll );
    // roll is now a rotation in world coordinates
    rollM.transform( zoneVector3d );
    mapVectorToAxis();
}

public void getWorldRotation( Quat4d q )
{
    Vector3d axis = new Vector3d( q.x, q.y, q.z );

    Matrix4d viewTrans = new Matrix4d();
    model .getViewTransform( viewTrans, 0d );
       viewTrans .invert();

    // now map the axis back to world coordinates
    viewTrans .transform( axis );
    q.x = axis.x; q.y = axis.y; q.z = axis.z;
}

public void addViewpointRotation( Quat4d rotation )
{
    Matrix3d m = new Matrix3d();
    m .set( rotation );
    m .invert();
    m .transform( mLookDirection );
    m .transform( mUpDirection );
}

private  void trackballRolled( MouseEvent e )
  {
      // get the new coordinates
      int newX = e .getX();
      int newY = e .getY();
      // the angle in degrees is just the pixel differences
      int angleX = newX - oldX;
      int angleY = newY - oldY;
      // set the old values
      oldX = newX;
      oldY = newY;

      double radians = ((double) angleY) * mSpeed;
      AxisAngle4d yAngle = new AxisAngle4d( new Vector3d( 1d, 0d, 0d ), radians );

      radians = ((double) angleX) * mSpeed;
      AxisAngle4d xAngle = new AxisAngle4d( new Vector3d( 0d, 1d, 0d ), radians );

Matrix4d x = new Matrix4d();
      x.set( xAngle );
Matrix4d y = new Matrix4d();
      y.set( yAngle );
      x .mul( y );
      Quat4d q = new Quat4d();
      x .get( q );

      trackballRolled( q );
  }

public AnimationCaptureController( CameraController cameraController, File directory )
{       
    this .iteration = NUM_ITERATIONS;
    double rotationRadians = 2 * Math.PI / NUM_ITERATIONS;

    this.cameraController = cameraController;
    Vector3f viewRotAxis = new Vector3f( 0f, 1.618f, 1f );
    this .cameraController .mapViewToWorld( viewRotAxis );
    this .rotation = new Quat4d();
    this .rotation .set( new AxisAngle4f( viewRotAxis, (float) rotationRadians ) );

    this .parentDir = directory .isDirectory()? directory : directory .getParentFile();
}

@Override
public MouseToolDefault getMouseTool()
{
    return new Trackball()
       {
           @Override
           protected void trackballRolled( Quat4d roll )
           {
               // TODO remove this?
           }

       };
}

@Override
public MouseToolDefault getMouseTool()
{
    return new Trackball()
       {
           @Override
           protected void trackballRolled( Quat4d roll )
           {
               // TODO remove this?
           }

       };
}

public void trackballRolled( Quat4d roll )
{
    mViewPlatform .getWorldRotation( roll );
    Matrix4d rollM = new Matrix4d();
    rollM.set( roll );
    // roll is now a rotation in world coordinates
    rollM.transform( zoneVector3d );
    mapVectorToAxis();
}

public void getWorldRotation( Quat4d q )
{
    Vector3d axis = new Vector3d( q.x, q.y, q.z );

    Matrix4d viewTrans = new Matrix4d();
    model .getViewTransform( viewTrans, 0d );
       viewTrans .invert();

    // now map the axis back to world coordinates
    viewTrans .transform( axis );
    q.x = axis.x; q.y = axis.y; q.z = axis.z;
}

/**
 * The orientation represents the rotation of the principal axes with
 * respect to the axes of the coordinate system (unit vectors [1,0,0],
 * [0,1,0] and [0,0,1]).
 * <p>
 * The orientation can be expressed as a unit quaternion.
 * 
 * @param points
 *            array of Point3d
 * @return the orientation of the point cloud as a unit quaternion
 */
public static Quat4d orientation(Point3d[] points) {

    MomentsOfInertia moi = new MomentsOfInertia();

    for (Point3d p : points)
        moi.addPoint(p, 1.0);

    // Convert rotation matrix to quaternion
    Quat4d quat = new Quat4d();
    quat.set(moi.getOrientationMatrix());

    return quat;
}

private void solve() {
        initialize();
        Vector3d trans = new Vector3d(subunits.getCentroid());
        trans.negate();
        List<Point3d[]> traces = subunits.getTraces();

//      Point3d[] x = SuperPosition.clonePoint3dArray(traces.get(0));
//      SuperPosition.center(x);
//      Point3d[] y = SuperPosition.clonePoint3dArray(traces.get(1));
//      SuperPosition.center(y);

        Point3d[] x = CalcPoint.clonePoint3dArray(traces.get(0));
        CalcPoint.translate(trans, x);
        Point3d[] y = CalcPoint.clonePoint3dArray(traces.get(1));
        CalcPoint.translate(trans, y);

        // TODO implement this piece of code using at origin superposition
        Quat4d quat = UnitQuaternions.relativeOrientation(
                x, y);
        AxisAngle4d axisAngle = new AxisAngle4d();
        Matrix4d transformation = new Matrix4d();

        transformation.set(quat);
        axisAngle.set(quat);

        Vector3d axis = new Vector3d(axisAngle.x, axisAngle.y, axisAngle.z);
        if (axis.lengthSquared() < 1.0E-6) {
            axisAngle.x = 0;
            axisAngle.y = 0;
            axisAngle.z = 1;
            axisAngle.angle = 0;
        } else {
            axis.normalize();
            axisAngle.x = axis.x;
            axisAngle.y = axis.y;
            axisAngle.z = axis.z;
        }

        CalcPoint.transform(transformation, y);

        // if rmsd or angle deviation is above threshold, stop
        double angleThresholdRadians = Math.toRadians(parameters.getAngleThreshold());
        double deltaAngle = Math.abs(Math.PI-axisAngle.angle);

        if (deltaAngle > angleThresholdRadians) {
            rotations.setC1(subunits.getSubunitCount());
            return;
        }

        // add unit operation
        addEOperation();

        // add C2 operation
        int fold = 2;
        combineWithTranslation(transformation);
        List<Integer> permutation = Arrays.asList(1,0);
        QuatSymmetryScores scores = QuatSuperpositionScorer.calcScores(subunits, transformation, permutation);
        scores.setRmsdCenters(0.0); // rmsd for superposition of two subunits centers is zero by definition

        if (scores.getRmsd() > parameters.getRmsdThreshold() || deltaAngle > angleThresholdRadians) {
            rotations.setC1(subunits.getSubunitCount());
            return;
        }

        Rotation symmetryOperation = createSymmetryOperation(permutation, transformation, axisAngle, fold, scores);
        rotations.addRotation(symmetryOperation);
    }

private Vector3d _rotate(Vector3d omega, Vector3d v) {
    double angle = omega.length() / this.w.fps / 2.0;
    double sin = Math.sin(angle);
    double cos = Math.cos(angle);

    // Convert the rotation vector into a rotation quaternion
    Quat4d q1 = new Quat4d(
            omega.x * sin,
            omega.y * sin,
            omega.z * sin,
            cos
    );
    q1.normalize();

    // Get the quaternion's conjugate
    Quat4d q2 = new Quat4d(q1);
    q2.conjugate();

    // Perform the rotation
    Quat4d rotated = new Quat4d();
    Quat4d qv = new Quat4d(v.x, v.y, v.z, 0);
    rotated.mul(q1, qv);
    rotated.mul(q2);

    // Get the X, Y and Z from the resulting rotated quaternion, and
    // resize the vector to its original length
    Vector3d ret = new Vector3d(rotated.x, rotated.y, rotated.z);
    ret.scale(v.length());

    return ret;
}

private Vector3d _rotate(Vector3d omega, Vector3d v) {
    double angle = omega.length() / this.w.fps / 2.0;
    double sin = Math.sin(angle);
    double cos = Math.cos(angle);

    // Convert the rotation vector into a rotation quaternion
    Quat4d q1 = new Quat4d(
            omega.x * sin,
            omega.y * sin,
            omega.z * sin,
            cos
        );
    q1.normalize();

    // Get the quaternion's conjugate
    Quat4d q2 = new Quat4d(q1);
    q2.conjugate();

    // Perform the rotation
    Quat4d rotated = new Quat4d();
    Quat4d qv = new Quat4d(v.x, v.y, v.z, 0);
    rotated.mul(q1, qv);
    rotated.mul(q2);

    // Get the X, Y and Z from the resulting rotated quaternion, and
    // resize the vector to its original length
    Vector3d ret = new Vector3d(rotated.x, rotated.y, rotated.z);
    ret.scale(v.length());

    return ret;
}

/**
 * Transform the orientation of the object to one in the local coordinate
 * system.
 */
private void getLocalOrientation(double[] position, AxisAngle4f axis) {
    posVec.x = position[0];
    posVec.y = position[1];
    posVec.z = position[2];

    double norm = posVec.x * posVec.x + posVec.y * posVec.y + posVec.z * posVec.z;

    if(norm != 0) {
        norm = 1 / Math.sqrt(norm);
        posVec.x *= norm;
        posVec.y *= norm;
        posVec.z *= norm;
    } else {
        posVec.x = 0.0f;
        posVec.y = 1.0f;
        posVec.z = 0.0f;
    }

    // Align Y and X axis
    double angle = YUP.angle(posVec);

    posVec.cross(YUP, posVec);

    axis.x = (float) posVec.x;
    axis.y = (float) posVec.y;
    axis.z = (float) posVec.z;
    axis.angle = (float) angle;

    angle = XUP.angle(posVec);

    posVec.cross(XUP, posVec);

    Quat4d orig = new Quat4d();
    orig.set(axis);
    Quat4d rot = new Quat4d();
    rot.set(new AxisAngle4d(posVec.x, posVec.y, posVec.z, angle));
    orig.mul(rot);
    axis.set(orig);
}

public Quat4d toJavaxD() {
    return new Quat4d(x, y, z, w);
}

QuatAttr(String label, Quat4d initValue)
{
    super(label);
    this.initValue.set(initValue);
    this.value.set(initValue);
}

public void set(Quat4d newValue)
{
    value.set(newValue);
}

public Quat4d getValue()
{
    return new Quat4d(value);
}

/**
 * Getter for the rotation part of the complete transform.
 * 
 * @return The rotation as Java3D Vecmath {@link Quat4d}
 */
public Quat4d getRotationQuat() {
    Quat4d quat = new Quat4d();
    transform.get(quat);
    return quat;
}

public boolean setTransform(Transform transform, boolean is_static)
        throws TransformerException {

    // prepare data
    String authority = transform.getAuthority();
    String frameChild = transform.getFrameChild().replace("/", "").trim();
    String frameParent = transform.getFrameParent().replace("/", "").trim();
    Quat4d quat = transform.getRotationQuat();
    Vector3d vec = transform.getTranslation();
    Transform stripped = new Transform(transform);
    stripped.setFrameChild(frameChild);
    stripped.setFrameParent(frameParent);

    // check input data validity
    if (frameChild.equals(frameParent)) {
        logger.error("Frames for parent and child are the same: "
                + frameChild);
        throw new TransformerException(
                "Frames for parent and child are the same: " + frameChild);
    }
    if (frameChild.isEmpty()) {
        logger.error("Child frame is empty");
        throw new TransformerException("Child frame is empty");
    }
    if (frameParent.isEmpty()) {
        logger.error("Parent frame is empty");
        throw new TransformerException("Parent frame is empty");
    }
    if (Double.isNaN(quat.w) || Double.isNaN(quat.x)
            || Double.isNaN(quat.y) || Double.isNaN(quat.z)
            || Double.isNaN(vec.x) || Double.isNaN(vec.y)
            || Double.isNaN(vec.z)) {
        logger.error("Transform contains nan: " + transform);
        throw new TransformerException("Transform contains nan: "
                + transform);
    }

    // perform the insertion
    synchronized (lock) {
        logger.debug("lookup child frame number");
        int frameNumberChild = lookupOrInsertFrameNumber(frameChild);
        logger.debug("get frame \"" + frameNumberChild + "\"");
        TransformCache frame = getFrame(frameNumberChild);
        if (!frame.isValid()) {
            logger.debug("allocate frame " + frameNumberChild);
            frame = allocateFrame(frameNumberChild, is_static);
        }

        logger.debug("lookup parent frame number");
        int frameNumberParent = lookupOrInsertFrameNumber(stripped
                .getFrameParent());
        logger.debug("insert transform " + frameNumberParent + " -> "
                + frameNumberChild + " to " + frame);
        if (frame.insertData(new TransformInternal(stripped,
                frameNumberParent, frameNumberChild))) {
            logger.debug("transform inserted. Add authority.");
            frameAuthority.put(frameNumberChild, authority);
        } else {
            logger.warn("TF_OLD_DATA ignoring data from the past for frame "
                    + stripped.getFrameChild()
                    + " at time "
                    + stripped.getTime()
                    + " according to authority "
                    + authority
                    + "\nPossible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained");
            return false;
        }
    }
    logger.debug("trigger check requests.");
    executor.execute(new Runnable() {
        public void run() {
            checkRequests();
        }
    });
    logger.debug("set transform done");
    return true;
}

public void trackballRolled( Quat4d roll )
{
    if ( point != null && ! usingWorkingPlane )
        zoneBall .trackballRolled( roll );  // some of these events will trigger the zone change
}