@Override protected Matrix3d initialValue() { return new Matrix3d(); } };
/** * Returns the vertices of an n-fold polygon of given radius and center * @return */ public static Point3d[] getPolygonVertices(int n, double radius, Point3d center) { Point3d[] polygon = new Point3d[n]; Matrix3d m = new Matrix3d(); for (int i = 0; i < n; i++) { polygon[i] = new Point3d(0, radius, 0); m.rotZ(i*2*Math.PI/n); m.transform(polygon[i]); polygon[i].add(center); } return polygon; }
Vector3d vector = ... //a vector to be transformed double x=2, y=0.12; Matrix3d rotMat = new Matrix3d(1,0,0, 0,1,0, 0,0,1); //identity matrix rotMat.rotX(x); //rotation on X axis rotMat.transform(vector); rotMat.rotY(y); // rotation on Y axis rotMat.transform(vector); // the vector should now have both x and y rotation // transformations applied System.out.println("Rotated vector :\n" + vector);
@Override public Matrix3d getViewMatrix(int index) { Matrix3d m = new Matrix3d(); switch (index) { case 0: m.setIdentity(); // front vertex-centered break; case 1: m.rotX(Math.PI); // back face-centered break; case 2: double angle = Math.PI - 0.5 * TETRAHEDRAL_ANGLE; // Side edge-centered m.rotX(angle); break; default: throw new IllegalArgumentException("getViewMatrix: index out of range:" + index); } return m; }
x.transpose(rotationMatrixToOrthogonalize); mx.setM00(x.getM00() * x.getM00() + x.getM10() * x.getM10() + x.getM20() * x.getM20()); mx.setM10(x.getM01() * x.getM00() + x.getM11() * x.getM10() + x.getM21() * x.getM20()); mx.setM20(x.getM02() * x.getM00() + x.getM12() * x.getM10() + x.getM22() * x.getM20()); mx.setM01(x.getM00() * x.getM01() + x.getM10() * x.getM11() + x.getM20() * x.getM21()); mx.setM11(x.getM01() * x.getM01() + x.getM11() * x.getM11() + x.getM21() * x.getM21()); mx.setM21(x.getM02() * x.getM01() + x.getM12() * x.getM11() + x.getM22() * x.getM21()); mx.setM02(x.getM00() * x.getM02() + x.getM10() * x.getM12() + x.getM20() * x.getM22()); mx.setM12(x.getM01() * x.getM02() + x.getM11() * x.getM12() + x.getM21() * x.getM22()); mx.setM22(x.getM02() * x.getM02() + x.getM12() * x.getM12() + x.getM22() * x.getM22()); rotationMatrixResultToPack.setElement(0, 0, x.getM00() - 0.5 * (x.getM00() * mx.getM00() + x.getM01() * mx.getM10() + x.getM02() * mx.getM20() - x.getM00())); rotationMatrixResultToPack.setElement(1, 0, x.getM01() - 0.5 * (x.getM00() * mx.getM01() + x.getM01() * mx.getM11() + x.getM02() * mx.getM21() - x.getM01())); rotationMatrixResultToPack.setElement(2, 0, x.getM02() - 0.5 * (x.getM00() * mx.getM02() + x.getM01() * mx.getM12() + x.getM02() * mx.getM22() - x.getM02())); rotationMatrixResultToPack.setElement(0, 1, x.getM10() - 0.5 * (x.getM10() * mx.getM00() + x.getM11() * mx.getM10() + x.getM12() * mx.getM20() - x.getM10())); rotationMatrixResultToPack.setElement(1, 1, x.getM11() - 0.5 * (x.getM10() * mx.getM01() + x.getM11() * mx.getM11() + x.getM12() * mx.getM21() - x.getM11())); rotationMatrixResultToPack.setElement(2, 1, x.getM12() - 0.5 * (x.getM10() * mx.getM02() + x.getM11() * mx.getM12() + x.getM12() * mx.getM22() - x.getM12())); rotationMatrixResultToPack.setElement(0, 2, x.getM20() - 0.5 * (x.getM20() * mx.getM00() + x.getM21() * mx.getM10() + x.getM22() * mx.getM20() - x.getM20())); rotationMatrixResultToPack.setElement(1, 2, x.getM21() - 0.5 * (x.getM20() * mx.getM01() + x.getM21() * mx.getM11() + x.getM22() * mx.getM21() - x.getM21())); rotationMatrixResultToPack.setElement(2, 2, x.getM22() - 0.5 * (x.getM20() * mx.getM02() + x.getM21() * mx.getM12() + x.getM22() * mx.getM22() - x.getM22())); corr.setM00(rotationMatrixResultToPack.getElement(0, 0) - x.getM00()); corr.setM01(rotationMatrixResultToPack.getElement(1, 0) - x.getM01()); corr.setM02(rotationMatrixResultToPack.getElement(2, 0) - x.getM02()); corr.setM10(rotationMatrixResultToPack.getElement(0, 1) - x.getM10()); corr.setM11(rotationMatrixResultToPack.getElement(1, 1) - x.getM11()); corr.setM12(rotationMatrixResultToPack.getElement(2, 1) - x.getM12()); corr.setM20(rotationMatrixResultToPack.getElement(0, 2) - x.getM20());
@Override public Matrix3d getViewMatrix(int index) { Matrix3d m = new Matrix3d(); switch (index) { case 0: m.setIdentity(); // front vertex-centered break; case 1: m.rotX(-0.6523581397843639); // back face-centered -0.5535743588970415 m.rotX(Math.toRadians(-26)); break; case 2: m.rotZ(Math.PI/2); Matrix3d m1 = new Matrix3d(); m1.rotX(-1.0172219678978445); m.mul(m1); break; default: throw new IllegalArgumentException("getViewMatrix: index out of range:" + index); } return m; }
@Override public Matrix3d getViewMatrix(int index) { Matrix3d m = new Matrix3d(); switch (index) { case 0: m.setIdentity(); // front break; case 1: m.rotX(Math.PI/2); // side edge-centered break; case 2: m.rotY(Math.PI/n); // side face-centered Matrix3d m1 = new Matrix3d(); m1.rotX(Math.PI/2); m.mul(m1); break; case 3: m.set(flipX()); // back break; default: throw new IllegalArgumentException("getViewMatrix: index out of range:" + index); } return m; }
private void calculateFramedMOITensor() { framedMOI = RotationMatrices.getZeroMatrix(3); Matrix3d pitch = new Matrix3d(); Matrix3d yaw = new Matrix3d(); Matrix3d roll = new Matrix3d(); pitch.rotX(Math.toRadians(parent.wrapper.pitch)); yaw.rotY(Math.toRadians(parent.wrapper.yaw)); roll.rotZ(Math.toRadians(parent.wrapper.roll)); pitch.mul(yaw); pitch.mul(roll); pitch.normalize(); Matrix3d inertiaBodyFrame = new Matrix3d(MoITensor); Matrix3d multipled = new Matrix3d(); multipled.mul(pitch, inertiaBodyFrame); pitch.transpose(); multipled.mul(pitch); framedMOI[0] = multipled.m00; framedMOI[1] = multipled.m01; framedMOI[2] = multipled.m02; framedMOI[3] = multipled.m10; framedMOI[4] = multipled.m11; framedMOI[5] = multipled.m12; framedMOI[6] = multipled.m20; framedMOI[7] = multipled.m21; framedMOI[8] = multipled.m22; invFramedMOI = RotationMatrices.inverse3by3(framedMOI); }
/** * Rotates the coordinate system counter clockwise around the specified axis by the given * angle in radians. This does not rotate existing graphics, instead it rotates a "cursor" * when another object is added it will be centered on the origin of the current system * as described by the translations and rotations applied since its creation at the joint * origin. * * @param rotationAngle the angle to rotate around the specified axis in radians. * @param rotationAxis Axis around which to rotate. Either Link.X, Link.Y or Link.Z */ public void rotate(double rotationAngle, Axis rotationAxis) { Matrix3d rot = new Matrix3d(); if (rotationAxis == Axis.X) rot.rotX(rotationAngle); else if (rotationAxis == Axis.Y) rot.rotY(rotationAngle); else if (rotationAxis == Axis.Z) rot.rotZ(rotationAngle); rotate(rot); }
/** * Returns the vertices of an n-fold polygon of given radius and center * @param n * @param radius * @param center * @return */ @Override public Point3d[] getVertices() { double x = getSideLengthFromCircumscribedRadius(circumscribedRadius)/2; double z = x/Math.sqrt(2); Point3d[] tetrahedron = new Point3d[4]; tetrahedron[0] = new Point3d(-x, 0, -z); tetrahedron[1] = new Point3d( x, 0, -z); tetrahedron[2] = new Point3d( 0, -x, z); tetrahedron[3] = new Point3d( 0, x, z); Point3d centroid = CalcPoint.centroid(tetrahedron); // rotate tetrahedron to align one vertex with the +z axis Matrix3d m = new Matrix3d(); m.rotX(0.5 * TETRAHEDRAL_ANGLE); for (Point3d p: tetrahedron) { p.sub(centroid); m.transform(p); } return tetrahedron; };
private void createButton(String buttonRobotName, Point3d buttonLocation, Vector3d pushVector, double forceVectorScale) { Matrix3d zRotation = new Matrix3d(); Matrix3d yRotation = new Matrix3d(); Matrix3d rotation = new Matrix3d(); pushVector.normalize(); double alpha = Math.atan(pushVector.getY() / pushVector.getX()); double beta = (Math.PI / 2.0) - Math.atan(pushVector.getZ() / Math.sqrt(Math.pow(pushVector.getX(), 2.0) + Math.pow(pushVector.getY(),2.0))); zRotation.rotZ(alpha); yRotation.rotY(beta); rotation.mul(zRotation, yRotation); RigidBodyTransform rootBodyTransform = new RigidBodyTransform(rotation, new Vector3d(buttonLocation)); ContactableButtonRobot button = new ContactableButtonRobot(buttonRobotName, rootBodyTransform, pushVector); button.createButtonRobot(); button.createAvailableContactPoints(1, NUMBER_OF_CONTACT_POINTS, forceVectorScale, true); buttonRobots.add(button); }
private ArrayList<Point2d> getPredictedContactPointsForFootstep(FootstepDataMessage footstepData, List<Point3d> points, double distanceTolerance) Matrix3d rotationMatrix = new Matrix3d(); rotationMatrix.set(footstepData.getOrientation()); Vector3d footNormal = new Vector3d(); rotationMatrix.getColumn(2, footNormal); Plane3d footPlane = new Plane3d(footstepData.getLocation(), footNormal); rotationMatrix.transpose(); tempPoint.setZ(footPlane.getZOnPlane(tempPoint.getX(), tempPoint.getY())); tempPoint.sub(footstepData.getLocation()); rotationMatrix.transform(tempPoint);
/** * Rotates a vector around an axis. * *@param vector vector to be rotated around axis *@param axis axis of rotation *@param angle angle to vector rotate around *@return rotated vector *author: egonw */ public static Vector3d rotate(Vector3d vector, Vector3d axis, double angle) { Matrix3d rotate = new Matrix3d(); rotate.set(new AxisAngle4d(axis.x, axis.y, axis.z, angle)); Vector3d result = new Vector3d(); rotate.transform(vector, result); return result; }
public static void integrate(Matrix3d rotationToPack, Tuple3d positionToPack, double dt, Twist twist) { twist.changeFrame(twist.getBodyFrame()); Vector3d dPosition = new Vector3d(); twist.getLinearPart(dPosition); // velocity in body frame rotationToPack.transform(dPosition); // velocity in base frame dPosition.scale(dt); // translation in base frame positionToPack.add(dPosition); Vector3d axis = new Vector3d(); twist.getAngularPart(axis); axis.scale(dt); double angle = axis.length(); if (angle > 0.0) axis.normalize(); else axis.set(1.0, 0.0, 0.0); AxisAngle4d axisAngle = new AxisAngle4d(axis, angle); Matrix3d dRotation = new Matrix3d(); dRotation.set(axisAngle); rotationToPack.mul(dRotation); }
@Override public Matrix3d getViewMatrix(int index) { Matrix3d m = new Matrix3d(); switch (index) { case 0: m.setIdentity(); // front break; case 1: m.rotY(Math.PI/2); // left break; case 2: m.rotY(Math.PI); // back break; case 3: m.rotY(-Math.PI/2); // right break; case 4: m.rotX(Math.PI/2); // top break; case 5: m.rotX(-Math.PI/2); // bottom break; default: throw new IllegalArgumentException("getViewMatrix: index out of range:" + index); } return m; } }
/** * Changes the frame in which this GeneralizedRigidBodyInertia is expressed. * * See Duindam, Port-Based Modeling and Control for Efficient Bipedal Walking Robots, page 40, eq. (2.57) * http://sites.google.com/site/vincentduindam/publications * * @param newFrame the frame in which this inertia should be expressed */ public void changeFrame(ReferenceFrame newFrame) { RigidBodyTransform transform = newFrame.getTransformToDesiredFrame(expressedInframe); Matrix3d rotation = new Matrix3d(); transform.getRotation(rotation); transform.getTranslation(tempVector); // p // mass moment of inertia part: subTildeTimesTildePlusTildeTimesTildeTransposeFromSymmetricMatrix(massMomentOfInertiaPart, crossPart, tempVector); // J - (tilde(c) * tilde(p))^T - tilde(c) * tilde(p) subScalarTimesTildeTimesTildeFromSymmetricMatrix(massMomentOfInertiaPart, tempVector, mass); // J - (tilde(c) * tilde(p))^T - tilde(c) * tilde(p) - m * tilde(p) * tilde(p) massMomentOfInertiaPart.mulTransposeLeft(rotation, massMomentOfInertiaPart); // RTranspose * (J - (tilde(c) * tilde(p))^T - tilde(c) * tilde(p) - m * tilde(p) * tilde(p)) massMomentOfInertiaPart.mul(rotation); // RTranspose * (J - (tilde(c) * tilde(p))^T - tilde(c) * tilde(p) - m * tilde(p) * tilde(p)) * R: done. // cross part: tempVector.scale(mass); // m * p crossPart.add(tempVector); // c + m * p rotation.transpose(); // RTranspose rotation.transform(crossPart); // RTranspose * (c + m * p) // mass part doesn't change // finally, change frame and check if cross term zero this.expressedInframe = newFrame; determineIfCrossPartIsZero(); }
yVec.normalize(); Matrix3d rotationMatrix = new Matrix3d(); rotationMatrix.setColumn(0, xVec); rotationMatrix.setColumn(1, yVec); rotationMatrix.setColumn(2, zVec); RotationTools.convertMatrixToQuaternion(rotationMatrix, polygonPose.getOrientation()); rotationMatrix.transpose(); polygonToPack.clear(); for (Point3d point : projectedPoints) rotationMatrix.transform(point); if (Math.abs(point.getZ()) > 1e-14)
/** * Transforms the given crystal basis coordinates into orthonormal coordinates. * e.g. transfToOrthonormal(new Point3d(1,1,1)) returns the orthonormal coordinates of the * vertex of the unit cell. * See Giacovazzo section 2.E, eq. 2.E.1 (or any linear algebra manual) * @param v */ public void transfToOrthonormal(Tuple3d v) { // see Giacovazzo section 2.E, eq. 2.E.1 getMTransposeInv().transform(v); }