- Add the Codota plugin to your IDE and get smart completions
private void myMethod () {BufferedReader b =
InputStream in;new BufferedReader(new InputStreamReader(in))
- Smart code suggestions by Tabnine
}
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; }
public static Vector3D32 nextVector3D32(Random random) { return new Vector3D32(nextVector3D(random)); }