How to use

Vector3D32

in

us.ihmc.euclid.tuple3D

public static MeshDataHolder Line(float x0, float y0, float z0, float x1, float y1, float z1, float width)
 Vector3D32 lineDirection = new Vector3D32(x1 - x0, y1 - y0, z1 - z0);
 float lineLength = (float) lineDirection.length();
 lineDirection.scale(1.0f / lineLength);
 MeshDataHolder line = Cube(width, width, lineLength, false, null);
 Point3D32[] vertices = line.getVertices();
 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());
   pitch = lineDirection.getZ() >= 0.0 ? 0.0f : (float) Math.PI;
   float vx = normal.getX32();
   float vy = normal.getY32();
   float vz = normal.getZ32();
   normal.setX(rxx * vx + rxy * vy + rxz * vz);
   normal.setY(ryx * vx + ryy * vy + ryz * vz);
   normal.setZ(rzx * vx + rzz * vz);

public Point3D32 getNearestIntersection(Point3D32 origin, Vector3D32 direction)
 direction.normalize();
 float dx, dy, dz, dot;
 double distanceToLine, distance;
   dz = origin.getZ32() - p.getZ32();
   dot = dx * direction.getX32() + dy * direction.getY32() + dz * direction.getZ32();
   dx = dx - dot * direction.getX32();
   dy = dy - dot * direction.getY32();
   dz = dz - dot * direction.getZ32();

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

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

 continue;
Vector3D32 vertexNormal = new Vector3D32();
 vertexNormal.add(normalsPerFace[face]);
vertexNormal.scale(1.0f / faces);
normalsPerVertex[pos++] = vertexNormal;

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

Vector3D32 vector = new Vector3D32();
  Assert.assertTrue(0 == vector.getX32());
  Assert.assertTrue(0 == vector.getY32());
  Assert.assertTrue(0 == vector.getZ32());
  float newZ = random.nextFloat();
  vector = new Vector3D32(newX, newY, newZ);
  Assert.assertTrue(newX == vector.getX32());
  Assert.assertTrue(newY == vector.getY32());
  Assert.assertTrue(newZ == vector.getZ32());
  copyRandomVector32Array[2] = randomVector32Array[2];
  Vector3D32 vectorArray = new Vector3D32(randomVector32Array);
  Assert.assertTrue(randomVector32Array[0] == vectorArray.getX32());
  Assert.assertTrue(randomVector32Array[1] == vectorArray.getY32());
  Assert.assertTrue(randomVector32Array[2] == vectorArray.getZ32());
  vector = new Vector3D32(vector2);
  Assert.assertTrue(vector.getX32() == vector2.getX32());
  Assert.assertTrue(vector.getY32() == vector2.getY32());
  Assert.assertTrue(vector.getZ32() == vector2.getZ32());

@Override
public void testSetters() throws Exception
{
 super.testSetters();
 Random random = new Random(621541L);
 Vector3D32 tuple1 = createEmptyTuple();
 for (int i = 0; i < ITERATIONS; i++)
 { // Test setX(float x)
   float x = random.nextFloat();
   tuple1.setX(x);
   assertEquals(tuple1.getX32(), x, getEpsilon());
 }
 for (int i = 0; i < ITERATIONS; i++)
 { // Test setY(float y)
   float y = random.nextFloat();
   tuple1.setY(y);
   assertEquals(tuple1.getY32(), y, getEpsilon());
 }
 for (int i = 0; i < ITERATIONS; i++)
 { // Test setZ(float z)
   float z = random.nextFloat();
   tuple1.setZ(z);
   assertEquals(tuple1.getZ32(), z, getEpsilon());
 }
}

Vector3D32 normal = new Vector3D32();
normal.setX((float) transform.getM02());
normal.setY((float) transform.getM12());
normal.setZ((float) transform.getM22());
vertexNormals[vertexIndex] = normal;

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

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

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 Vector3D32 createTuple(double x, double y, double z)
{
 return new Vector3D32((float) x, (float) y, (float) z);
}

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

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 Vector3D32 createEmptyTuple()
{
 return new Vector3D32();
}

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

direction.normalize();
float dx, dy, dz, dot;
double distanceToLine, distance;
  dz = origin.getZ32() - p.z;
  dot = dx * direction.getX32() + dy * direction.getY32() + dz * direction.getZ32();
  dx = dx - dot * direction.getX32();
  dy = dy - dot * direction.getY32();
  dz = dz - dot * direction.getZ32();

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

direction.normalize();
float dx, dy, dz, dot;
double distanceToLine, distance;
  dz = origin.getZ32() - p.z;
  dot = dx * direction.getX32() + dy * direction.getY32() + dz * direction.getZ32();
  dx = dx - dot * direction.getX32();
  dy = dy - dot * direction.getY32();
  dz = dz - dot * direction.getZ32();