Java Quat4d类代码示例

import javax.vecmath.Quat4d; //导入依赖的package包/类
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);
}
 

import javax.vecmath.Quat4d; //导入依赖的package包/类
@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));
}
 

import javax.vecmath.Quat4d; //导入依赖的package包/类
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();
	}
 

import javax.vecmath.Quat4d; //导入依赖的package包/类
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;
	}
 

import javax.vecmath.Quat4d; //导入依赖的package包/类
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));
    }
 

import javax.vecmath.Quat4d; //导入依赖的package包/类
/**
     * 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;
    }
 

import javax.vecmath.Quat4d; //导入依赖的package包/类
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>");
}
 

import javax.vecmath.Quat4d; //导入依赖的package包/类
/**
   * 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();
  }
 

import javax.vecmath.Quat4d; //导入依赖的package包/类
@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);
}
 

import javax.vecmath.Quat4d; //导入依赖的package包/类
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();

}
 

import javax.vecmath.Quat4d; //导入依赖的package包/类
public Vector3d quatRotate(Quat4d r, Vector3d v) {
	Matrix3d rotMat = new Matrix3d();
	rotMat.set(r);
	Vector3d result = new Vector3d();
	rotMat.transform(v, result);
	return result;
}
 

import javax.vecmath.Quat4d; //导入依赖的package包/类
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;
}
 

import javax.vecmath.Quat4d; //导入依赖的package包/类
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;
}
 

import javax.vecmath.Quat4d; //导入依赖的package包/类
/**
     * 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;
    }
 

import javax.vecmath.Quat4d; //导入依赖的package包/类
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 );
  }