Java 类javax.vecmath.Vector3d 实例源码

private Vector3d steerCurrLink(NavPointNeighbourLink currLink, NavPointNeighbourLink nextLink) {
    Location projection = NavigationGraphHelper.projectPointToLinkLine(currLink, bot.getLocation());
    double distance = NavigationGraphHelper.NAV_LINK_GET_DISTANCE.getDistance(currLink, bot.getLocation());

    double ratio;

    if (currLink.getCollisionR() <= 0) {
        ratio = 0;
    } else {
        ratio = distance / (double)currLink.getCollisionR();
    }

    Location result = projection.sub(bot.getLocation()).scale(ratio).setZ(0);

    if (SteeringManager.DEBUG) {
        System.out.println("StickToPathSteer: HAS LINK");
        System.out.println("StickToPathSteer: collision radius = " + currLink.getCollisionR());
        System.out.println("StickToPathSteer: distance         = " + distance);
        System.out.println("StickToPathSteer: ratio            = " + ratio);
        System.out.println("StickToPathSteer: result           = [" + result.x + ", " + result.y + ", " + result.z + "]");
        System.out.println("StickToPathSteer: length           = " + result.getLength());
    }

    return result.asVector3d();
}

private void prepareRays() {

        /*Délky postranních paprsků jsou 8 a 12. Délky šikmých se vynásobí 2 a předního se vynásobí odmocninou(3).*/
        int shortLength = 8;
        int longLength = 12;

        shortSideRayLength = (int) (UnrealUtils.CHARACTER_COLLISION_RADIUS * shortLength * DISTANCE_FROM_THE_WALL / 166f);        //8
        longSideRayLength = (int) (UnrealUtils.CHARACTER_COLLISION_RADIUS * longLength * DISTANCE_FROM_THE_WALL / 166f);        //12
        shortSideFrontRayLength = (int) (UnrealUtils.CHARACTER_COLLISION_RADIUS * shortLength * 2 * DISTANCE_FROM_THE_WALL / 166f);  //20
        longSideFrontRayLength = (int) (UnrealUtils.CHARACTER_COLLISION_RADIUS * longLength * 2 * DISTANCE_FROM_THE_WALL / 166f);   //30
        shortFrontRayLength = (int) (UnrealUtils.CHARACTER_COLLISION_RADIUS * shortLength * Math.sqrt(3) * DISTANCE_FROM_THE_WALL / 166f);      //18
        longFrontRayLength = (int) (UnrealUtils.CHARACTER_COLLISION_RADIUS * longLength * Math.sqrt(3) * DISTANCE_FROM_THE_WALL / 166f);       //27

        //Five rays are created.
        LinkedList<SteeringRay> rayList = new LinkedList<SteeringRay>();
        rayList.add(new SteeringRay(NLEFT, new Vector3d(0, -1, 0), longSideRayLength));
        rayList.add(new SteeringRay(NLEFTFRONT, new Vector3d(Math.sqrt(3), -1, 0), longSideFrontRayLength));
        rayList.add(new SteeringRay(NRIGHTFRONT, new Vector3d(Math.sqrt(3), 1, 0), longSideFrontRayLength));
        rayList.add(new SteeringRay(NRIGHT, new Vector3d(0, 1, 0), longSideRayLength));
        rayList.add(new SteeringRay(NFRONT, new Vector3d(1, 0, 0), longFrontRayLength));
        rayManager.addRays(SteeringType.WALL_FOLLOWING, rayList, this);
        raysReady = false;
        //System.out.println("Rays wall preparation end.");
    }

/**If it's really the front collision, the "mirror vector" is returned. Otherwise the unchanged parameter normal is returned.*/
private Vector3d computeFrontCollisionVector(Vector3d normal) {
    Vector3d av = botself.getVelocity().getVector3d();
    Vector3d result = new Vector3d(normal.x, normal.y, 0);
    Vector3d negativeActual = new Vector3d(-av.x, -av.y, 0);

    if (SteeringManager.DEBUG) System.out.println("Angle "+SteeringTools.radiansToDegrees(normal.angle(negativeActual)));
    if (result.angle(negativeActual) <= Math.PI/2) {
        boolean turnLeft;
        if (result.angle(negativeActual) == 0) {
            turnLeft = random.nextBoolean();
        } else {
            turnLeft = SteeringTools.pointIsLeftFromTheVector(av, result);
        }
        Vector3d turn = SteeringTools.getTurningVector2(av, turnLeft);  //Tady se původně používal getTurningVector1.
        turn.normalize();
        turn.scale(0.5);    //Aby neměl rotační vektor tak velký vliv.
        result.add(turn);
        result.normalize();
        if (SteeringManager.DEBUG) System.out.println("Obstacle avoidance front collision: turn left "+turnLeft);
    }
    return result;
}

