public void getAverageMatrix(Matrix3d averageRotationMatrixToPack) { averageRotationMatrixToPack.set(averageQuaternion); } }
@Override public void set(TransformableMatrix3d other) { super.set(other); }
public static void convertRotationVectorToMatrix(Vector3d rotationVector, Matrix3d rotationMatrixToPack) { AxisAngle4d localAxisAngle = axisAngleForRotationVectorConvertor.get(); convertRotationVectorToAxisAngle(rotationVector, localAxisAngle); rotationMatrixToPack.set(localAxisAngle); }
public static void integrateAngularVelocity(Vector3d angularVelocityToIntegrate, double integrationTime, Matrix3d orientationResultToPack) { AxisAngle4d axisAngleResult = axisAngleForIntegrator.get(); integrateAngularVelocity(angularVelocityToIntegrate, integrationTime, axisAngleResult); orientationResultToPack.set(axisAngleResult); }
public double getYaw() { tempMatrixForYawPitchRollConversion.set(quaternion); return RotationTools.computeYaw(tempMatrixForYawPitchRollConversion); }
public double getRoll() { tempMatrixForYawPitchRollConversion.set(quaternion); return RotationTools.computeRoll(tempMatrixForYawPitchRollConversion); }
public Matrix3d getMatrix3dCopy() { Matrix3d ret = new Matrix3d(); ret.set(quaternion); return ret; }
public void set(GeneralizedRigidBodyInertia other) { this.expressedInframe = other.expressedInframe; this.massMomentOfInertiaPart.set(other.massMomentOfInertiaPart); this.crossPart.set(other.crossPart); this.mass = other.mass; this.crossPartZero = other.crossPartZero; }
public void update(Quat4d currentOrientation) { currentOrientationMatrix.set(currentOrientation); update(currentOrientationMatrix); }
public void update(AxisAngle4d currentOrientation) { currentOrientationMatrix.set(currentOrientation); update(currentOrientationMatrix); }
public double getPitch() { tempMatrixForYawPitchRollConversion.set(quaternion); return RotationTools.computePitch(tempMatrixForYawPitchRollConversion); }
public static void convertMatrixToAxisAngle(Matrix3f rotationMatrix, AxisAngle4f axisAngle4fToPack) { Quat4d quat4d = threadLocalTemporaryQuaternion.get(); Matrix3d rotationMatrixDouble = rotationMatrixForConvertingFloatToDouble.get(); rotationMatrixDouble.set(rotationMatrix); convertMatrixToQuaternion(rotationMatrixDouble, quat4d); axisAngle4fToPack.set(quat4d); }
public static Matrix3d generateRandomRotationMatrix3d(Random random) { Quat4d quaternion = generateRandomQuaternion(random); Matrix3d ret = new Matrix3d(); ret.set(quaternion); return ret; }
public void rotate(AxisAngle4d rotationAxisAngle) { Matrix3d rotation = new Matrix3d(); rotation.set(rotationAxisAngle); rotate(rotation); }
public void rotate(AxisAngle4d rotationAxisAngle) { Matrix3d rotation = new Matrix3d(); rotation.set(rotationAxisAngle); rotate(rotation); }
public void corrupt(Matrix3d signal) { generateGaussianRotation(noiseAxisAngle, random, standardDeviation.getDoubleValue()); noiseRotationMatrix.set(noiseAxisAngle); signal.mul(noiseRotationMatrix); }
private void computeNormalContactVectorRotation(Matrix3d normalContactVectorRotationMatrixToPack) { yoPlaneContactState.getContactNormalFrameVector(contactNormalVector); contactNormalVector.changeFrame(planeFrame); contactNormalVector.normalize(); GeometryTools.getAxisAngleFromZUpToVector(contactNormalVector.getVector(), normalContactVectorRotation); normalContactVectorRotationMatrixToPack.set(normalContactVectorRotation); }
public void get(FloatingJoint joint) { rotationMatrix.set(rotation); joint.setRotation(rotationMatrix); joint.setPosition(translation); twist.getAngularPart(tempVector); joint.setAngularVelocityInBody(tempVector); twist.getLinearPart(tempVector); joint.setVelocity(tempVector); }
public void corrupt(Matrix3d signal) { yawDriftVelocity.add(yawDriftAcceleration.getDoubleValue() * dt); yawDriftAngle.add(yawDriftVelocity.getDoubleValue() * dt); yawDriftRotation.rotZ(yawDriftAngle.getDoubleValue()); tempRotation.mul(yawDriftRotation, signal); signal.set(tempRotation); corruptedIMUYawAngle.set(RotationTools.computeYaw(tempRotation)); } }