/** * Stores the three principal axes in the given Matrix3d such that it can be used as the rotation matrix describing the rotation from the principal frame to the parent coordinate system. * @param rotationMatrixToPack */ public void getPrincipalFrameRotationMatrix(Matrix3d rotationMatrixToPack) { rotationMatrixToPack.setColumn(0, principalAxis); rotationMatrixToPack.setColumn(1, secondaryAxis); rotationMatrixToPack.setColumn(2, thirdAxis); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { positionToPointAt.get(xAxis); xAxis.setZ(0.0); xAxis.normalize(); yAxis.cross(zAxis, xAxis); rotationMatrix.setColumn(0, xAxis); rotationMatrix.setColumn(1, yAxis); rotationMatrix.setColumn(2, zAxis); transformToParent.setRotation(rotationMatrix); } }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { x.set(xAxis.getX(), xAxis.getY(), 0.0); z.set(0.0, 0.0, 1.0); y.cross(z, x); rotation.setColumn(0, x); rotation.setColumn(1, y); rotation.setColumn(2, z); transformToParent.setRotationAndZeroTranslation(rotation); } }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { x.set(xAxis.getX(), xAxis.getY(), 0.0); z.set(0.0, 0.0, 1.0); y.cross(z, x); rotation.setColumn(0, x); rotation.setColumn(1, y); rotation.setColumn(2, z); transformToParent.setRotationAndZeroTranslation(rotation); } }
private void computeOffset() { Vector3d cameraZAxis = new Vector3d(forward); Vector3d cameraYAxis = new Vector3d(down); Vector3d cameraXAxis = new Vector3d(); cameraXAxis.cross(cameraYAxis, cameraZAxis); Matrix3d rotationOffset = new Matrix3d(); rotationOffset.setColumn(0, cameraXAxis); rotationOffset.setColumn(1, cameraYAxis); rotationOffset.setColumn(2, cameraZAxis); JavaFXTools.convertRotationMatrixToAffine(rotationOffset, offset); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { steeringWheelRotationAxis.get(localZAxis); steeringWheelZeroAxis.get(localXAxis); localYAxis.cross(localZAxis, localXAxis); localYAxis.normalize(); localXAxis.cross(localYAxis, localZAxis); steeringWheelZeroAxis.set(localXAxis); steeringWheelCenter.get(localTranslation); localRotation.setColumn(0, localXAxis); localRotation.setColumn(1, localYAxis); localRotation.setColumn(2, localZAxis); transformToParent.set(localRotation, localTranslation); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { eX.sub(p2.getPoint(), p1.getPoint()); eX.normalize(); eY.sub(p3.getPoint(), p1.getPoint()); // temp only eZ.cross(eX, eY); eZ.normalize(); eY.cross(eZ, eX); rotation.setColumn(0, eX); rotation.setColumn(1, eY); rotation.setColumn(2, eZ); transformToParent.setRotationAndZeroTranslation(rotation); transformToParent.setTranslation(p1.getX(), p1.getY(), p1.getZ()); } }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { //TODO: Combine with RotationTools.removePitchAndRollFromTransform(). origin.getReferenceFrame().getTransformToDesiredFrame(nonZUpToWorld, worldFrame); nonZUpToWorld.getRotation(nonZUpToWorldRotation); double yAxisX = nonZUpToWorldRotation.getM01(); double yAxisY = nonZUpToWorldRotation.getM11(); yAxis.set(yAxisX, yAxisY, 0.0); yAxis.normalize(); zAxis.set(0.0, 0.0, 1.0); xAxis.cross(yAxis, zAxis); zUpToWorldRotation.setColumn(0, xAxis); zUpToWorldRotation.setColumn(1, yAxis); zUpToWorldRotation.setColumn(2, zAxis); transformToParent.setRotation(zUpToWorldRotation); originPoint3d.set(origin.getPoint()); nonZUpToWorld.transform(originPoint3d); translation.set(originPoint3d); transformToParent.setTranslation(translation); } }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { originVector.set(origin.getPoint()); positionToPointAt.get(xAxis); xAxis.sub(originVector); double norm = xAxis.length(); if (norm > 1e-12) { xAxis.scale(1.0 / norm); } else { xAxis.set(1.0, 0.0, 0.0); } zAxis.set(0.0, 0.0, 1.0); yAxis.cross(zAxis, xAxis); yAxis.normalize(); zAxis.cross(xAxis, yAxis); rotationMatrix.setColumn(0, xAxis); rotationMatrix.setColumn(1, yAxis); rotationMatrix.setColumn(2, zAxis); RotationTools.checkProperRotationMatrix(rotationMatrix); transformToParent.setRotationAndZeroTranslation(rotationMatrix); transformToParent.setTranslation(originVector); } }
protected void computeRotationTranslation(Transform3d transform3D) { transform3D.setIdentity(); z_rot.set(x.getDoubleValue(), y.getDoubleValue(), z.getDoubleValue()); double length = z_rot.length(); if (length < 1e-7) z_rot.set(0.0, 0.0, 1.0); else z_rot.normalize(); if (Math.abs(z_rot.getX()) <= 0.99) x_rot.set(1.0, 0.0, 0.0); else x_rot.set(0.0, 1.0, 0.0); y_rot.cross(z_rot, x_rot); y_rot.normalize(); x_rot.cross(y_rot, z_rot); x_rot.normalize(); rotMatrix.setColumn(0, x_rot); rotMatrix.setColumn(1, y_rot); rotMatrix.setColumn(2, z_rot); translationVector.set(baseX.getDoubleValue(), baseY.getDoubleValue(), baseZ.getDoubleValue()); transform3D.setScale(lineThickness, lineThickness, length); transform3D.setTranslation(translationVector); transform3D.setRotation(rotMatrix); }
@Override public void computeTransform(RigidBodyTransform currXform) { update(); CameraMountInterface cameraMount = getCameraMount(); if (isMounted() && (cameraMount != null)) { cameraMount.getTransformToCamera(currXform); return; } positionOffset.set(getCamX(), getCamY(), getCamZ()); xAxis.set(getFixX(), getFixY(), getFixZ()); fixPointNode.translateTo(getFixX(), getFixY(), getFixZ()); xAxis.sub(positionOffset); xAxis.normalize(); zAxis.set(0.0, 0.0, 1.0); yAxis.cross(zAxis, xAxis); zAxis.cross(xAxis, yAxis); rotationMatrix.setColumn(0, xAxis); rotationMatrix.setColumn(1, yAxis); rotationMatrix.setColumn(2, zAxis); currXform.setRotationAndZeroTranslation(rotationMatrix); currXform.setTranslation(positionOffset); currXform.normalizeRotationPart(); }
private void diagonalizeTensor() { Matrix m = new Matrix(getInertiaTensor()); EigenvalueDecomposition eig = m.eig(); principalMomentsOfInertia = eig.getRealEigenvalues(); double[][] eigenVectors = eig.getV().getArray(); // Get the principal axes from the eigenVectors principalAxes[0] = new Vector3d(eigenVectors[0][0], eigenVectors[1][0], eigenVectors[2][0]); principalAxes[1] = new Vector3d(eigenVectors[0][1], eigenVectors[1][1], eigenVectors[2][1]); principalAxes[2] = new Vector3d(eigenVectors[0][2], eigenVectors[1][2], eigenVectors[2][2]); // Convert the principal axes into a rotation matrix for (int i = 0; i < 3; i++) orientation.setColumn(i, principalAxes[i]); orientation.negate(); } }