/**
 * Creates the new SteeringManager. This class manages the whole navigation layer of the steered bot. The manager calls steerings to compute the force, combines all forces and sends the computed next velocity to the locomotion layer (modul locomotion).
 * @param bot The bot, who should be steered be these steerings.
 * @param raycasting The instance of the class Raycasting.
 * @param locomotion The AdvancedLocomotion of the bot.
 */
public SteeringManager(UT2004Bot bot, Raycasting raycasting, AdvancedLocomotion locomotion) {
    this.botself = bot;
    //this.raycasting = raycasting;
    this.locomotion = locomotion;
    this.multiplier = 1;
    rayManager = new RaycastingManager(botself, raycasting);
    mySteerings = new HashMap<SteeringType,ISteering>();
    steeringWeights = new HashMap<SteeringType, Double>();
    steeringForces = new HashMap<SteeringType, Vector3d>();
    steeringManagerInitialized();
    myActualVelocity = new Vector3d();
    myNextVelocity = new Vector3d();
    drawRaycasting = false;
    canEnlargeVelocity = true;
}

/**
 *
 * @param bot The bot, who should be steered be these steerings. This class manages the whole navigation layer of the steered bot. The manager calls steerings to compute the force, combines all forces and sends the computed next velocity to the locomotion layer (modul locomotion).
 * @param raycasting The instance of the class Raycasting.
 * @param locomotion The AdvancedLocomotion of the bot.
 * @param multiplier Default value is 1. The multiplier of the velocity. E.g, if you want to make this bot run nearly all the time, set the multiplier 2. But remember, that steerings could work worse.
 */
public SteeringManager(UT2004Bot bot, Raycasting raycasting, AdvancedLocomotion locomotion, double multiplier) {
    this.botself = bot;
    //this.raycasting = raycasting;
    this.locomotion = locomotion;
    this.multiplier = multiplier;
    rayManager = new RaycastingManager(botself, raycasting);
    mySteerings = new HashMap<SteeringType,ISteering>();
    steeringWeights = new HashMap<SteeringType, Double>();
    steeringForces = new HashMap<SteeringType, Vector3d>();
    steeringManagerInitialized();
    myActualVelocity = new Vector3d();
    myNextVelocity = new Vector3d();
    drawRaycasting = false;
    canEnlargeVelocity = true;
}

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 static List<Line> sliceTri(ObjRead mesh, double h, int majorAxis ) {

     int[] flatAxis = new int[] { ( majorAxis + 1 ) % 3, ( majorAxis + 2 ) % 3 };

     if (flatAxis[0] > flatAxis[1]) {
         int tmp = flatAxis[0];
         flatAxis[0] = flatAxis[1]; 
         flatAxis[1] = tmp;
     }

    Vector3d sliceNormal = new Vector3d(
            majorAxis == 0 ? 1: 0, 
            majorAxis == 1 ? 1: 0,
            majorAxis == 2 ? 1: 0),

            slicePt = new Vector3d( sliceNormal );

    slicePt.scale(h);
    LinearForm3D slicePlane = new LinearForm3D(sliceNormal, slicePt);

    return slice(mesh, flatAxis, slicePlane, Double.MAX_VALUE, null, Double.MAX_VALUE).stream().map(l -> new Line(l.start.x, l.start.z, l.end.x, l.end.z)).collect(Collectors.toList());
}

