How to use

us.ihmc.euclid.tuple3D.Vector3D32

constructor

public void setSize(Vector3D size)
{
 isEmpty = false;
 this.size = new Vector3D32(size);
}

@Override
public Vector3D32 createTuple(double x, double y, double z)
{
 return new Vector3D32((float) x, (float) y, (float) z);
}

@Override
public Vector3D32 createEmptyTuple()
{
 return new Vector3D32();
}

public static Vector3D32 nextVector3D32(Random random)
{
 return new Vector3D32(nextVector3D(random));
}

public static PlanarRegionSegmentationMessage createPlanarRegionSegmentationMessage(int id, Point3D origin, Vector3D normal,
                                          OcTreeKeyMessage[] regionNodeKeys, List<Point3D> hitLocations)
{
 PlanarRegionSegmentationMessage message = new PlanarRegionSegmentationMessage();
 message.id = id;
 message.origin = new Point3D32(origin);
 message.normal = new Vector3D32(normal);
 message.nodeKeys = regionNodeKeys;
 message.hitLocations = hitLocations.stream().map(Point3D32::new).toArray(Point3D32[]::new);
 return message;
}

private void loadHeader(File headerFile) throws IOException
{
 FileReader fileReader = new FileReader(headerFile);
 BufferedReader bufferedReader = new BufferedReader(fileReader);
 String line = "";
 String cvsSplitBy = ",";
 while ((line = bufferedReader.readLine()) != null)
 {
   line = line.replaceAll("regionId: ", "");
   line = line.replaceAll("origin: ", "");
   line = line.replaceAll("normal: ", "");
   String[] values = line.split(cvsSplitBy);
      int regionId = Integer.parseInt(values[0]);
   float xOrigin = Float.parseFloat(values[1]);
   float yOrigin = Float.parseFloat(values[2]);
   float zOrigin = Float.parseFloat(values[3]);
   float xNormal = Float.parseFloat(values[4]);
   float yNormal = Float.parseFloat(values[5]);
   float zNormal = Float.parseFloat(values[6]);
   Point3D32 origin = new Point3D32(xOrigin, yOrigin, zOrigin);
   Vector3D32 normal = new Vector3D32(xNormal, yNormal, zNormal);
   PlanarRegionSegmentationRawData rawData = new PlanarRegionSegmentationRawData(regionId, normal, origin);
   planarRegionSegmentationRawData.add(rawData);
 }
 bufferedReader.close();
}

@Override
public void set(PlanarRegionSegmentationMessage other)
{
 id = other.id;
 origin = new Point3D32(other.origin);
 normal = new Vector3D32(other.normal);
 nodeKeys = new OcTreeKeyMessage[other.nodeKeys.length];
 for (int i = 0; i < nodeKeys.length; i++)
 {
   nodeKeys[i] = new OcTreeKeyMessage();
   nodeKeys[i].set(other.nodeKeys[i]);
 }
 hitLocations = Arrays.stream(other.hitLocations).map(Point3D32::new).toArray(Point3D32[]::new);
 setPacketInformation(other);
}

 continue;
Vector3D32 vertexNormal = new Vector3D32();

public CircleVertices(int numberOfVertices)
{
  vertices = new Point3D32[numberOfVertices];
  normals = new Vector3D32[numberOfVertices];
  for (int i = 0; i < numberOfVertices; i++)
  {
   vertices[i] = new Point3D32();
   normals[i] = new Vector3D32();
  }
  for (int i = 0; i < numberOfVertices; i++)
  {
   double angle = i / (double) numberOfVertices * 2.0 * Math.PI;
   double ux = Math.cos(angle);
   double uy = Math.sin(angle);
   vertices[i].set(radius * ux, radius * uy, 0.0);
   normals[i].set(ux, uy, 0.0);
  }
}

