/** * Returns the corners of the given bounding box in local coordinates. * * @param boundingBox The bounding box. * @return The corners of the bounding box in local coordinates. */ private static List<Point3d> getCorners(AxisAlignedBoundingBox3DFloat boundingBox) { final List<Point3d> corners = new ArrayList<>(); Translation lfb = boundingBox.getLeftFrontBottom(); Point3d leftFrontBottom = new Point3d(lfb.getX(), lfb.getY(), lfb.getZ()); for (boolean w : BOOLEAN_VALUES) { for (boolean d : BOOLEAN_VALUES) { for (boolean h : BOOLEAN_VALUES) { Point3d p = new Point3d(leftFrontBottom); if (w) { p.x += boundingBox.getWidth(); } if (d) { p.y += boundingBox.getDepth(); } if (h) { p.z += boundingBox.getHeight(); } corners.add(p); } } } return corners; }
@Override public void processData(String id, IdentifiableMessage<String, UnitConfig, UnitConfig.Builder> entry, ProtoBufMessageMap<String, UnitConfig, UnitConfig.Builder> entryMap, ProtoBufRegistry<String, UnitConfig, UnitConfig.Builder> registry) throws CouldNotPerformException, EntryModification { UnitConfig.Builder unitConfig = entry.getMessage().toBuilder(); if (unitConfig.getType() == UnitTemplateType.UnitTemplate.UnitType.LOCATION) { return; } if (unitConfig.hasPlacementConfig() && unitConfig.getPlacementConfig().hasPosition()) { final Pose pose = unitConfig.getPlacementConfig().getPosition(); if (pose.getTranslation().getX() == 0 && pose.getTranslation().getY() == 0 && pose.getTranslation().getZ() == 0) { unitConfig.getPlacementConfigBuilder().clearPosition(); throw new EntryModification(entry.setMessage(unitConfig), this); } } } }
/** * Gets the center coordinates of the unit's BoundingBox in the unit coordinate system as a Point3d object. * * @param unitConfig the unit config to refer the unit. * @return center coordinates of the unit's BoundingBox relative to unit * @throws NotAvailableException is thrown if the center can not be calculate. */ default Point3d getUnitBoundingBoxCenterPoint3d(final UnitConfig unitConfig) throws NotAvailableException { final AxisAlignedBoundingBox3DFloatType.AxisAlignedBoundingBox3DFloat bb = getUnitShape(unitConfig).getBoundingBox(); final TranslationType.Translation lfc = bb.getLeftFrontBottom(); final Point3d center = new Point3d(bb.getWidth(), bb.getDepth(), bb.getHeight()); center.scale(0.5); center.add(new Point3d(lfc.getX(), lfc.getY(), lfc.getZ())); return center; }
private Vector3d toVector(final Translation translation) { return new Vector3d(translation.getX(), translation.getY(), translation.getZ()); }
public static final Point3D toPoint3D(final Translation translation) { return new Point3D(translation.getX(), translation.getY(), translation.getZ()); }
/** * Transforms the argument to root coordinates. * * @param translation translation that shall be transformed. * @return The transformed <code>translation</code> argument. */ private Translation transformPosition(Translation translation) { Translation.Builder transformedTranslation = translation.toBuilder().clone().clearX().clearY().clearZ(); if (translation.hasX() && translation.hasY() && translation.hasZ()) { Point3d point = new Point3d(-translation.getX(), -translation.getY(), translation.getZ()); transform.transform(point); transformedTranslation.setX(point.x).setY(point.y).setZ(point.z); } return transformedTranslation.build(); }
public static final Vec3DFloat toVec3DFloat(final Translation translation) { return Vec3DFloat.newBuilder() .setX((float) translation.getX()) .setY((float) translation.getY()) .setZ((float) translation.getZ()).build(); }
public static Transform3D transform(final PoseType.Pose position) { RotationType.Rotation pRotation = position.getRotation(); TranslationType.Translation pTranslation = position.getTranslation(); Quat4d jRotation = new Quat4d(pRotation.getQx(), pRotation.getQy(), pRotation.getQz(), pRotation.getQw()); Vector3d jTranslation = new Vector3d(pTranslation.getX(), pTranslation.getY(), pTranslation.getZ()); return new Transform3D(jRotation, jTranslation, 1.0); } }