static Matrix4d combineTransformation(Matrix4d matrix, Vector3d translation, Vector3d rotation) {
    Matrix4d gM = new Matrix4d(matrix);
    Matrix4d m = new Matrix4d();

    m.setIdentity();
    m.setTranslation(translation);
    gM.mul(m);

    m.setIdentity();
    m.rotZ(rotation.z);
    gM.mul(m);

    m.setIdentity();
    m.rotY(rotation.y);
    gM.mul(m);

    m.setIdentity();
    m.rotX(rotation.x);
    gM.mul(m);

    return gM;
}

public static double getTorsionAngleRadians(double[] a, double[] b, double[] c, double[] d,
                                            Vector3d r1, Vector3d r2, Vector3d r3) {
  //   a --r1--> b --r2--> c --r3--> d

  // get r1 x r2 ==> r1 
  // and r2 x r3 ==> r3
  // then
  // sinTheta/cosTheta = r2.(r1 x r3)/(r1 . r3)

  sub(b, a, r1); 
  sub(c, b, r2);
  r2.normalize();
  r1.cross(r1, r2); //p1
  sub(d, c, r3); 
  r3.cross(r2, r3); //p2
  double p1dotp2 = r1.dot(r3);
  r1.cross(r3, r1);
  double theta = Math.atan2(
      -r2.dot(r1), // sin theta ~ r2.(p2 x p1) / |r2|
      p1dotp2);   // cos theta ~ p1.p2  
  return theta;
}

public static double restorativeForceAndDistance(Vector3d a, Vector3d b, Vector3d vab) {

  // a and b will be set to the force on the atom when r > r0

  vab.sub(a, b);
  double rab = vab.length();
  if (rab < 0.1) {// atoms are too close to each other
    randomizeUnitVector(vab);
    rab = 0.1;
  }
  vab.normalize();
  a.set(vab);
  a.scale(-1); // -drab/da
  b.set(vab); // -drab/db

  return rab;
}

/**
 * Rotate an {@link AxisAlignedBB} by the specified rotation matrix.
 *
 * @param axisAlignedBB  The AABB
 * @param rotationMatrix The rotation matrix
 * @param forcePositive  If true, set each coordinate of the rotated AABB to it absolute value
 * @return The rotated AABB
 */
public static AxisAlignedBB rotateAABB(AxisAlignedBB axisAlignedBB, Matrix3d rotationMatrix, boolean forcePositive) {
    // Extract the minimum and maximum coordinates of the AABB into vectors
    final Vector3d minCoords = new Vector3d(axisAlignedBB.minX, axisAlignedBB.minY, axisAlignedBB.minZ);
    final Vector3d maxCoords = new Vector3d(axisAlignedBB.maxX, axisAlignedBB.maxY, axisAlignedBB.maxZ);

    // Rotate the vectors in-place
    rotationMatrix.transform(minCoords);
    rotationMatrix.transform(maxCoords);

    if (forcePositive) {
        // Get the absolute value of the coordinates
        minCoords.absolute();
        maxCoords.absolute();
    }

    // Return an AABB with the new coordinates
    return new AxisAlignedBB(minCoords.getX(), minCoords.getY(), minCoords.getZ(), maxCoords.getX(), maxCoords.getY(), maxCoords.getZ());
}

public BlockImpl(final int x, final int y, final int z, final ChunkImpl chunk, final BlockMaterialData type)
{
    this.x = (byte) x;
    this.y = y;
    this.z = (byte) z;
    this.chunk = chunk;
    this.type = type;
    this.lazyBox = new LazyValue<>(() -> {
        final int x1 = this.x + (this.chunk.getX() << 4);
        final int x2 = (x1 >= 0) ? (x1 + 1) : (x1 - 1);

        final int y1 = this.z + (this.chunk.getZ() << 4);
        final int y2 = y1 + 1;

        final int z1 = this.z + (this.chunk.getZ() << 4);
        final int z2 = (z1 >= 0) ? (z1 + 1) : (z1 - 1);

        return BoundingBox.fromCorners(new Vector3d(x1, y1, z1), new Vector3d(x2, y2, z2));
    });
    this.state = getBlockState(this);
}

