public static RigidBodyTransform yawPitchDegreesTransform(Vector3d center, double yawCCWDegrees, double pitchDownDegrees) { RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawCCWDegrees)); RigidBodyTransform tilt = new RigidBodyTransform(); tilt.setRotationPitchAndZeroTranslation(Math.toRadians(pitchDownDegrees)); location.multiply(tilt); location.setTranslation(center); return location; }
@Override public void applyTransform(RigidBodyTransform transform) { transform.transform(this.tuple); }
public static RigidBodyTransform getTransformFromA2toA1(RigidBodyTransform transformFromBtoA1, RigidBodyTransform transformFromBtoA2) { RigidBodyTransform temp = new RigidBodyTransform(transformFromBtoA2); temp.invert(); RigidBodyTransform ret = new RigidBodyTransform(transformFromBtoA1); ret.multiply(temp); return ret; }
/** * Set this transform to have translation described in translation and a rotation equal to the * Matrix3f matrix. * * @param matrix */ public final void set(Matrix3f matrix, Tuple3f translation) { setRotation(matrix); setTranslation(translation); }
/** * Convert and pack rotation part of transform into Quat4d and pack * translation into Tuple3d. * * @param quaternionToPack * @param translationToPack */ public void get(Quat4d quaternionToPack, Tuple3d translationToPack) { getRotation(quaternionToPack); getTranslation(translationToPack); }
public static Affine createAffineFromQuaternionAndTuple(Quat4d quaternion, Tuple3d translation) { RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotation(quaternion); transform.setTranslation(translation.getX(), translation.getY(), translation.getZ()); return convertRigidBodyTransformToAffine(transform); }
protected Point3f getPoint3f(int index, float range) { Point3f p = new Point3f(range, 0.0f, 0.0f); RigidBodyTransform transform = new RigidBodyTransform(); getInterpolatedTransform(index, transform); transform.multiply(getSweepTransform(index)); transform.transform(p); return p; }
public void shiftInSoleFrame(Vector2d shiftVector) { RigidBodyTransform shiftTransform = new RigidBodyTransform(); shiftTransform.setTranslation(new Vector3d(shiftVector.getX(), shiftVector.getY(), 0.0)); soleTransform.multiply(soleTransform, shiftTransform); }
public static RigidBodyTransform transformLocalZ(RigidBodyTransform originalTransform, double magnitude) { RigidBodyTransform transform = new RigidBodyTransform(originalTransform); Vector3d localZTranslation = new Vector3d(0.0, 0.0, magnitude); RigidBodyTransform postTranslation = new RigidBodyTransform(); postTranslation.setTranslationAndIdentityRotation(localZTranslation); transform.multiply(postTranslation); return transform; }
/** * Add a rotation to the current transform. */ public final void applyRotationY(double angle) { RigidBodyTransform temp = new RigidBodyTransform(); temp.setRotationPitchAndZeroTranslation(angle); multiply(temp); }
/** * Add a rotation to the current transform. */ public final void applyRotationZ(double angle) { RigidBodyTransform temp = new RigidBodyTransform(); temp.setRotationYawAndZeroTranslation(angle); multiply(temp); }
public static RigidBodyTransform createTranslationTransform(Vector3d translation) { RigidBodyTransform transform = new RigidBodyTransform(); transform.setTranslation(translation); return transform; }
private FootstepDataMessage adjustFootstepForAnkleHeight(FootstepDataMessage footstep){ FootstepDataMessage copy = new FootstepDataMessage(footstep); Point3d ankleOffset = new Point3d(0, 0, ankleHeight); RigidBodyTransform footTransform = new RigidBodyTransform(); footTransform.setRotationAndZeroTranslation(copy.getOrientation()); footTransform.transform(ankleOffset); copy.getLocation().add(ankleOffset); return copy; }
/** * Create RigidBodyTransform from AxisAngle4f and Tuple3f * * @param axisAngle * @param translation */ public RigidBodyTransform(AxisAngle4f axisAngle, Vector3f translation) { set(axisAngle, translation); }
public void corruptTransformToParentPreMultiply(RigidBodyTransform preCorruptionTransform) { if (this.preCorruptionTransform == null) { this.preCorruptionTransform = new RigidBodyTransform(); } this.preCorruptionTransform.set(preCorruptionTransform); this.update(); }
public void setSolePose(RigidBodyTransform transform) { if (transformFromAnkleToWorld == null) transformFromAnkleToWorld = new RigidBodyTransform(); transformFromAnkleToWorld.multiply(transform, transformFromAnkleToSoleFrame); this.ankleReferenceFrame.setPoseAndUpdate(transformFromAnkleToWorld); }
@Override public void applyTransform(RigidBodyTransform transform) { transform.transform(position); if (tempQuaternionForTransform == null) tempQuaternionForTransform = new Quat4d(); transform.getRotation(tempQuaternionForTransform); orientation.mul(tempQuaternionForTransform, orientation); transform.transform(linearVelocity); transform.transform(angularVelocity); }
public void getTransformToParent(RigidBodyTransform transformToParentToPack) { if (parentNode == null) { transformToParentToPack.setIdentity(); } parentNode.getSoleTransform(transformToParentToPack); transformToParentToPack.invert(); transformToParentToPack.multiply(transformToParentToPack, soleTransform); }