@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);
pinJointZRotation.rotZ(-FastMath.PI / 2.0); Matrix3d pinJointRotation = new Matrix3d(); pinJointRotation.rotY(-FastMath.PI / 4.0); pinJointRotation.mul(pinJointZRotation);
/** * 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; }
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); }
@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); }
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); }
beta = -_vecteur.angle(new Vector3d(lines[1].x, lines[1].y, lines[1].z)); rot_.setZero(); rot_.rotY(beta); rot_.transform(lines[1]); rot_.transform(lines[2]); beta = -_vecteur.angle(new Vector3d(lines[1].x, lines[1].y, lines[1].z)); rot_.setZero(); rot_.rotY(beta); rot_.transform(lines[1]); rot_.transform(lines[2]); beta = new Vector3d(lines[1].x, lines[1].y, lines[1].z).angle(_vecteur); rot_.setZero(); rot_.rotY(beta); rot_.transform(lines[1]); rot_.transform(lines[2]);