private static Vector3D32[] findNormalsPerFace(int[] indices, Point3D32[] vertices)
{
 Vector3D32[] normalsPerFace = new Vector3D32[indices.length / 3]; // Abuse integer division.
 Vector3D32 firstVector = new Vector3D32();
 Vector3D32 secondVector = new Vector3D32();
 Point3D32[] faceVertices = new Point3D32[3];
 for (int face = 0; face < normalsPerFace.length; face++)
 {
   normalsPerFace[face] = new Vector3D32();
   for (int i = 0; i < faceVertices.length; i++)
   {
    faceVertices[i] = vertices[indices[face * 3 + i]];
   }
   firstVector.set(faceVertices[2]);
   firstVector.sub(faceVertices[0]);
   secondVector.set(faceVertices[2]);
   secondVector.sub(faceVertices[1]);
   normalsPerFace[face].cross(firstVector, secondVector);
   normalsPerFace[face].normalize();
 }
 return normalsPerFace;
}

@Override
public void set(BoxMessage other)
{
 setPacketInformation(other);
 isEmpty = other.isEmpty;
 size = new Vector3D32(other.size);
 center = new Point3D32(center);
 orientation = new Quaternion32(other.orientation);
}

public static MeshDataHolder FlatRectangle(float xMin, float yMin, float xMax, float yMax, float z)
{
 Point3D32[] points = new Point3D32[4];
 Vector3D32[] normals = new Vector3D32[4];
 TexCoord2f[] textPoints = new TexCoord2f[4];
 points[0] = new Point3D32(xMin, yMin, z);
 points[1] = new Point3D32(xMax, yMin, z);
 points[2] = new Point3D32(xMax, yMax, z);
 points[3] = new Point3D32(xMin, yMax, z);
 textPoints[0] = new TexCoord2f(0.0f, 0.0f);
 textPoints[1] = new TexCoord2f(1.0f, 0.0f);
 textPoints[2] = new TexCoord2f(1.0f, 1.0f);
 textPoints[3] = new TexCoord2f(0.0f, 1.0f);
 normals[0] = new Vector3D32(0.0f, 0.0f, 1.0f);
 normals[1] = new Vector3D32(0.0f, 0.0f, 1.0f);
 normals[2] = new Vector3D32(0.0f, 0.0f, 1.0f);
 normals[3] = new Vector3D32(0.0f, 0.0f, 1.0f);
 int[] triangleIndices = new int[3 * 2];
 int index = 0;
 triangleIndices[index++] = 0;
 triangleIndices[index++] = 3;
 triangleIndices[index++] = 1;
 triangleIndices[index++] = 3;
 triangleIndices[index++] = 2;
 triangleIndices[index++] = 1;
 return new MeshDataHolder(points, textPoints, triangleIndices, normals);
}

previousFootstep.setTranslation(new Vector3D32(position));
previousFootstep.setRotation(orientation);

/**
* Utility method to rotate a given mesh using a given rotation matrix.
* @param input the mesh to rotate. Not modified.
* @param matrix the rotation to apply to the mesh. Not Modified.
* @return the rotated mesh.
*/
public static MeshDataHolder rotate(MeshDataHolder input, RotationMatrix matrix)
{
 TexCoord2f[] texturePoints = input.getTexturePoints();
 int[] triangleIndices = input.getTriangleIndices();
 Point3D32[] inputVertices = input.getVertices();
 Vector3D32[] inputNormals = input.getVertexNormals();
 Point3D32[] outputVertices = new Point3D32[inputVertices.length];
 Vector3D32[] outputNormals = new Vector3D32[inputNormals.length];
 for (int i = 0; i < inputVertices.length; i++)
 {
   outputVertices[i] = new Point3D32();
   outputNormals[i] = new Vector3D32();
   matrix.transform(inputVertices[i], outputVertices[i]);
   matrix.transform(inputNormals[i], outputNormals[i]);
 }
 return new MeshDataHolder(outputVertices, texturePoints, triangleIndices, outputNormals);
}

