public double getPlaneZGivenXY(RigidBodyTransform planeToWorldTransform, double xWorld, double yWorld) { // The three components of the plane origin double x0 = planeToWorldTransform.getM03(); double y0 = planeToWorldTransform.getM13(); double z0 = planeToWorldTransform.getM23(); // The three components of the plane normal double a = planeToWorldTransform.getM02(); double b = planeToWorldTransform.getM12(); double c = planeToWorldTransform.getM22(); // Given the plane equation: a*x + b*y + c*z + d = 0, with d = -(a*x0 + b*y0 + c*z0), we find z: double z = a / c * (x0 - xWorld) + b / c * (y0 - yWorld) + z0; return z; }
/** * Computes the z-coordinate in world of the plane for a given xy-coordinates in world. * * @param xWorld x-coordinate of the query * @param yWorld y-coordinate of the query * @return the z-coordinate */ public double getPlaneZGivenXY(double xWorld, double yWorld) { // The three components of the plane origin double x0 = fromLocalToWorldTransform.getM03(); double y0 = fromLocalToWorldTransform.getM13(); double z0 = fromLocalToWorldTransform.getM23(); // The three components of the plane normal double a = fromLocalToWorldTransform.getM02(); double b = fromLocalToWorldTransform.getM12(); double c = fromLocalToWorldTransform.getM22(); // Given the plane equation: a*x + b*y + c*z + d = 0, with d = -(a*x0 + b*y0 + c*z0), we find z: double z = a / c * (x0 - xWorld) + b / c * (y0 - yWorld) + z0; return z; }
private double getPlaneZGivenXY(ReferenceFrame planeFrame, double xWorld, double yWorld) { RigidBodyTransform fromLocalToWorldTransform = planeFrame.getTransformToWorldFrame(); // The three components of the plane origin double x0 = fromLocalToWorldTransform.getM03(); double y0 = fromLocalToWorldTransform.getM13(); double z0 = fromLocalToWorldTransform.getM23(); // The three components of the plane normal double a = fromLocalToWorldTransform.getM02(); double b = fromLocalToWorldTransform.getM12(); double c = fromLocalToWorldTransform.getM22(); // Given the plane equation: a*x + b*y + c*z + d = 0, with d = -(a*x0 + b*y0 + c*z0), we find z: double z = a / c * (x0 - xWorld) + b / c * (y0 - yWorld) + z0; return z; }
/** * Creates and returns the Plane3D that this planar region lies on. */ public Plane3D getPlane() { Plane3D ret = new Plane3D(); ret.setPoint(fromLocalToWorldTransform.getM03(), fromLocalToWorldTransform.getM13(), fromLocalToWorldTransform.getM23()); ret.setNormal(fromLocalToWorldTransform.getM02(), fromLocalToWorldTransform.getM12(), fromLocalToWorldTransform.getM22()); return ret; }
private Graphics3DObject createBlock() { CombinedTerrainObject3D blocksTerrain = new CombinedTerrainObject3D("Block"); AxisAngle axisAngle = new AxisAngle(0.0, 0.0, 1.0, Math.PI / 4.0); RigidBodyTransform configuration = new RigidBodyTransform(axisAngle, new Vector3D(0.65, 0.0, 0.5)); blocksTerrain.addRotatableBox(configuration, 0.5, 0.5, 0.5, YoAppearance.DarkGray()); Vector3D offset = new Vector3D(0.0, -0.5, 0.25); axisAngle.transform(offset); RigidBodyTransform configuration2 = new RigidBodyTransform(configuration); configuration2.setTranslation(configuration2.getM03() + offset.getX(), configuration2.getM13() + offset.getY(), configuration2.getM23() + offset.getZ()); blocksTerrain.addRotatableBox(configuration2, 0.5, 0.5, 1.0, YoAppearance.DarkCyan()); offset = new Vector3D(0.5, 0.0, 0.25); axisAngle.transform(offset); RigidBodyTransform configuration3 = new RigidBodyTransform(configuration); configuration3.setTranslation(configuration3.getM03() + offset.getX(), configuration3.getM13() + offset.getY(), configuration3.getM23() + offset.getZ()); blocksTerrain.addRotatableBox(configuration3, 0.5, 0.5, 1.0, YoAppearance.DarkCyan()); return blocksTerrain.getLinkGraphics(); }
assertTrue(transform.getElement(2, 2) == coeff); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, coeff = random.nextDouble()); assertTrue(transform.getM23() == coeff); assertTrue(transform.getElement(2, 3) == coeff);
public static void convertRigidBodyTransformToAffine(RigidBodyTransform rigidBodyTransform, Affine affineToPack) { affineToPack.setMxx(rigidBodyTransform.getM00()); affineToPack.setMxy(rigidBodyTransform.getM01()); affineToPack.setMxz(rigidBodyTransform.getM02()); affineToPack.setMyx(rigidBodyTransform.getM10()); affineToPack.setMyy(rigidBodyTransform.getM11()); affineToPack.setMyz(rigidBodyTransform.getM12()); affineToPack.setMzx(rigidBodyTransform.getM20()); affineToPack.setMzy(rigidBodyTransform.getM21()); affineToPack.setMzz(rigidBodyTransform.getM22()); affineToPack.setTx(rigidBodyTransform.getM03()); affineToPack.setTy(rigidBodyTransform.getM13()); affineToPack.setTz(rigidBodyTransform.getM23()); }
@Test public void testTransformWithVector4D() throws Exception { Random random = new Random(5634L); RigidBodyTransform transform = EuclidCoreRandomTools.nextRigidBodyTransform(random); Vector4D vectorOriginal = EuclidCoreRandomTools.nextVector4D(random); Vector4D vectorExpected = new Vector4D(); Vector4D vectorActual = new Vector4D(); Vector3D vector3D = new Vector3D(vectorOriginal.getX(), vectorOriginal.getY(), vectorOriginal.getZ()); transform.transform(vector3D); vectorExpected.set(vector3D); vectorExpected.setS(vectorOriginal.getS()); vectorExpected.addX(vectorExpected.getS() * transform.getM03()); vectorExpected.addY(vectorExpected.getS() * transform.getM13()); vectorExpected.addZ(vectorExpected.getS() * transform.getM23()); transform.transform(vectorOriginal, vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); vectorActual.set(vectorOriginal); transform.transform(vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); // Try with dense-matrix DenseMatrix64F transformDenseMatrix = new DenseMatrix64F(4, 4); transform.get(transformDenseMatrix); DenseMatrix64F vectorOriginalDenseMatrix = new DenseMatrix64F(4, 1); vectorOriginal.get(vectorOriginalDenseMatrix); DenseMatrix64F vectorTransformedDenseMatrix = new DenseMatrix64F(4, 1); CommonOps.mult(transformDenseMatrix, vectorOriginalDenseMatrix, vectorTransformedDenseMatrix); vectorExpected.set(vectorTransformedDenseMatrix); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); }
double m21 = expected.getM21(); double m22 = expected.getM22(); double m23 = expected.getM23();
double m21 = expected.getM21(); double m22 = expected.getM22(); double m23 = expected.getM23(); actual.set(m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, 0.0); m21 = expected.getM21(); m22 = expected.getM22(); m23 = expected.getM23(); actual.set(m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, 0.0); m21 = expected.getM21(); m22 = expected.getM22(); m23 = expected.getM23(); actual.set(m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, 0.0); double m21 = expected.getM21(); double m22 = expected.getM22(); double m23 = expected.getM23(); actual.setUnsafe(m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, 0.0); m21 = expected.getM21(); m22 = expected.getM22(); m23 = expected.getM23(); actual.setUnsafe(m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, 0.0);
currentRow.createCell(currentCellIndex++).setCellValue(transformToParent.getM21()); currentRow.createCell(currentCellIndex++).setCellValue(transformToParent.getM22()); currentRow.createCell(currentCellIndex++).setCellValue(transformToParent.getM23());