How to use

getY

method

in

us.ihmc.euclid.tuple3D.Vector3D32

private void writeHeaderFile(File header, PlanarRegionSegmentationMessage[] segmentationData) throws IOException
{
 FileWriter fileWriter = new FileWriter(header);
 for (PlanarRegionSegmentationMessage message : segmentationData)
 {
   Point3D32 origin = message.getOrigin();
   Vector3D32 normal = message.getNormal();
   fileWriter.write("regionId: ");
   fileWriter.write(Integer.toString(message.getRegionId()));
   fileWriter.write(", origin: ");
   fileWriter.write(origin.getX() + ", " + origin.getY() + ", " + origin.getZ());
   fileWriter.write(", normal: ");
   fileWriter.write(normal.getX() + ", " + normal.getY() + ", " + normal.getZ());
   fileWriter.write("\n");
 }
 fileWriter.close();
}

if (Math.abs(lineDirection.getZ()) < 1.0 - 1.0e-7)
  yaw = (float) Math.atan2(lineDirection.getY(), lineDirection.getX());
  double xyLength = Math.sqrt(lineDirection.getX() * lineDirection.getX() + lineDirection.getY() * lineDirection.getY());
  pitch = (float) Math.atan2(xyLength, lineDirection.getZ());

public void rotate(Matrix3DReadOnly rotationMatrix)
{
  for (Point3D32 vertex : vertices)
  {
   double x = rotationMatrix.getM00() * vertex.getX() + rotationMatrix.getM01() * vertex.getY();
   double y = rotationMatrix.getM10() * vertex.getX() + rotationMatrix.getM11() * vertex.getY();
   double z = rotationMatrix.getM20() * vertex.getX() + rotationMatrix.getM21() * vertex.getY();
   vertex.set(x, y, z);
  }
  for (Vector3D32 normal : normals)
  {
   double x = rotationMatrix.getM00() * normal.getX() + rotationMatrix.getM01() * normal.getY();
   double y = rotationMatrix.getM10() * normal.getX() + rotationMatrix.getM11() * normal.getY();
   double z = rotationMatrix.getM20() * normal.getX() + rotationMatrix.getM21() * normal.getY();
   normal.set(x, y, z);
  }
}

Point3D32 boxCenter = newMessage.getCenter();
MeshDataHolder boxMeshDataHolder = MeshDataGenerator.Cube(boxSize.getX(), boxSize.getY(), boxSize.getZ(), true);
boxMeshDataHolder = MeshDataHolder.rotate(boxMeshDataHolder, boxOrientation);
boxMeshDataHolder = MeshDataHolder.translate(boxMeshDataHolder, boxCenter);

@Override
public void receivedPacket(RobotConfigurationData packet)
{
 latestRobotConfigurationData = packet;
 FloatingJointBasics rootJoint = fullRobotModel.getRootJoint();
 TFloatArrayList newJointAngles = packet.getJointAngles();
 TFloatArrayList newJointVelocities = packet.getJointAngles();
 TFloatArrayList newJointTorques = packet.getJointTorques();
 OneDoFJointBasics[] oneDoFJoints = fullRobotModel.getOneDoFJoints();
 for (int i = 0; i < newJointAngles.size(); i++)
 {
   oneDoFJoints[i].setQ(newJointAngles.get(i));
   oneDoFJoints[i].setQd(newJointVelocities.get(i));
   oneDoFJoints[i].setTau(newJointTorques.get(i));
 }
 pelvisTranslationFromRobotConfigurationData.set(packet.getRootTranslation());
 pelvisOrientationFromRobotConfigurationData.set(packet.getRootOrientation());
 rootJoint.getJointPose().setPosition(pelvisTranslationFromRobotConfigurationData.getX(), pelvisTranslationFromRobotConfigurationData.getY(), pelvisTranslationFromRobotConfigurationData.getZ());
 rootJoint.getJointPose().getOrientation().setQuaternion(pelvisOrientationFromRobotConfigurationData.getX(), pelvisOrientationFromRobotConfigurationData.getY(), pelvisOrientationFromRobotConfigurationData.getZ(), pelvisOrientationFromRobotConfigurationData.getS());
 
 computeDriftTransform();
 rootJoint.getPredecessor().updateFramesRecursively();
 yoVariableServer.update(System.currentTimeMillis());
}