public BlockImpl(final int x, final int y, final int z, final ChunkImpl chunk)
{
    this.x = (byte) x;
    this.y = y;
    this.z = (byte) z;
    this.chunk = chunk;
    if (! chunk.isLoaded())
    {
        chunk.load();
    }
    this.type = chunk.getBlockType(x, y, z);
    this.lazyBox = new LazyValue<>(() -> {
        final int x1 = this.x + (this.chunk.getX() << 4);
        final int x2 = (x1 >= 0) ? (x1 + 1) : (x1 - 1);

        final int z1 = this.z + (this.chunk.getZ() << 4);
        final int z2 = (z1 >= 0) ? (z1 + 1) : (z1 - 1);

        return BoundingBox.fromCorners(new Vector3d(x1, this.y, z1), new Vector3d(x2, this.y + 1, z2));
    });
    this.state = getBlockState(this);
}

public static Group createCompass() {
  Appearance ap = Compass.generateAppearance(Color.pink, 0.5, true);
  Cone c = new Cone(1f, 5f, ap);

  Transform3D tf = new Transform3D();
  tf.rotX(Math.PI / 2);

  Transform3D tf1 = new Transform3D();
  tf1.setTranslation(new Vector3d(10, 15, -40));

  TransformGroup tg = new TransformGroup(tf);
  tg.addChild(c);

  TransformGroup tg2 = new TransformGroup(tf1);
  tg2.addChild(tg);

  return tg2;
}

/**
 * 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);
}

/**
 * Updates the given view platform transformation from yaw angle, pitch angle and scale. 
 */
private void updateViewPlatformTransform(TransformGroup viewPlatformTransform, float viewYaw, float viewPitch,
        float viewScale)
{
    // Default distance used to view a 2 unit wide scene
    double nominalDistanceToCenter = 1.4 / Math.tan(Math.PI / 8);
    // We don't use a TransformGroup in scene tree to be able to share the same scene 
    // in the different views displayed by OrientationPreviewComponent class 
    Transform3D translation = new Transform3D();
    translation.setTranslation(new Vector3d(0, 0, nominalDistanceToCenter));
    Transform3D pitchRotation = new Transform3D();
    pitchRotation.rotX(viewPitch);
    Transform3D yawRotation = new Transform3D();
    yawRotation.rotY(viewYaw);
    Transform3D scale = new Transform3D();
    scale.setScale(viewScale);

    pitchRotation.mul(translation);
    yawRotation.mul(pitchRotation);
    scale.mul(yawRotation);
    viewPlatformTransform.setTransform(scale);
}

public static Vector3d normal(Geometry g) throws GeometryException {
     switch ( g.geometryTypeId() ) {
        case TYPE_POINT:
        case TYPE_LINESTRING:
            return normal((LineString)g) ;
        case TYPE_POLYGON:
            return normal( (Polygon)g);
        case TYPE_TRIANGLE:
            return normal( (Triangle)g);
        case TYPE_MULTIPOINT:
        case TYPE_MULTILINESTRING:
        case TYPE_MULTIPOLYGON:
        case TYPE_GEOMETRYCOLLECTION:
        case TYPE_TRIANGULATEDSURFACE:
        case TYPE_POLYHEDRALSURFACE:
        }
       throw new GeometryException( "missing case in SFCGAL::algorithm::area3D" );
}

private boolean areAntiPodal(Vertex v, Edge e){
        double t1=0;
        double t2=1;
        for (Vertex w:convexHull.neighborVertices(v)){
        Vector3d v1=e.getNFaces().get(0).getNormal();
        Vector3d v2=e.getNFaces().get(1).getNormal();
        double p=v1.dot(GeometryUtils.vectorSubtract(w.pnt, v.pnt));
        double q=GeometryUtils.vectorSubtract(v1, v2).dot(GeometryUtils.vectorSubtract(w.pnt, v.pnt));
        if (q>0){
            t2=Double.min(t2,p/q);
        }else if (q<0){
            t1=Double.max(t1, p/q);
        } else if (p<0){
            return false;
        }

    }
        return t1<=t2;
}

