/** * Returns true if this PlanarRegion is purely vertical, as far as numerical roundoff is concerned. * Checks z component of surface normal. If absolute value is really small, then returns true. * @return true if vertical. false otherwise. */ public boolean isVertical() { return (Math.abs(fromLocalToWorldTransform.getM22()) < 1e-10); }
private boolean isTransformZUp(RigidBodyTransform soleTransformBeforeSnap) { return Math.abs(soleTransformBeforeSnap.getM22() - 1.0) < 1e-4; }
/** * Retrieves the normal of this planar region and stores it in the given {@link Vector3d}. * @param normalToPack used to store the normal of this planar region. */ public void getNormal(Vector3d normalToPack) { normalToPack.setX(fromLocalToWorldTransform.getM02()); normalToPack.setY(fromLocalToWorldTransform.getM12()); normalToPack.setZ(fromLocalToWorldTransform.getM22()); }
/** * Retrieves the normal of this planar region and stores it in the given {@link Vector3f}. * @param normalToPack used to store the normal of this planar region. */ public void getNormal(Vector3f normalToPack) { normalToPack.setX((float) fromLocalToWorldTransform.getM02()); normalToPack.setY((float) fromLocalToWorldTransform.getM12()); normalToPack.setZ((float) fromLocalToWorldTransform.getM22()); }
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; }
/** * 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; }
/** * Removes the pitch and roll from a RigidBodyTransform. Preserves the yaw. * When done, the xAxis should still point in the same direction from the point of view of looking down (-z) * @param transform */ public static void removePitchAndRollFromTransform(RigidBodyTransform transform) { if (Math.abs(transform.getM22() - 1.0) < 1e-4) return; double m00 = transform.getM00(); double m10 = transform.getM10(); double magnitude = Math.sqrt(m00*m00 + m10*m10); m00 = m00 / magnitude; m10 = m10 / magnitude; transform.setM00(m00); transform.setM10(m10); transform.setM20(0.0); transform.setM01(-m10); transform.setM11(m00); transform.setM21(0.0); transform.setM02(0.0); transform.setM12(0.0); transform.setM22(1.0); }
normal.setX((float) transform.getM02()); normal.setY((float) transform.getM12()); normal.setZ((float) transform.getM22()); vertexNormals[vertexIndex] = normal;
public static DenseMatrix64F differentiate(RigidBodyTransform t1, RigidBodyTransform t2, double dt) { DenseMatrix64F ret = new DenseMatrix64F(4,4); ret.set(0,0,(t2.getM00() - t1.getM00()) / dt); ret.set(0,1,(t2.getM01() - t1.getM01()) / dt); ret.set(0,2,(t2.getM02() - t1.getM02()) / dt); ret.set(0,3,(t2.getM03() - t1.getM03()) / dt); ret.set(1,0,(t2.getM10() - t1.getM10()) / dt); ret.set(1,1,(t2.getM11() - t1.getM11()) / dt); ret.set(1,2,(t2.getM12() - t1.getM12()) / dt); ret.set(1,3,(t2.getM13() - t1.getM13()) / dt); ret.set(2,0,(t2.getM20() - t1.getM20()) / dt); ret.set(2,1,(t2.getM21() - t1.getM21()) / dt); ret.set(2,2,(t2.getM22() - t1.getM22()) / dt); ret.set(2,3,(t2.getM23() - t1.getM23()) / dt); ret.set(3,0,0); ret.set(3,1,0); ret.set(3,2,0); ret.set(3,3,1);; return ret; }
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()); }
if (Math.abs(snapTransform.getM22()) < parameters.getMinimumSurfaceNormalZ())
currentRow.createCell(currentCellIndex++).setCellValue(transformToParent.getM20()); currentRow.createCell(currentCellIndex++).setCellValue(transformToParent.getM21()); currentRow.createCell(currentCellIndex++).setCellValue(transformToParent.getM22()); currentRow.createCell(currentCellIndex++).setCellValue(transformToParent.getM23());