public void setInitialConfiguration(KinematicsToolboxOutputStatus kinematicsToolboxOutputStatus)
{
 RobotConfigurationData configurationData = new RobotConfigurationData();
 configurationData.setJointNameHash(kinematicsToolboxOutputStatus.getJointNameHash());
 MessageTools.copyData(kinematicsToolboxOutputStatus.getDesiredJointAngles(), configurationData.getJointAngles());
 configurationData.getRootTranslation().set(new Vector3D32(kinematicsToolboxOutputStatus.getDesiredRootTranslation()));
 configurationData.getRootOrientation().set(new Quaternion32(kinematicsToolboxOutputStatus.getDesiredRootOrientation()));
 setInitialConfiguration(configurationData);
}

public static Transform j3dTransform3DToJMETransform(RigidBodyTransform transform3D)
{
 Quaternion32 quat = new Quaternion32();
 us.ihmc.euclid.tuple3D.Vector3D32 vector = new us.ihmc.euclid.tuple3D.Vector3D32();
 transform3D.get(quat, vector);
 Vector3f jmeVector = new Vector3f(vector.getX32(), vector.getY32(), vector.getZ32());
 Quaternion jmeQuat = new Quaternion(quat.getX32(), quat.getY32(), quat.getZ32(), quat.getS32());
 Transform ret = new Transform(jmeVector, jmeQuat, new Vector3f(1.0f, 1.0f, 1.0f));
 return ret;
}

private static Transform j3dTransform3DToJMETransform(RigidBodyTransform transform3D)
{
 us.ihmc.euclid.tuple4D.Quaternion32 quat = new us.ihmc.euclid.tuple4D.Quaternion32();
 us.ihmc.euclid.tuple3D.Vector3D32 vector = new us.ihmc.euclid.tuple3D.Vector3D32();
 transform3D.get(quat, vector);
 Vector3f jmeVector = new Vector3f(vector.getX32(), vector.getY32(), vector.getZ32());
 Quaternion jmeQuat = new Quaternion(quat.getX32(), quat.getY32(), quat.getZ32(), quat.getS32());
 Transform ret = new Transform(jmeVector, jmeQuat, new Vector3f(1.0f, 1.0f, 1.0f));
 return ret;
}

public static PlanarRegionSegmentationMessage createPlanarRegionSegmentationMessage(PlanarRegionSegmentationNodeData nodeData)
{
 int regionId = nodeData.getId();
 Point3D32 origin = new Point3D32(nodeData.getOrigin());
 Vector3D32 normal = new Vector3D32(nodeData.getNormal());
 OcTreeKeyMessage[] nodeKeys = new OcTreeKeyMessage[nodeData.getNumberOfNodes()];
 Point3D32[] nodeHitLocations = new Point3D32[nodeData.getNumberOfNodes()];
 for (int nodeIndex = 0; nodeIndex < nodeData.getNumberOfNodes(); nodeIndex++)
 {
   NormalOcTreeNode node = nodeData.getNode(nodeIndex);
   OcTreeKeyMessage nodeKey = OcTreeMessageConverter.createOcTreeKeyMessage(node.getKeyCopy());
   nodeKeys[nodeIndex] = nodeKey;
   nodeHitLocations[nodeIndex] = new Point3D32(node.getHitLocationCopy());
 }
 PlanarRegionSegmentationMessage planarRegionNodeKeysMessage = createPlanarRegionSegmentationMessage(regionId, origin, normal, nodeKeys, nodeHitLocations);
 return planarRegionNodeKeysMessage;
}

normals[i] = new Vector3D32(planeNormal);

Vector3D32 dir = new Vector3D32(ray.getDirection().x, ray.getDirection().y, ray.getDirection().z);
Point3D32 nearest = getNearestIntersection(origin, dir, getLidarResolution(), geom.getWorldTransform());
if (nearest != null)