public static Box toBox(PolyhedralSurface ps){
    Polyhedron p=new Polyhedron(ps);
    Vertex v=p.getVertices().get(0);
    Point3d pt=v.pnt;
    Vertex v1=v.edges.get(0).anotherVertex(v);
    Vertex v2=v.edges.get(1).anotherVertex(v);
    Vertex v3=v.edges.get(2).anotherVertex(v);
    Point3d pt1=v1.pnt;
    Point3d pt2=v2.pnt;
    Point3d pt3=v3.pnt;
    Vector3d vector1=GeometryUtils.vectorSubtract(pt1, pt);
    Vector3d vector2=GeometryUtils.vectorSubtract(pt2, pt);
    Vector3d vector3=GeometryUtils.vectorSubtract(pt3, pt);         
    Point3d max=GeometryUtils.pointAdd(pt,vector1);
    max=GeometryUtils.pointAdd(max,vector2);
    max=GeometryUtils.pointAdd(max,vector3);
    vector1.normalize();
    vector2.normalize();
    vector3.normalize();
    BoxOrientation bo=new BoxOrientation(vector1,vector2,vector3);
       return new Box(pt,max,bo);   
}

private Vector3d getBiggerVelocity(Vector3d velocity, double scale, boolean negate, RefBoolean wantsToGoFaster) {
    Vector3d result = new Vector3d(velocity.x, velocity.y, velocity.z);
    if (negate) {
        result.negate();
        wantsToGoFaster.setValue(false);
    }
    result.scale(scale);
    return result;
}

private Vector3d steerNextLink(NavPointNeighbourLink nextLink) {
    Location projection = nextLink.getFromNavPoint().getLocation();

    Location result = projection.sub(bot.getLocation()).getNormalized().scale(50).setZ(0);

    if (SteeringManager.DEBUG) {
        System.out.println("StickToPathSteer: TARGETING NEXT LINK");
        System.out.println("StickToPathSteer: result           = [" + result.x + ", " + result.y + ", " + result.z + "]");
        System.out.println("StickToPathSteer: length           = " + result.getLength());
    }

    return result.asVector3d();
}

public Vector3d getForceToPartner() {
    /*if (forceToPartner == null || forceToPartner.length() == 0) {
        return forceFromPartner;
    } else if (forceToPartner != null && forceFromPartner != null && forceFromPartner.length() > 0) {
        Vector3d sum = new Vector3d(forceFromPartner);
        sum.add(forceToPartner);
        return sum;
        //System.out.println("POZOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOR");
        //System.out.println(forceFromPartner.length());
        //System.out.println(forceToPartner.length());
    } else {
        return forceToPartner;
    }*/
    return forceFromPartner;
}

private Vector3d getAverageLeadersVelocity() {
    double x = 0;
    double y = 0;
    for(Vector3d velo : leadersVelocities) {
        x += velo.x;
        y += velo.y;
    }
    x = x/leadersVelocities.size();
    y = y/leadersVelocities.size();
    Vector3d averageVelo = new Vector3d(x,y,0);
    return averageVelo;
}

private void prepareRays() {
    //Five rays are created.
    LinkedList<SteeringRay> rayList = new LinkedList<SteeringRay>();
    rayList.add(new SteeringRay(LEFT, new Vector3d(0, -1, 0), shortRayLength));
    rayList.add(new SteeringRay(LEFTFRONT, new Vector3d(Math.sqrt(3) * 2, -1, 0), longRayLength));
    rayList.add(new SteeringRay(RIGHTFRONT, new Vector3d(Math.sqrt(3) * 2, 1, 0), longRayLength));
    rayList.add(new SteeringRay(RIGHT, new Vector3d(0, 1, 0), shortRayLength));
    rayList.add(new SteeringRay(FRONT, new Vector3d(1, 0, 0), longRayLength));
    //rayList.add(new SteeringRay(LEFTFRONT2, new Vector3d(Math.sqrt(3), -1, 0), longRayLength2));   //delete
    //rayList.add(new SteeringRay(RIGHTFRONT2, new Vector3d(Math.sqrt(3), 1, 0), longRayLength2));   //delete
    rayManager.addRays(SteeringType.OBSTACLE_AVOIDANCE, rayList, this);
    raysReady = false;
}

