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>"); }
/** * 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? } }; }
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 ); }
/** * 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; }
/** * 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 }
