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)