/**If it's really the front collision, the "mirror vector" is returned. Otherwise the unchanged parameter normal is returned.*/
private Vector3d computeTreeCollisionVector(Vector3d normal) {        
    Vector3d av = botself.getVelocity().getVector3d();
    /* Jestliže signalizuje pravý přední paprsek a ne levý přední -
     * a navíc jde normálová síla v místě kolize stejným směrem jako jde paprsek, tedy doleva ==> pak by se neměla přičítat tato normála, ale spíše síla na durhou stranu.
     * Značí to situaci, kdy jsme narazili na úzkou překážku (strom) a levý přední paprsek prošel levým krajem překážky.
     * Bez tohoto ošetření by nás to stočilo doleva, což nechceme.*/
    /* Pro pravou stranu je to naopak. *//* Jestliže signalizuje levý přední paprsek a ne pravý přední -
     * a navíc jde normálová síla v místě kolize stejným směrem jako jde paprsek, tedy doleva ==> pak by se neměla přičítat tato normála, ale spíše síla na durhou stranu.
     * Značí to situaci, kdy jsme narazili na úzkou překážku (strom) a levý přední paprsek prošel levým krajem překážky.
     * Bez tohoto ošetření by nás to stočilo doleva, což nechceme.*/

    Vector2d start = new Vector2d(botself.getLocation().x, botself.getLocation().y);
    Vector2d end = new Vector2d(start.x-av.x, start.y-av.y);
    Vector2d point = new Vector2d(start.x + normal.x, start.y + normal.y);
    Vector2d pata = SteeringTools.getNearestPoint(start, end, point, false);
    Vector2d pointToPata = new Vector2d(pata.x - point.x, pata.y - point.y);
    pointToPata.scale(2);
    Vector2d mirrorPoint = new Vector2d(point.x + pointToPata.x, point.y + pointToPata.y);

    Vector3d result = new Vector3d(mirrorPoint.x - start.x, mirrorPoint.y - start.y, 0);

    if (SteeringManager.DEBUG) {
        System.out.println("Obstacle avoidance tree collision. " + result.length());
    }
    return result;
}

/** When called, the bot starts steering, when possible, he get's nearer the target location. */
@Override
public Vector3d run(Vector3d scaledActualVelocity, RefBoolean wantsToGoFaster, RefBoolean wantsToStop, RefLocation focus)
{
    // Supposed velocity in the next tick of logic, after applying various steering forces to the bot.
    Vector3d nextVelocity = new Vector3d(0,0,0);

    for(Target_packet tp : targets) {

        /** ISteering properties: target location - bot approaches this location. */
        Location targetLocation = tp.getTargetLocation();

        // A vector from the bot to the target location.
        Vector3d vectorToTarget = new Vector3d(targetLocation.x - botself.getLocation().x, targetLocation.y - botself.getLocation().y, 0);

        double distFromTarget = vectorToTarget.length();

        /** ISteering properties: target gravity - a parameter meaning how attracted the bot is to his target location. */
        int attractiveForce = tp.getAttractiveForce(distFromTarget);

        if (distFromTarget < NEARLY_THERE_DISTANCE) {
            wantsToStop.setValue(true);
            //if (SteeringManager.DEBUG) System.out.println("We reached the target");
        } else {
            vectorToTarget.normalize();
            vectorToTarget.scale(attractiveForce);
            nextVelocity.add((Tuple3d) vectorToTarget);
        }
    }
    wantsToGoFaster.setValue(true);
    return nextVelocity;
}

public SteeringResult(Vector3d force, double accurancyMPL)
{
    x = force.x;
    y = force.y;
    z = force.z;
    this.accurancyMultiplier = accurancyMPL;
}

private void stopMovement(Location focusLocation) {

    myNextVelocity = new Vector3d(0, 0, 0);
    locomotion.setSpeed(0.0);//just for sure...
    locomotion.stopMovement();
    if (!focusLocation.equals(new Location(0,0,0))) {
        locomotion.turnTo(focusLocation);
    }
    if (SteeringManager.DEBUG) System.out.println(botself.getName()+" We turn to "+focusLocation);
    if(animationEngine.canBeInterupt(botself)){
        animationEngine.playAnimation("ambi_stand_normal01", botself, false);
    }
}

