How to use

getZ

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)
  pitch = (float) Math.atan2(xyLength, lineDirection.getZ());
  pitch = lineDirection.getZ() >= 0.0 ? 0.0f : (float) Math.PI;

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());
}