public NavPoint getNearestNavPoint(SPLocation location) { // TODO: implement using oct-trees Point3d loc = location.asPoint3d(); double nearestDistance = Double.MAX_VALUE; NavPoint nearest = null; for (NavPoint navPoint : getNavPoints()) { try{ double distance = loc.distance(navPoint.getLocation().getPoint3d()); if (distance < nearestDistance) { nearestDistance = distance; nearest = navPoint; } }catch(NullPointerException npe){ } } return nearest; }
private void createBlocks( Closer<Point3d> closer, LoopL<Point3d> polies ) { Map<Point3d, Integer> bMap = closer.findMap(); if (TweedSettings.settings.snapFootprintVert > 0) { Loopz.dirtySnap(polies, TweedSettings.settings.snapFootprintVert); } for ( Loop<Point3d> poly : polies) if (poly.count() > 0) { int key = bMap.get ( poly.start.get() ); LoopL<Point3d> loopl = blocks.get(key); if (loopl == null) blocks.put(key, loopl = new LoopL<>() ); if (TweedSettings.settings.calculateFootprintNormals) { if (Loopz.area( Loopz.to2dLoop( poly, 1, null ) ) < 0) poly.reverse(); } loopl.add(poly); } }
public static Prof buildProfile( Line3d oLine, Point3d cen ) { Matrix4d to2D = new Matrix4d(); Vector3d c2 = oLine.dir(), c3 = new Vector3d(); c2.normalize(); c3.cross( c2, UP ); to2D.setIdentity(); to2D.setRow( 0, c3.x, c3.y, c3.z, 0 ); to2D.setRow( 1, UP.x, UP.y, UP.z, 0 ); to2D.setRow( 2, c2.x, c2.y, c2.z, 0 ); { Point3d start = new Point3d( cen.x, 0, cen.z ); to2D.transform( start ); to2D.m03 = -start.x; to2D.m13 = -start.y; to2D.m23 = -start.z; to2D.m33 = 1; } Prof monotonic = new Prof(to2D, c2); return monotonic; }
public Point3d at3DHeight( double h3 ) { double h = to2d (new Point3d(0,h3,0) ).y; int i = 0; while (get(i).y <= h && i < size() - 1) i++; if (i == 0) return null; else if (get(i).y <= h) return null; else return to3d( new Point2d( new Line(get(i-1), get(i)).xAtY( h ), h ) ); }
private boolean inBounds( Matrix4d mini, List<double[]> bounds ) { // mini matrix is in mini-mesh format: a translation from a 255^3 cube in the first quadrant // trans.offset is a transform from that space, into jme rendered space (cartesian in meters, around the origin) Matrix4d m = new Matrix4d(); m.mul( Jme3z.fromMatrix ( trans.offset ), mini ); for (Point2d p : Arrays.stream( cubeCorners ).map( c -> { Point3d tmp = new Point3d(); m.transform( c, tmp ); return new Point2d(tmp.x, tmp.z); }).collect( Collectors.toList() ) ) { for (double[] bound : bounds) { if ( bound[0] < p.x && bound[1] > p.x && bound[2] < p.y && bound[3] > p.y ) return true; } } return false; }
@Override public void blockSelected( LoopL<Point3d> polies, BlockGen blockGen ) { Point2d cen = Loopz.average( Loopz.to2dLoop( polies, 1, null ) ); for (File f : new File (tweed.DATA+File.separator +"solutions").listFiles()) { try { Point2d fp = FeatureCache.fileToCenter( f.getName() ); if (fp.distanceSquared( cen ) < 1) { new Thread( () -> load( f, true ) ).start(); return; } } catch (Throwable th) { System.out.println( "unable to read solution "+f.getName() ); } } JOptionPane.showMessageDialog( tweed.frame(), "Can't find solution for center " + cen ); }
private static boolean order( Face a, Face b ) { Point3d ap = a.definingSE.iterator().next().getStart( a ); Point3d bp = b.definingSE.iterator().next().getStart(b); if (ap == null || bp == null) return true; //?! if (ap.x < bp.x) return true; if (ap.x > bp.x) return false; if (ap.y < bp.y) return true; if (ap.y > bp.y) return false; return false; }
public static Point2d worldToLLong( Tweed tweed, Point3d cen ) { Point3d out = new Point3d( cen ); TweedSettings.settings.fromOrigin.transform( out ); try { double[] latLong = new double[3]; toLatLong.transform( new double[] { out.x, out.y, out.z }, 0, latLong, 0, 1 ); return new Point2d( latLong[ 0 ], latLong[ 1 ] ); } catch ( TransformException e ) { e.printStackTrace(); } return null; }
private Point3d interpolateFP(Point2d point) { double minDistance = Double.POSITIVE_INFINITY; Point3d nearestPoint = new Point3d(); for (Point3d midlinePoint : midlinePoints) { double distance = point.distance(new Point2d(midlinePoint.y, midlinePoint.z)); if (distance < minDistance) { minDistance = distance; nearestPoint = midlinePoint; if (distance == 0.d) { break; } } } return nearestPoint; }
public float[] getImageCoord(Point3d p) { int nearestIndex = 0; int indexCounter = 0; double minDistance = Double.POSITIVE_INFINITY; for (Vector3f vert : verts) { double distance = new Point3d(p).distance(new Point3d(vert)); if (distance < minDistance) { minDistance = distance; nearestIndex = indexCounter; if (distance == 0) { break; } } indexCounter++; } Vector3f coordVector = texCoords.get(vertTexMap.get(nearestIndex)); return new float[]{coordVector.x, coordVector.y}; }
private Point getTextureFPfromModel(FacialPointType type) { if (objFPmodel == null) { return null; } FacialPoint fp = objFPmodel.getFacialPoint(type); Point imagePoint = textureMapper.getImagePoint(new Point3d(fp.getCoords())); imagePoint.x *= originalImage.height(); imagePoint.y = imagePoint.y; imagePoint.y *= originalImage.width(); imagePoint.x *= resizeRatio; imagePoint.y *= resizeRatio; Point rotatedPoint = rotatePoint(imagePoint, 90, workingImage); OCVutils.drawPoint(workingImage, rotatedPoint, OCVutils.GREEN); return imagePoint; }
public static Corner getIntersectionBetweenCornerAndPlane(PlaneEquation planeEq, Corner corner, LinkedList<Point3d> intersectPoints, ArrayList<Vector3f> verts) { Vector3f point1 = verts.get(corner.vertex); Vector3f point2 = verts.get(corner.next.vertex); Vector3f point3 = verts.get(corner.prev.vertex); Point3d intersectPoint; Corner intersectCorner = null; intersectPoint = getIntersectionBetweenLineAndPlane(planeEq, point2, point3); if (intersectPoint != null) { intersectCorner = corner.next; } else { intersectPoint = getIntersectionBetweenLineAndPlane(planeEq, point3, point1); if (intersectPoint != null) { intersectCorner = corner.prev; } } if (intersectPoint != null) { intersectPoints.add(intersectPoint); } return intersectCorner; }
/** * Permet de déplacer de manière à centrer sur le point P(x y z ) * @param x * @param y * @param z * @param direction * Il s'agit de la direction dans laquelle est regardée le * point. La caméra se trouvera à la translation vecteur appliqué à P * La norme du vecteur indique la distance entre la caméra et le * point */ public void zoomOn(double x, double y, double z, Vecteur direction) { Transform3D viewTrans = new Transform3D(); if (direction == null) { return; } if (direction.norme() == 0) { return; } // point the view at the center of the object Point3d center = new Point3d(x + this.translate.x, y + this.translate.y, z + this.translate.z); Point3d eyePos = new Point3d(x + this.translate.x + direction.getX(), y + this.translate.y + direction.getY(), z + this.translate.z + direction.getZ()); viewTrans.setIdentity(); viewTrans.lookAt(eyePos, center, new Vector3d(0, 0, 1)); // set the view transform viewTrans.invert(); InterfaceMap3D.tgvu.setTransform(viewTrans); }
/** * Gets the C-alpha {@link Atom} for the given input {@link Segment}. * @param segment the input {@link Segment} object * @return the C-alpha array of {@link Atom} objects */ public static Atom[] getCaAtoms(Segment segment) { Point3d[] points = segment.getCoordinates(); Chain chain = new ChainImpl(); chain.setId(CHAIN_NAME); chain.setName(CHAIN_NAME); Atom[] atoms = new Atom[points.length]; for (int i = 0, j = 0; i < points.length; i++) { if (points[i] != null) { atoms[j] = new AtomImpl(); atoms[j].setName(CA_NAME); Group group = new AminoAcidImpl(); group.setPDBName("GLU"); group.addAtom(atoms[j]); group.setChain(chain); group.setResidueNumber(new ResidueNumber(CHAIN_NAME, j, '\0')); atoms[j].setX(points[i].x); atoms[j].setY(points[i].y); atoms[j].setZ(points[i].z); j++; } } return atoms; }
/** * Returns a texture coordinates generator that wraps the given texture on front face. */ private TexCoordGeneration getTextureCoordinates(Appearance appearance, HomeTexture texture, Vector3f pieceSize, BoundingBox modelBounds) { Point3d lower = new Point3d(); modelBounds.getLower(lower); Point3d upper = new Point3d(); modelBounds.getUpper(upper); float minimumSize = ModelManager.getInstance().getMinimumSize(); float textureWidth = TextureManager.getInstance().getRotatedTextureWidth(texture); float textureHeight = TextureManager.getInstance().getRotatedTextureHeight(texture); float sx = pieceSize.x / (float) Math.max(upper.x - lower.x, minimumSize) / textureWidth; float sw = texture.isLeftToRightOriented() ? (float) -lower.x * sx : 0; float ty = pieceSize.y / (float) Math.max(upper.y - lower.y, minimumSize) / textureHeight; float tz = pieceSize.z / (float) Math.max(upper.z - lower.z, minimumSize) / textureHeight; float tw = texture.isLeftToRightOriented() ? (float) (-lower.y * ty + upper.z * tz) : 0; return new TexCoordGeneration(TexCoordGeneration.OBJECT_LINEAR, TexCoordGeneration.TEXTURE_COORDINATE_2, new Vector4f(sx, 0, 0, sw), new Vector4f(0, ty, -tz, tw)); }
public J3dTest(){ // 创建一个虚拟空间 SimpleUniverse universe = new SimpleUniverse(); // 创建一个用来包含对象的数据结构 BranchGroup group = new BranchGroup(); // 创建一个球并把它加入到group中 Sphere sphere = new Sphere(0.5f); // 小球的半径为0.5米 group.addChild(sphere); Color3f light1Color = new Color3f(1.8f, 0.1f, 0.1f); // 设置光线的颜色 BoundingSphere bounds = new BoundingSphere(new Point3d(0.0,0.0,0.0), 100.0); // 设置光线的作用范围 Vector3f light1Direction = new Vector3f(4.0f, -7.0f, -12.0f); // 设置光线的方向 DirectionalLight light1= new DirectionalLight(light1Color, light1Direction); // 指定颜色和方向,产生单向光源 light1.setInfluencingBounds(bounds); // 把光线的作用范围加入光源中 group.addChild(light1); // 将光源加入group组,安放观察点 universe.getViewingPlatform().setNominalViewingTransform(); // 把group加入到虚拟空间中 universe.addBranchGraph(group); }
public static void main(String[] args) { SurfaceEvaluator evaluator = new SurfaceEvaluator(NUM_COLUMNS, NUM_ROWS); God<UV, Point3d> god = new God<UV, Point3d>(POPULATION_SIZE, evaluator, S3DAltOrganismFactory.INSTANCE); god.addFactory(new WrappedVector3Factory<Pixel>(MultiplyNodeFactory.INSTANCE)); god.addFactory(new WrappedVector3Factory<Pixel>(AddNodeFactory.INSTANCE)); god.addFactory(new WrappedVector3Factory<Pixel>(new XNodeFactory(3))); god.addFactory(new WrappedVector3Factory<Pixel>(new YNodeFactory(3))); // god.addFactory(new WrappedVector3Factory<Pixel>(AndNodeFactory.INSTANCE)); god.addFactory(new WrappedVector3Factory<Pixel>(SineNodeFactory.INSTANCE)); god.addFactory(new WrappedVector3Factory<Pixel>(new ConstantNodeFactory(3))); for (Mutation mut : BasicMutation.values()) { god.addMutation(mut); } god.run(); }
/** * Constructs a SimpleAgent. * * @param pos * the starting position. * @param name * the name of the agent. */ public SimpleAgent(Vector3d pos, String name) { this.name = name; super.create3D(true); startPosition = new Vector3d(pos); sensors = new ArrayList(); actuators = new ArrayList(); // interactingAgents = new ArrayList(); // reserve collision detection stuff collisionBounds = new BoundingSphere(); collisionPickBounds = new PickBounds(); collisionBoundsCenter = new Point3d(); detachedFromSceneGraph = false; }
/** Create 3D geometry. */ @Override protected void create3D() { Appearance appear = new Appearance(); material.setDiffuseColor(color); material.setSpecularColor(black); appear.setMaterial(material); int flags = Primitive.GEOMETRY_NOT_SHARED | Primitive.ENABLE_GEOMETRY_PICKING | Primitive.GENERATE_NORMALS; body = new Sphere(radius, flags, appear); // we must be able to change the pick flag of the agent body.setCapability(Node.ALLOW_PICKABLE_READ); body.setCapability(Node.ALLOW_PICKABLE_WRITE); body.setPickable(true); body.setCollidable(true); addChild(body); // Add bounds for interactions Bounds bounds = new BoundingSphere(new Point3d(0, 0, 0), radius); setBounds(bounds); }
/** Create 3D geometry. */ @Override protected void create3D() { Appearance appear = new Appearance(); material.setCapability(Material.ALLOW_COMPONENT_WRITE); material.setDiffuseColor(color); material.setSpecularColor(black); appear.setMaterial(material); int flags = Primitive.GEOMETRY_NOT_SHARED | Primitive.ENABLE_GEOMETRY_PICKING | Primitive.GENERATE_NORMALS; body = new Sphere(radius, flags, appear); // we must be able to change the pick flag of the agent body.setCapability(Node.ALLOW_PICKABLE_READ); body.setCapability(Node.ALLOW_PICKABLE_WRITE); body.setPickable(true); body.setCollidable(true); addChild(body); // Add bounds for interactions Bounds bounds = new BoundingSphere(new Point3d(0, 0, 0), radius); setBounds(bounds); }
public static LoopL<Bar> fromEdges( LoopL<Edge> outside ) { LoopL<Bar> loopl = new LoopL(); Cache<Point3d, Point2d> cache = new Cache<Point3d, Point2d>() { @Override public Point2d create( Point3d i ) { return new Point2d (i.x, i.y); } }; for (Loop<Edge> pLoop : outside) { Loop<Bar> loop = new Loop(); loopl.add( loop ); for ( Edge edge : pLoop ) loop.append( new Bar( cache.get( edge.start ), cache.get( edge.end ) ) ); } return loopl; }
private void intersect(Point3d result, Point3d lowerPt, double lowerR, Point3d upperPt, double upperR, double desiredR) { double deltaR = upperR - lowerR; double desiredDeltaR = desiredR - lowerR; double fraction = 0.0; if (deltaR > 0.00001) { fraction = desiredDeltaR / deltaR; if (fraction > 1.0) { fraction = 1.0; } } result.x = ((1.0 - fraction) * lowerPt.x) + fraction * upperPt.x; result.y = ((1.0 - fraction) * lowerPt.y) + fraction * upperPt.y; result.z = ((1.0 - fraction) * lowerPt.z) + fraction * upperPt.z; }
void updateCenter(Point3d minCoord, Point3d maxCoord) { centerOffset.x = -(maxCoord.x - minCoord.x) / 2.0; centerOffset.y = -(maxCoord.y - minCoord.y) / 2.0; centerOffset.z = -(maxCoord.z - minCoord.z) / 2.0; if (Double.isNaN(centerOffset.x) || Double.isInfinite(centerOffset.x)) { centerOffset.x = 0; } if (Double.isNaN(centerOffset.y) || Double.isInfinite(centerOffset.y)) { centerOffset.y = 0; } if (Double.isNaN(centerOffset.z) || Double.isInfinite(centerOffset.z)) { centerOffset.z = 0; } System.out.println(centerOffset); centerXform.setTranslation(centerOffset); centerGroup.setTransform(centerXform); }
public static double distFromPointToLine(Point4d x0, Point4d x1, Point4d x2) { Point3d v = p4top3(x0), l = p4top3(x1), r = p4top3(x2); // calculate |v|, that is, the distance from the coordinate // v to the line (l, r) // |(r - l) x (l - v) | // this is given by |v| = ----------------------- // |(r - l)| // where a,b are points of the line and c the point to measure // TODO can get rid of sqrt if we want, but it is no big deal Vector3d rl = new Vector3d(); rl.sub(r, l); Vector3d lv = new Vector3d(); lv.sub(l, v); Vector3d temp = new Vector3d(); temp.cross(rl, lv); double dist = temp.length()/rl.length(); return dist; }
PointArray makePointGeometry( Connector ball, AlgebraicField field, Embedding embedding ) { PointArray result = new PointArray( 1, GeometryArray.COORDINATES ); Point3d pt = new Point3d(); AlgebraicVector gv = null; RealVector v = null; gv = ball .getLocation(); v = embedding .embedInR3( gv ); pt.x = v.x; pt.y = v.y; pt.z = v.z; result .setCoordinate( 0, pt ); return result; }
public void setLines() { LineArray ver_Line = new LineArray(8, LineArray.COORDINATES); Point3d verts2[] = new Point3d[8]; //bottom two lines verts2[0] = new Point3d(0, 0, 0); verts2[1] = new Point3d(0, 1.6, 0); verts2[2] = new Point3d(0.6, 0, 0); verts2[3] = new Point3d(0.6, 1.6, 0); //top two lines verts2[4] = new Point3d(0, 0, -0.6); verts2[5] = new Point3d(0, 1.6, -0.6); verts2[6] = new Point3d(0.6, 0, -0.6); verts2[7] = new Point3d(0.6, 1.6, -0.6); ver_Line.setCoordinates(0, verts2); setGeometry(ver_Line); }
public static void main(String[] args) { SurfaceEvaluator evaluator = new SurfaceEvaluator(NUM_COLUMNS, NUM_ROWS); God<UV, Point3d> god = new God<UV, Point3d>(POPULATION_SIZE, evaluator, S3DOrganismFactory.INSTANCE); god.addFactory(new WrappedDoubleFactory<Pixel>(MultiplyNodeFactory.INSTANCE)); god.addFactory(new WrappedDoubleFactory<Pixel>(AddNodeFactory.INSTANCE)); god.addFactory(new WrappedDoubleFactory<Pixel>(new XNodeFactory(1))); god.addFactory(new WrappedDoubleFactory<Pixel>(new YNodeFactory(1))); // god.addFactory(new WrappedDoubleFactory<Pixel>(AndNodeFactory.INSTANCE)); god.addFactory(new WrappedDoubleFactory<Pixel>(SineNodeFactory.INSTANCE)); god.addFactory(new WrappedDoubleFactory<Pixel>(new ConstantNodeFactory(1))); god.addFactory(new WrappedDoubleFactory<Pixel>(AbsNodeFactory.INSTANCE)); god.addFactory(new WrappedDoubleFactory<Pixel>(BlendXNodeFactory.INSTANCE), .25); god.addFactory(new WrappedDoubleFactory<Pixel>(BlendYNodeFactory.INSTANCE), .25); god.addFactory(new WrappedDoubleFactory<Pixel>(CubeNodeFactory.INSTANCE)); //god.addFactory(new WrappedDoubleFactory<Pixel>(ExpNodeFactory.INSTANCE)); for (Mutation mut : BasicMutation.values()) { god.addMutation(mut); } god.run(); }
/** Create polyline. * @param points array of <code>Point3D</code>s * @param appearance object' Appearance * @preconditions points.length > 1 */ public PolyLine(Point3d[] points, Appearance appearance) { if (points.length < 2) { return; } int[] counts = new int[1]; counts[0] = points.length; LineStripArray lineArray = new LineStripArray(points.length, GeometryArray.COORDINATES| GeometryArray.NORMALS, counts); float[] normal = {0, 0, 0}; lineArray.setCoordinates(0, points); lineArray.setNormal(0, normal); setGeometry(lineArray); setAppearance(appearance); }
public static LoopL<Point3d> dupeNewAllPoints( LoopL<Point3d> ribbon ) { final Cache<Point3d, Point3d> cacheC = new Cache<Point3d, Point3d>() { @Override public Point3d create(Point3d i) { return new Point3d(i); } }; LoopL<Point3d> loopl = new LoopL(); for (Loop<Point3d> pLoop : ribbon) { Loop<Point3d> loop = new Loop(); loopl.add( loop ); for ( Point3d c : pLoop ) loop.append( cacheC.get(c) ); } return loopl; }
/** * Prueft, ob ein Punkt ueber einem Loch liegt * * @param pos * @return true, wenn der Bot ueber dem loch steht */ public boolean checkHole(Point3d pos) { if ((pos.x < 0) || (pos.y < 0) || (pos.x > dimX * blockSizeInM) || (pos.y > dimY * blockSizeInM)) { return true; } Iterator it = holes.iterator(); while (it.hasNext()) { Vector2f min = new Vector2f((Vector2d) it.next()); min.scale(blockSizeInM); Vector2f max = new Vector2f(min); max.add(new Vector2f(blockSizeInM, blockSizeInM)); if ((pos.x > min.x) && (pos.x < max.x) && (pos.y > min.y) && (pos.y < max.y)) { return true; } } return false; }
/** * @param newPos * @param newHeading * @param dX * @param dY * @param radius * @param t Transformationsmatrix (wird veraendert) * @return erzeugte Bounds */ private Bounds createBounds(Point3d newPos, double newHeading, double dX, double dY, double radius, Transform3D t) { final double dZ = - CtBotSimTcp.BOT_HEIGHT / 2; /* Vektor fuer die Verschiebung erstellen */ Vector3d v = new Vector3d(dX, dY, dZ); /* Transformations-Matrix fuer die Rotation erstellen */ Transform3D r = new Transform3D(); r.rotZ(newHeading); /* Transformation um Verschiebung ergaenzen */ r.transform(v); v.add(newPos); t.setIdentity(); t.setTranslation(v); /* Bounds erstellen */ Bounds bounds = new BoundingSphere(new Point3d(0, 0, 0), radius); /* Bounds transformieren */ bounds.transform(t); return bounds; }
/** * Convert the terrain coordinates from raw form to TriangleStripArray form * (with repeated nodes) * @param inRawPoints array of raw points as formed from the track * @return point coordinates as array */ public Point3d[] getTerrainCoordinates(Point3d[] inRawPoints) { final int numNodes = _gridSize * _gridSize; if (_gridSize <= 1 || inRawPoints == null || inRawPoints.length != numNodes) {return null;} // Put these nodes into a new result array (repeating nodes as necessary) final int resultSize = _gridSize * (_gridSize * 2 - 2); Point3d[] result = new Point3d[resultSize]; final int numStrips = _gridSize - 1; int resultIndex = 0; for (int strip=0; strip<numStrips; strip++) { for (int col=0; col<_gridSize; col++) { int bottomNodeIndex = strip * _gridSize + col; int topNodeIndex = bottomNodeIndex + _gridSize; result[resultIndex++] = inRawPoints[bottomNodeIndex]; result[resultIndex++] = inRawPoints[topNodeIndex]; } } return result; }
/** * @param sensor Distanzsensor * @param isLeft links? * @throws IOException */ public void buisitDistanceSim(final Sensors.Distance sensor, boolean isLeft) throws IOException { final double MAX_RANGE = 0.85; final Point3d distFromBotCenter = isLeft ? new Point3d(- 0.036, 0.0554, 0) : new Point3d(+ 0.036, 0.0554, 0); final Point3d endPoint = new Point3d(distFromBotCenter); endPoint.add(new Point3d(0.0, MAX_RANGE, 0.0)); final boolean GP2Y0A60 = Config.getValue("GP2Y0A60").equals("true"); final Characteristic charstic = GP2Y0A60 ? (isLeft ? new Characteristic("characteristics/gp2y0a60Left.txt", 100) : new Characteristic("characteristics/gp2y0a60Right.txt", 100)) : (isLeft ? new Characteristic("characteristics/gp2d12Left.txt", 100) : new Characteristic("characteristics/gp2d12Right.txt", 80)); simulators.add(new Runnable() { private final double OPENING_ANGLE_IN_RAD = Math.toRadians(3); public void run() { double distInM = world.watchObstacle(parent.worldCoordFromBotCoord(distFromBotCenter), parent.worldCoordFromBotCoord(endPoint), OPENING_ANGLE_IN_RAD, parent.getShape()); double distInCm = 100 * distInM; double sensorReading = charstic.lookupPrecise(distInCm); sensor.set(sensorReading); } }); }
/** * @param sensor Lichtsensor * @param isLeft links? */ public void buisitLightSim(final Sensors.Light sensor, boolean isLeft) { final Point3d distFromBotCenter = isLeft ? new Point3d(- 0.032, 0.048, 0.060 - CtBotSimTcp.BOT_HEIGHT / 2) : new Point3d(+ 0.032, 0.048, 0.060 - CtBotSimTcp.BOT_HEIGHT / 2); final Vector3d headingInBotCoord = looksForward(); simulators.add(new Runnable() { public void run() { sensor.set(world.sensLight( parent.worldCoordFromBotCoord(distFromBotCenter), parent.worldCoordFromBotCoord(headingInBotCoord), LIGHT_MAX_DISTANCE, LIGHT_OPENING_ANGLE)); } }); }
@Override public boolean process( Skeleton skel ) { SkeletonCapUpdate capUpdate = new SkeletonCapUpdate(skel); LoopL<Corner> flatTop = capUpdate.getCap(height); LoopL<Point3d> pts = new LoopL(); for (Loop<Corner> cLoop : flatTop) { Loop<Point3d> loop = new Loop(); pts.add(loop); for ( Corner c : cLoop) loop.append( new Point3d( c.x, c.y, height) ); } skel.output.addNonSkeletonOutputFace2(pts , new Vector3d(0,0,1) ); capUpdate.update(new LoopL(), new SetCorrespondence<Corner, Corner>(), new DHash<Corner, Corner>()); skel.qu.clearFaceEvents(); skel.qu.clearOtherEvents(); return true; }
/** * <p> * Liefert die Helligkeit, die auf einen Lichtsensor fällt. * </p> * <p> * Da diese Methode unter Verwendung des PickConeRay implementiert ist, ist * sie seinen Bugs unterworfen. Ausführliche Dokumentation siehe * watchObstacle(Point3d, Vector3d, double, Shape3D). * </p> * * @param pos Die Position des Lichtsensors * @param heading Die Blickrichtung des Lichtsensors * @param lightReach Reichweite des Lichtsensors / m * @param openingAngle Der Oeffnungswinkel des Blickstrahls / radians * @return Die Dunkelheit um den Sensor herum, von 1023 (100%) bis 0 (0%) * @see PickConeRay */ public int sensLight(Point3d pos, Vector3d heading, double lightReach, double openingAngle) { // Falls die Welt verschoben wurde: Point3d relPos = new Point3d(pos); Transform3D transform = new Transform3D(); worldTG.getTransform(transform); transform.transform(relPos); // oder rotiert: Vector3d relHeading = new Vector3d(heading); transform.transform(relHeading); PickConeRay picky = new PickConeRay(relPos, relHeading, openingAngle); PickInfo pickInfo; pickInfo = lightBG.pickClosest(PickInfo.PICK_GEOMETRY, PickInfo.CLOSEST_DISTANCE, picky); if (pickInfo == null) { return 1023; } int darkness = (int) ((pickInfo.getClosestDistance() / lightReach) * 1023.0); if (darkness > 1023) { darkness = 1023; } return darkness; }
public NavPoint getFurthestNavPoint(SPLocation location) { // TODO: implement using oct-trees Point3d loc = location.asPoint3d(); double furthestDistance = Double.MAX_VALUE; NavPoint furthest = null; for (NavPoint navPoint : getNavPoints()) { double distance = loc.distance(navPoint.getLocation().getPoint3d()); if (distance > furthestDistance) { furthestDistance = distance; furthest = navPoint; } } return furthest; }
public NavPoint getNearestNavPoint(SPLocation location) { // TODO: implement using oct-trees Point3d loc = location.asPoint3d(); double nearestDistance = Double.MAX_VALUE; NavPoint nearest = null; for (NavPoint navPoint : getNavPoints()) { double distance = loc.distance(navPoint.getLocation().getPoint3d()); if (distance < nearestDistance) { nearestDistance = distance; nearest = navPoint; } } return nearest; }
private Vector3f toWorld (Vector2f a) { Vector3f s = target.getWorldBound().getCenter(); Vector3f c = tweed.getCamera().getLocation(); Vector3f cd = tweed.getCamera().getWorldCoordinates(a, 0).subtract(c); Point3d pt = new Ray3d ( tod ( c ), tod ( cd ) ).closestPointOn( new Ray3d ( tod ( s ), tod ( dir ) ) ); return new Vector3f( (float) pt.x, (float) pt.y, (float) pt.z); }