private static Point3D32[] makePoint3fArrayFromPoint3dArray(Point3DReadOnly[] pPoints) { Point3D32[] points3f = new Point3D32[pPoints.length]; int i = 0; for (Point3DReadOnly point3d : pPoints) { points3f[i++] = new Point3D32(point3d); } return points3f; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testNextPoint3D32Array() { Random random = new Random(); Point3D32 min = new Point3D32(-7.0f, -5.0f, -6.0f); Point3D32 max = new Point3D32(5.0f, 4.0f, 8.0f); Point3D32[] randomPoint3fCloud = RandomGeometry.nextPoint3D32Array(random, 1000, min, max); for (int i = 0; i < 1000; i++) { assertTrue((randomPoint3fCloud[i].getX() >= -7.0f) && (randomPoint3fCloud[i].getX() <= 5.0)); assertTrue((randomPoint3fCloud[i].getY() >= -5.0f) && (randomPoint3fCloud[i].getY() <= 4.0)); assertTrue((randomPoint3fCloud[i].getZ() >= -6.0f) && (randomPoint3fCloud[i].getZ() <= 8.0)); } }
Point3D32 point = new Point3D32(); Assert.assertTrue(0 == point.getX()); Assert.assertTrue(0 == point.getY()); Assert.assertTrue(0 == point.getZ()); float newZ = random.nextFloat(); point = new Point3D32(newX, newY, newZ); Assert.assertTrue(newX == point.getX32()); Assert.assertTrue(newY == point.getY32()); Assert.assertTrue(newZ == point.getZ32()); Point3D32 point = new Point3D32(randomPoint32Array); Assert.assertTrue(randomPoint32Array[0] == point.getX32()); Assert.assertTrue(randomPoint32Array[1] == point.getY32()); Assert.assertTrue(randomPoint32Array[2] == point.getZ32()); point = new Point3D32(point2); EuclidCoreTestTools.assertTuple3DEquals(point, point2, EPS);
public Node generatePointCloudGraph(Point3D32[] pointCoordinates3d, ColorRGBA[] colorsRGBA) { Vector3f[] vectorArray = new Vector3f[pointCoordinates3d.length]; for (int i = 0; i < pointCoordinates3d.length; i++) { vectorArray[i] = new Vector3f(pointCoordinates3d[i].getX32(), pointCoordinates3d[i].getY32(), pointCoordinates3d[i].getZ32()); } return generatePointCloudGraph(vectorArray, colorsRGBA); }
com.jme3.math.Vector3f p = new com.jme3.math.Vector3f(p1.getX32(), p1.getY32(), p1.getZ32()); pointTransform.transformVector(p, p); dx = origin.getX32() - p.x; dy = origin.getY32() - p.y; dz = origin.getZ32() - p.z; Point3D32 curpt = new Point3D32(p.x, p.y, p.z); distance = origin.distanceSquared(curpt);
public static Point3D32[] unpackLocalizationPointMap(LocalizationPointMapPacket localizationPointMapPacket) { int numberOfPoints = localizationPointMapPacket.getLocalizationPointMap().size() / 3; Point3D32[] points = new Point3D32[numberOfPoints]; for (int i = 0; i < numberOfPoints; i++) { Point3D32 point = new Point3D32(); point.setX(localizationPointMapPacket.getLocalizationPointMap().get(3 * i)); point.setY(localizationPointMapPacket.getLocalizationPointMap().get(3 * i + 1)); point.setZ(localizationPointMapPacket.getLocalizationPointMap().get(3 * i + 2)); points[i] = point; } return points; }
@Override public void testSetters() throws Exception { super.testSetters(); Random random = new Random(621541L); Point3D32 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()); } }
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); } }
/** * Add a 3D line to this builder. * @param x0 x-coordinate of the line start. * @param y0 y-coordinate of the line start. * @param z0 z-coordinate of the line start. * @param xf x-coordinate of the line end. * @param yf y-coordinate of the line end. * @param zf z-coordinate of the line end. * @param lineWidth width of the line. * @param startColor color at the line start. Color accuracy depends on the color palette in use. * @param endColor color at the line end. Color accuracy depends on the color palette in use. */ public void addLine(float x0, float y0, float z0, float xf, float yf, float zf, float lineWidth, Color startColor, Color endColor) { MeshDataHolder lineMeshData = MeshDataGenerator.Line(x0, y0, z0, xf, yf, zf, lineWidth); Point3D32[] vertices = lineMeshData.getVertices(); TexCoord2f[] texturePoints = lineMeshData.getTexturePoints(); Point3D32 start = new Point3D32(x0, y0, z0); Point3D32 end = new Point3D32(xf, yf, zf); for (int i = 0; i < vertices.length; i++) { if (vertices[i].distanceSquared(start) < vertices[i].distanceSquared(end)) texturePoints[i].set(colorPalette.getTextureLocation(startColor)); else texturePoints[i].set(colorPalette.getTextureLocation(endColor)); } meshBuilder.addMesh(lineMeshData); }
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 protected void copy(Point3D32 src, Point3D32 dest) { dest.set(src); }
@Override protected float getY(Point3D32 data) { return data.getY32(); }
@Override protected float getZ(Point3D32 data) { return data.getZ32(); }
@Override protected float getX(Point3D32 data) { return data.getX32(); }
@Override public void receivedPacket(PointCloudWorldPacket pointCloud) { numberOfLidarScansConsumed++; // jmeLidarScanVisualizer.addPointCloud(Arrays.asList(pointCloud.getDecayingWorldScan())); try { List<Point3D32> lidarWorldPoints = Arrays.asList(HumanoidMessageTools.getDecayingWorldScan(pointCloud)); numberOfLidarPointsConsumed += lidarWorldPoints.size(); for (Point3D32 lidarWorldPoint : lidarWorldPoints) { if (lidarWorldPoint.getX() > 0.5) { assertEquals(WALL_DISTANCE, lidarWorldPoint.getX(), SCAN_TOLERANCE); } } } catch (AssertionError assertionError) { errorQueue.add(assertionError); } } }
if ((ray.direction != null) && (ray.origin != null) && (ray.direction.lengthSquared() != 0)) Point3D32 origin = new Point3D32(ray.getOrigin().x, ray.getOrigin().y, ray.getOrigin().z); Vector3D32 dir = new Vector3D32(ray.getDirection().x, ray.getDirection().y, ray.getDirection().z); Point3D32 nearest = getNearestIntersection(origin, dir, getLidarResolution(), geom.getWorldTransform()); if (nearest != null) CollisionResult collRes = new CollisionResult(geom, new com.jme3.math.Vector3f(nearest.getX32(), nearest.getY32(), nearest.getZ32()), (float) origin.distance(nearest), 0); collRes.setContactNormal(new com.jme3.math.Vector3f(0, 0, 1)); results.addCollision(collRes);
com.jme3.math.Vector3f p = new com.jme3.math.Vector3f(p1.getX32(), p1.getY32(), p1.getZ32()); pointTransform.transformVector(p, p); dx = origin.getX32() - p.x; dy = origin.getY32() - p.y; dz = origin.getZ32() - p.z; Point3D32 curpt = new Point3D32(p.x, p.y, p.z); distance = origin.distanceSquared(curpt);