private static Matrix3d flipX() { Matrix3d rot = new Matrix3d(); rot.m00 = 1; rot.m11 = -1; rot.m22 = -1; return rot; }
@Override protected Matrix3d initialValue() { return new Matrix3d(); } };
@Override public Matrix3d createIntegralGainMatrix() { return new Matrix3d(); }
@Override public Matrix3d initialValue() { return new Matrix3d(); } };
@Override protected Matrix3d initialValue() { return new Matrix3d(); } };
private void calcRotAxesAndAngles() { axisAngles = new AxisAngle4d[multiplicity]; // identity operator (transformId==0) axisAngles[0] = new AxisAngle4d(new Vector3d(0,0,0), 0.0); for (int i=1;i<this.transformations.size();i++){ Matrix3d r = new Matrix3d(transformations.get(i).m00,transformations.get(i).m01,transformations.get(i).m02, transformations.get(i).m10,transformations.get(i).m11,transformations.get(i).m12, transformations.get(i).m20,transformations.get(i).m21,transformations.get(i).m22); axisAngles[i] = getRotAxisAndAngle(r); } }
@Override public Matrix3d getRotationMatrix() { run(); Matrix3d m = new Matrix3d(); transformationMatrix.getRotationScale(m); return m; }
public Matrix3d getMTranspose() { if (Mtransp!=null){ return Mtransp; } Matrix3d M = getM(); Mtransp = new Matrix3d(); Mtransp.transpose(M); return Mtransp; }
private Matrix3d getMTransposeInv() { if (MtranspInv!=null){ return MtranspInv; } MtranspInv = new Matrix3d(); MtranspInv.invert(getMTranspose()); return MtranspInv; }
public static Matrix3d generateRandomMatrix3d(Random random, double maxAbsolute) { Matrix3d ret = new Matrix3d(); for (int row = 0; row < 3; row++) { for (int column = 0; column < 3; column++) { ret.setElement(row, column, generateRandomDouble(random, maxAbsolute)); } } return ret; }
public static Matrix3d generateRandomRotationMatrix3d(Random random) { Quat4d quaternion = generateRandomQuaternion(random); Matrix3d ret = new Matrix3d(); ret.set(quaternion); return ret; }
public Box3d(Point3d position, Quat4d orientation, double length, double width, double height) { setPose(position, orientation); dimensions = new EnumMap<Direction, Double>(Direction.class); faces = new EnumMap<FaceName, Plane3d>(FaceName.class); temporaryPoint = new Point3d(); temporaryMatrix = new Matrix3d(); commonConstructor(length, width, height); }
public void rotate(AxisAngle4d rotationAxisAngle) { Matrix3d rotation = new Matrix3d(); rotation.set(rotationAxisAngle); rotate(rotation); }
@Override public Matrix3d createIntegralGainMatrix() { Matrix3d integralGainMatrix = new Matrix3d(); for (int i = 0; i < 3; i++) { integralGains[i].addVariableChangedListener(new MatrixUpdater(i, i, integralGainMatrix)); integralGains[i].notifyVariableChangedListeners(); } return integralGainMatrix; }
@Override public Matrix3d createDerivativeGainMatrix() { Matrix3d derivativeGainMatrix = new Matrix3d(); for (int i = 0; i < 3; i++) { derivativeGain.addVariableChangedListener(new MatrixUpdater(i, i, derivativeGainMatrix)); } derivativeGain.notifyVariableChangedListeners(); return derivativeGainMatrix; }
@Override public Matrix3d createDerivativeGainMatrix() { Matrix3d derivativeGainMatrix = new Matrix3d(); derivativeYGain.addVariableChangedListener(new MatrixUpdater(1, 1, derivativeGainMatrix)); derivativeYGain.notifyVariableChangedListeners(); return derivativeGainMatrix; }
@Override public Matrix3d createProportionalGainMatrix() { Matrix3d proportionalGainMatrix = new Matrix3d(); for (int i = 0; i < 3; i++) { proportionalGains[i].addVariableChangedListener(new MatrixUpdater(i, i, proportionalGainMatrix)); proportionalGains[i].notifyVariableChangedListeners(); } return proportionalGainMatrix; }
public void transform(RigidBodyTransform transform) { Matrix3d rotation = new Matrix3d(); Vector3d translation = new Vector3d(); transform.get(rotation, translation); translate(translation); rotate(rotation); }
private void updateRootJointRotation(FloatingInverseDynamicsJoint rootJoint, FrameOrientation estimationLinkOrientation, ReferenceFrame estimationFrame) { tempOrientationEstimatinLink.setIncludingFrame(estimationLinkOrientation); computeEstimationLinkToWorldTransform(tempEstimationLinkToWorld, tempOrientationEstimatinLink); computeRootJointToWorldTransform(rootJoint, estimationFrame, tempRootJointToWorld, tempEstimationLinkToWorld); Matrix3d rootJointRotation = new Matrix3d(); tempRootJointToWorld.getRotation(rootJointRotation); rootJoint.setRotation(rootJointRotation); }
@Override public Matrix3d createDerivativeGainMatrix() { Matrix3d derivativeGainMatrix = new Matrix3d(); derivativeXYGain.addVariableChangedListener(new MatrixUpdater(0, 0, derivativeGainMatrix)); derivativeXYGain.addVariableChangedListener(new MatrixUpdater(1, 1, derivativeGainMatrix)); derivativeZGain.addVariableChangedListener(new MatrixUpdater(2, 2, derivativeGainMatrix)); derivativeXYGain.notifyVariableChangedListeners(); derivativeZGain.notifyVariableChangedListeners(); return derivativeGainMatrix; }