@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; }
@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.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; } }
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 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; }
/** * 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); }
/** * 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); }
@Override public Matrix3d getViewMatrix(int index) { Matrix3d m = new Matrix3d(); switch (index) { case 0: m.setIdentity(); // C4 vertex-centered break; case 1: m.rotX(-0.5 * TETRAHEDRAL_ANGLE); // C3 face-centered 2.0*Math.PI/3 Matrix3d m1 = new Matrix3d(); m1.rotZ(Math.PI/4); m.mul(m1); break; case 2: m.rotY(Math.PI/4); // side face-centered break; default: throw new IllegalArgumentException("getViewMatrix: index out of range:" + index); } return m; }
/** * The up vector only depends on the roll * * @param roll * @return the cameras up-vector. */ private Vector3d calcUp( double roll, Point3d eye, Point3d poi ) { Vector3d newUP = new Vector3d( 0, 0, 1 ); if ( Math.abs( roll ) > 1E-10 ) { Matrix3d mat = new Matrix3d(); mat.rotX( roll ); mat.transform( newUP ); } Vector3d resultUp = new Vector3d(); Vector3d viewDir = new Vector3d(); viewDir.sub( eye, poi ); viewDir.normalize(); double dot = newUP.dot( viewDir ); if ( Math.abs( dot ) < 1E-10 ) { return newUP; } viewDir.scale( dot ); resultUp.sub( newUP, viewDir ); resultUp.normalize(); return resultUp; }
/** * 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 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); }
public FramePose getPoseFromCylindricalCoordinates(RobotSide robotSide, ReferenceFrame frame, double radiansFromYAxis, double radius, double z, double outwardRotation, double pitchRotation) { getPosition(position, frame, radiansFromYAxis, radius, z); RotationTools.convertYawPitchRollToMatrix(0.0, Math.PI / 2.0, -Math.PI / 2.0, preRotation); rotX.rotX(robotSide.negateIfRightSide(Math.PI / 2.0) - radiansFromYAxis); rotZ.rotZ(robotSide.negateIfRightSide(outwardRotation)); rotY.rotY(pitchRotation); rotation.mul(preRotation, rotX); rotation.mul(rotZ); rotation.mul(rotY); orientation.setIncludingFrame(frame, rotation); return new FramePose(position, orientation); }
invertThirdAxis.rotX(Math.PI); coneRotation.mul(invertThirdAxis);
rotationMatrix.rotX(0.6); rootJoint.setRotation(rotationMatrix);