How to use

set

method

in

us.ihmc.euclid.tuple3D.Vector3D32

private void set(CircleVertices other)
{
  int numberOfVertices = getNumberOfVertices();
  for (int i = 0; i < numberOfVertices; i++)
  {
   vertices[i].set(other.vertices[i]);
   normals[i].set(other.normals[i]);
  }
}

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

  texturePoints.add().set(texturePoint);
for (Vector3D32 normal : meshDataHolder.getVertexNormals())
  vertexNormals.add().set(normal);

  /**
  * Append a mesh to this.
  * @param other the mesh to append. Not modified.
  * @param updateTriangleIndices whether the indices of the given mesh should be updated when appended. Highly recommended, set it to false only if you what you are doing.
  */
  public void add(ModifiableMeshDataHolder other, boolean updateTriangleIndices)
  {
   if (updateTriangleIndices)
   {
     int shift = vertices.size();
     for (int i = 0; i < other.triangleIndices.size(); i++)
      triangleIndices.add(other.triangleIndices.get(i) + shift);
   }
   else
   {
     triangleIndices.addAll(other.triangleIndices);
   }
   for (int i = 0; i < other.vertices.size(); i++)
     vertices.add().set(other.vertices.get(i));
   for (int i = 0; i < other.texturePoints.size(); i++)
     texturePoints.add().set(other.texturePoints.get(i));
   for (int i = 0; i < other.vertexNormals.size(); i++)
     vertexNormals.add().set(other.vertexNormals.get(i));
  }
}

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

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