@Override
protected Vector3d setVelocitySpecific(ISteering steering, RefBoolean wantsToGoFaster, RefBoolean wantsToStop, RefLocation newFocus) {
    if (steering instanceof ISocialSteering) {
        return ((ISocialSteering) steering).runSocial(myNextVelocity, wantsToGoFaster, wantsToStop, newFocus);
    } else {
        return super.setVelocitySpecific(steering, wantsToGoFaster, wantsToStop, newFocus);
    }
}

/**
 * Clear the screen, set up correct observer position and other stuff
 * @param gl
 */
private synchronized void prepareCanvas(GL gl) {
    // Clear the drawing area
    gl.glEnable(GL.GL_DEPTH_TEST); // GL.GL_NORMALIZE|
    gl.glClearColor(0.45f, 0.45f, 0.45f, 0f);

    gl.glClear(GL.GL_COLOR_BUFFER_BIT | GL.GL_DEPTH_BUFFER_BIT);

    // Reset the modelview to the "identity"
    gl.glMatrixMode(GL.GL_MODELVIEW);
    gl.glLoadIdentity();

    // Move the "drawing cursor" around
    Location observerLoc = viewpoint.getLocation();
    Location eyeLoc = viewpoint.getEye();
    Vector3d upVec = viewpoint.getUp();
    //    gl.glTranslated(observerLocation.x, observerLocation.y, observerLocation.z);
    glu.gluLookAt(
            eyeLoc.x,
            eyeLoc.y,
            eyeLoc.z,
            observerLoc.x,
            observerLoc.y,
            observerLoc.z,
            upVec.x,
            upVec.y,
            upVec.z);

    gl.glScaled (1., 1., -1.);
}

/**
 * Rotate by angle around vector axis. Thumb of right hand in same direction as
 * axis vector and closing palm shows direction of rotation.
 * @param vector
 * @param axis
 * @param angle angle in degrees
 */
public static Location rotateVectorAroundAxis(Location vector, Location axis, double angle) {
    // move eye stuff back
    double x = axis.x;
    double y = axis.y;
    double z = axis.z;

    double radianAngle = angle * Math.PI / 180;
    double c = Math.cos(radianAngle);
    double s = Math.sin(radianAngle);

    /*
    ( xx(1-c)+c     xy(1-c)-zs  xz(1-c)+ys   0  )
    | yx(1-c)+zs    yy(1-c)+c   yz(1-c)-xs   0  |
    | xz(1-c)-ys    yz(1-c)+xs  zz(1-c)+c    0  |
    (    0          0           0            1  )
     */

    Matrix4d rotateMatrix = new Matrix4d(
            x * x * (1 - c) + c, x * y * (1 - c) - z * s, x * z * (1 - c) + y * s, 0,
            y * x * (1 - c) + z * s, y * y * (1 - c) + c, y * z * (1 - c) - x * s, 0,
            x * z * (1 - c) - y * s, y * z * (1 - c) + x * s, z * z * (1 - c) + c, 0,
            0, 0, 0, 1);

    Vector3d vec = new Vector3d(vector.x, vector.y, vector.z);

    rotateMatrix.transform(vec);

    return new Location(vec.x, vec.y, vec.z);
}

/**
 * This method creates a ray and puts it into worldview
 * @param id
 * @param direction
 * @param length
 * @param floorCorrection
 * @return 
 */
public Future<AutoTraceRay> createRay(String id, Vector3d direction, int length, boolean floorCorrection) {

        UnrealId unrealId = UnrealId.get(id);
        BSPRayInfoContainer rayInfo = new BSPRayInfoContainer(unrealId, direction, length, floorCorrection);
        rayInfoContainers.put(id, rayInfo);
        // that's all for now. the ray will be put in worldview in selfUpdate()
        selfUpdate(agent.getSelf());           
        return null;
}

public NavPointNeighbourLink parse(NavPoint from) {
     UnrealId id = nextId();
     nextPast(",");
     int flags = nextInt();
     nextPast(",");
     int collisionR = nextInt();
     nextPast(",");
     int collisionH = nextInt();
     nextPast(",");
     double translocZOffset = nextDouble();
     nextPast(",");
     String translocId = nextString();
     nextPast(",");
     boolean translocatorOnly = nextBoolean();
     nextPast(",");
     boolean forceDoubleJump = nextBoolean();
     nextPast(",");
     Vector3d neededJump = nextVector3d();
     nextPast(",");
     boolean neverImpact = nextBoolean();
     nextPast(",");
     boolean noLowGrav = nextBoolean();
     nextPast(",");
     double calcGravityZ = nextDouble();

     return new NavPointNeighbourLink(
         id, flags, collisionR, collisionH, translocZOffset, translocId, translocatorOnly, forceDoubleJump,
         neededJump, neverImpact, noLowGrav, calcGravityZ, from, navGraph.navPointsById.get(id)
    );
}

private void drawLine(BufferBuilder tess, Vector3d v1, Vector3d v2, LaserBaseTile tile) {
    tess.pos(v1.getX(), v1.getY(), v1.getZ()).endVertex();
    double x = ((int) v1.getX()) == -1 ? 0 : ((int) v1.getX()) == 2 ? 1 : v2.x;
    double z = (((int) v1.getZ()) == -1 ? 0 : ((int) v1.getZ()) == 2 ? 1 : v2.z);
    double time = (tile.getWorld().getWorldTime() % 80);


    if (x == 0) z = time < 40 ? time / 40f : 2 - time / 40f;
    else if (x == -1) x = time > 40 ? -2 + time / 40f : -time / 40f;
    else if (z == 0) x = time > 40 ? -1 + time / 40f : 1 - time / 40f;
    else if (z == 1) x = time < 40 ? time / 40f : 2 - time / 40f;
    tess.pos(x, -1 + tile.getCurrentWork() / (double) tile.getMaxWork(), z).endVertex();
}

public static Vector3d perp( Vector3d v, double scale ) {
    Vector3d out = new Vector3d( -v.z, 0, v.x );
    double l = out.length();
    if ( l < 0.001 )
        return new Vector3d();
    out.scale( scale / l );
    return out;
}

public Prof() {

    Matrix4d i = new Matrix4d();
    i.setIdentity();

    this.toFlat = this.to3d = i;
    this.dir = new Vector3d(1,0,0);
}

public Prof( Matrix4d toFlat, Vector3d dir3 ) {

    this.toFlat = toFlat;
    this.to3d = new Matrix4d();
    to3d.invert(toFlat);
    this.dir = dir3;
}

public Pano( String name, Vector3d location, float a1, float a2, float a3 ) {

        this.name = name;
        this.location = location;
        this.orig = new File( name + ".jpg" );

        buildPlanes( new File( name + ".txt" ) );

        set(    oa1 = a1 * FastMath.DEG_TO_RAD,
                oa2 = a2 * FastMath.DEG_TO_RAD, 
                oa3 = a3 * FastMath.DEG_TO_RAD );
    }

private static LinearForm3D cut(Point3d a, Point3d b, Point3d c) {

        Vector3d ab = new Vector3d(b);
        ab.sub(a);
        Vector3d bc = new Vector3d(c);
        bc.sub(b);

        ab.normalize();
        bc.normalize();

//      if ( true || ab.z > 0.0 || bc.z > 0.0) {
            ab.add( bc );
            ab.normalize();

            return new LinearForm3D( toXZ( ab ), toXZ( b ) );
//      }
//      Vector2d ab2 = new Vector2d( ab.x, ab.y ),
//              bc2 = new Vector2d( bc.x, bc.y );
//      
//      ab2.normalize();
//      bc2.normalize();
//      
//      ab2.add( bc2 );
//      
//      Vector3d normal = new Vector3d(ab2.x , ab2.y, 0);
//      normal.normalize();
//      
//      return new LinearForm3D( toXZ( normal ), toXZ( b ) );
    }

private static void cap( MeshBuilder out, Collection<LinearForm3D> after, 
        Vector3d along, List<Point3d> profilePts, boolean reverse ) {

    MultiMap<LinearForm3D, Point3d>faces = new MultiMap<>();
    for (Point3d p : profilePts) {
        List<LinearForm3D> hit = new ArrayList<>();
        Point3d c = clip (p, along, after, hit);
        for (LinearForm3D h : hit)
            faces.put( h, c );
    }

    for (Map.Entry<LinearForm3D, List<Point3d>> e : faces.map.entrySet()) 
        out.add ( new Loop<Point3d> ( e.getValue() ).singleton(), reverse );
}