Java 类javax.vecmath.Tuple3d 实例源码

/**
 *
 * This is a robust collision of a's adjacent edges with a horizontal plane at the given height
 * When Two parallel edges are given, we can assume that the
 */
public static Tuple3d collide (Corner a, double height)
{
    LinearForm3D ceiling = new LinearForm3D( 0, 0, 1, -height );

    // this can cause Jama not to return...
    if ( a.prevL.linearForm.hasNaN() || a.nextL.linearForm.hasNaN() )
                throw new Error();

        try {
            return ceiling.collide(a.prevL.linearForm, a.nextL.linearForm);
        } catch (RuntimeException e) {
            assert (a.prevL.sameDirectedLine(a.nextL));

            // a vector in the direction of uphill from a
            Vector3d dir = new Vector3d ( a.prevL.uphill );
            dir.normalize();
            // via similar triangle (pyramids)
            dir.scale(height - a.z);
            dir.add(a);

            //assume, they're all coincident?
            return new Point3d (dir.x, dir.y, height);
        }
}

/**
 * @see http://pl.wikipedia.org/wiki/Elementarne_macierze_transformacji
 * @param pTuple3d
 * @param pAngle
 * @return
 */
public static Vector3d rotateX3d(Tuple3d pTuple, double pAngle) {

    double x = pTuple.x;
    double y = pTuple.y;
    double z = pTuple.z;

    double cosA = Math.cos(pAngle);
    double sinA = Math.sin(pAngle);

    return new Vector3d(
            x,
            y * cosA - z * sinA,
            y * sinA + z * cosA);

}

/**
 * @see http://pl.wikipedia.org/wiki/Elementarne_macierze_transformacji
 * @param pTuple3d
 * @param pAngle
 * @return
 */
public static Vector3d rotateY3d(Tuple3d pTuple3d, double pAngle) {

    double x = pTuple3d.x;
    double y = pTuple3d.y;
    double z = pTuple3d.z;

    double cosB = Math.cos(pAngle);
    double sinB = Math.sin(pAngle);

    return new Vector3d(
          x * cosB + z * sinB,
          y,
          -x * sinB + z * cosB);

}

public Matrix4f  computeSurfaceTransform(
        double lat, double lon, double hEllps,
        double az, double ha, Matrix4f mat, Tuple3d origin) {
    if (mat == null) {
        mat = new Matrix4f();
    } else {
        mat.m01 = mat.m10 = mat.m02 =
                mat.m20 = mat.m12 = mat.m21 =
                mat.m30 = mat.m31 = mat.m32 = 0;
    }
    mat.m00 = mat.m11 = mat.m22 = mat.m33 = 1;
    synchronized (eye) {
        toCartesian(lat, lon, hEllps, eye);
        mat.m03 = (float)(eye.x - origin.x);
        mat.m13 = (float)(eye.y - origin.y);
        mat.m23 = (float)(eye.z - origin.z);
    }
    synchronized (workTrans) {
        workTrans.rotZ((float)(Math.PI/2.+lon)); mat.mul(workTrans);
        workTrans.rotX((float)(Math.PI/2.-lat)); mat.mul(workTrans);
        workTrans.rotZ((float)(-az));            mat.mul(workTrans);
        workTrans.rotX((float)(Math.PI/2.+ha));  mat.mul(workTrans);
    }
    return mat;
}

/** 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 FacialPoint(FacialPointType type, Tuple3d coords) {        
    this.pos = new Vector3f(coords);

    this.type = type;
    this.name = FpTexter.getInstance().getFPname(type);
    this.info = FpTexter.getInstance().getFPinfo(type);
}

public int compare( Chain o1, Chain o2 )
        {
            Corner c1 = o1.chain.get( 0 );
            Corner c2 = o2.chain.get( 0 );

            // except for the first and and last point
            // chain's non-start/end points are always at the position of the collision - so to
            // find the angle of the first edge at the specified height, we project it's start
            // coordinate the desired height and take the angle relative to the collision
            // !could speed up with a chain-class that caches this info!

//            try
//            {
                Tuple3d p1 = Edge.collide(c1, height);//ceiling.collide( c1.prevL.linearForm, c1.nextL.linearForm );
                Tuple3d p2 = Edge.collide(c2, height );//ceiling.collide( c2.prevL.linearForm, c2.nextL.linearForm );

                p1.sub( loc );
                p2.sub( loc );

                // start/end line is (+-)Pi
                return Double.compare(
                        Math.atan2( p1.y, p1.x ),
                        Math.atan2( p2.y, p2.x ) );
//            }
//            catch (RuntimeException e)
//            {
//                // we can probably fix these up (by assuming that they're horizontal?)
//                // todo: can we prove they are safe to ignore? eg: no parallel edges inbound, none outbound etc..
//                 System.err.println( "didn't like colliding 1" + c1.prevL + " and " + c1.nextL );
//                 System.err.println( "                      2" + c2.prevL + " and " + c2.nextL );
//                 return 0;
//            }
        }

public void addOutputSideTo( boolean isTop, Tuple3d a, Tuple3d b, Edge... edges ) // a.y == b.yequals(b)
    {
        for ( Edge edge : edges )
        {
            Corner c = edge.start; // assumption: start of edge will always be a leading corner on the face!
            Face f = faces.get( c );
            assert (f != null);
            if (isTop)
                f.topSE.add( createEdge( a, b ) );
            // just check those tuple's aren't corners....
            f.results.add( new Point3d( a ), new Point3d( b ) );
//            System.out.println(">>");
        }
    }

private SharedEdge createEdge ( Tuple3d start, Tuple3d end)
{
    SharedEdge newEdge = new SharedEdge (new Point3d (start), new Point3d (end ));
    newEdge = edges.get( newEdge ); // identity lookup - only one edge!

    return newEdge;
}

public void addCube (Tuple3d corner, Tuple3d up, Tuple3d along, double upL, double alongL, double inL ) {
    Vector3d in = new Vector3d();
    in.cross( new Vector3d(up), new Vector3d(along) );
    addCube (
            Jme3z.to( corner ), Jme3z.to( up ).normalizeLocal(), 
            Jme3z.to( along ).normalizeLocal(), Jme3z.to( in ).normalizeLocal(), 
            (float) upL, (float) alongL, (float) inL );
}

private String printTuple3d( Tuple3d t )
{
    StringBuilder buf = new StringBuilder( "<" );
    buf .append( FORMAT.format(t.x) );
    buf .append( "," );
    buf .append( FORMAT.format(t.y) );
    buf .append( "," );
    buf .append( FORMAT.format(t.z) );
    buf .append( ">" );
    return buf .toString();
}

public static void checkReal(Tuple3d p) {
    if (Double.isNaN(p.x) || Double.isNaN(p.y) || Double.isNaN(p.z)) {
        throw new IllegalArgumentException("tuple has NaN values: " + p);
    }
    if (Double.isInfinite(p.x) || Double.isInfinite(p.y) || Double.isInfinite(p.z)) {
        throw new IllegalArgumentException("tuple has Infinite values: " + p);
    }
}

/**
 * Sets the camera location to <code>newLocation</code> without affecting the rotation.
 */
public void setLocation( Tuple3d newLocation , boolean applyNow )
{
    m_pendingPosition.setLocation( newLocation );
    if( applyNow )
    {
        exportVPT( );
    }
}

static void checkValid( Tuple3d t )
{
    if( Double.isNaN( t.x ) || Double.isNaN( t.y ) || Double.isNaN( t.z ) )
    {
        throw new IllegalArgumentException( "tuple has NaN values" );
    }
    if( Double.isInfinite( t.x ) || Double.isInfinite( t.y ) || Double.isInfinite( t.z ) )
    {
        throw new IllegalArgumentException( "tuple has Infinite values" );
    }
}

/**
 * Sets the camera location to <code>newLocation</code> without affecting the rotation.
 */
public void setLocation( Tuple3d newLocation )
{
    checkValid( newLocation );
    location.set( newLocation );
    xform.setTranslation( location );
    invXformUpToDate = false;
    locationUpToDate = true;
}

protected static String getJmolPoint(Tuple3d point) {
    StringBuilder s = new StringBuilder();
    s.append("{");
    s.append(fDot2(point.x));
    s.append(",");
    s.append(fDot2(point.y));
    s.append(",");
    s.append(fDot2(point.z));
    s.append("}");
    return s.toString();
}

public final void toCartesian(double lat, double lon, double h, Tuple3d p) {
    double n = getN(lat);
    double r = (n+h) * Math.cos(lat);

    p.x = r * Math.cos(lon);
    p.y = r * Math.sin(lon);
    p.z = (n * (1.-getE2()) + h) * Math.sin(lat);
}

/**
 * The main method. This method must be called in each tick (logic), if we want the navigation layer to compute the next velocity and send it to the locomotion layer.
 * Note: Should not be called anymore. Use start() and stop() methods.
 */
public void run() {
    steeringForces.clear();

    Vector3d velocity = botself.getVelocity().getVector3d();

    if (SteeringManager.DEBUG) System.out.println("Velocity "+velocity+" length "+velocity.length());

    // Supposed velocity in the next tick of logic, after applying various steering forces to the bot.
    Vector3d nextVelocity = new Vector3d(velocity.x, velocity.y, velocity.z);

    double actualWeight;

    if (useLastVeloWeight) {
        actualWeight = lastVeloWeight;
    } else {
        actualWeight = 3 - velocity.length()/WALK_VELOCITY_LENGTH;  //This causes that <= WALK_VEOCITY_LENGTH will have actualWeight 2, sth. >= 2*WALK_VELOCITY_LENGTH 1, and other values wil be between 1 and 2.
        if (actualWeight <1)
            actualWeight = 1;
        else if (actualWeight > 2)
            actualWeight = 2;
        if (velocity.length() == 0)
            actualWeight = 0;
    }

    //The actual velocity has bigger weigh ==> the behavior will be smoother.   //5389.0,-6203.0,-3446.65
    nextVelocity.scale(actualWeight);

    myActualVelocity = new Vector3d(nextVelocity.x, nextVelocity.y, nextVelocity.z);
    Vector3d myStopVelocity = new Vector3d(nextVelocity.x, nextVelocity.y, nextVelocity.z);

    double totalWeight = actualWeight;

    boolean everyoneWantsToGoFaster = canEnlargeVelocity;
    RefBoolean wantsToGoFaster = new RefBoolean(false);
    RefBoolean wantsToStop = new RefBoolean(false);
    Location focusLoc = new Location(0,0,0);

    for(SteeringType stType : mySteerings.keySet()) {
        ISteering steering = mySteerings.get(stType);
        RefLocation newFocus = new RefLocation(); 
        newFocus.data = new Location(0, 0, 0);
        Vector3d newVelocity = setVelocitySpecific(steering, wantsToGoFaster, wantsToStop, newFocus);
        focusLoc = setFocusSpecific(stType,wantsToStop.getValue(),newFocus.data,focusLoc);
        if (wantsToStop.getValue()) {   //Wants to stop causes, tak bot stops, if this steering is the only one. Otherwise the other steerings can cause that bot will again move.
            newVelocity.x = -myStopVelocity.x;
            newVelocity.y = -myStopVelocity.y;
            newVelocity.z = -myStopVelocity.z;
            myStopVelocity.sub(newVelocity);
            everyoneWantsToGoFaster = false;
            if (SteeringManager.DEBUG) System.out.println("We stop.");
            wantsToStop.setValue(false);
        } else {
            if (newVelocity.length() > MAX_FORCE) newVelocity.scale(MAX_FORCE/newVelocity.length());
            newVelocity.scale(steeringWeights.get(stType)); //Each steering has its own weight.
            everyoneWantsToGoFaster = everyoneWantsToGoFaster && wantsToGoFaster.getValue();
        }
        if (newVelocity.length()>0) {
            //TODO: WARNING hack to use different type of steering return values
            //it should be redone, more cleaner and robust way... Petr B.
            newVelocity.add((Tuple3d)nextVelocity);
            nextVelocity = newVelocity;
            if (newVelocity.length() > MIN_VALUE_TO_SUM)    //Only significant steerings are counted into totalWeight.
                totalWeight += steeringWeights.get(stType);
        }
        if (SteeringManager.DEBUG) System.out.println(steering.toString()+"| length "+newVelocity.length()+" | weight: "+steeringWeights.get(stType));
        steeringForces.put(stType, newVelocity);
    }
    if (SteeringManager.DEBUG) System.out.print("Sum "+nextVelocity.length()+" TotalWeight: "+totalWeight);
    if (totalWeight > 0) {
        nextVelocity.scale(1/totalWeight);
    }
    if (SteeringManager.DEBUG) System.out.println(" Result "+nextVelocity.length());

    moveTheBot(nextVelocity, everyoneWantsToGoFaster, focusLoc);
}

@Override
public Vector3d run(Vector3d scaledActualVelocity, RefBoolean wantsToGoFaster, RefBoolean wantsToStop, RefLocation focus) {
    // <editor-fold defaultstate="collapsed" desc="debug">
    if (properties == null) {
        if (SOC_STEER_LOG.DEBUG) {
            SOC_STEER_LOG.AddLogLineWithDate("no properties", "triangleError");

        }
    }// </editor-fold>

    Location newFocus = getFocus();
    if (newFocus != null) {
        focus.data = newFocus;
    }


    //returns ideal place where steered agent wants to stay...
    Location targetLocation = WhereToGo(botself, properties);

    // Supposed velocity in the next tick of logic, after applying various steering forces to the bot.
    SteeringResult nextVelocity = new SteeringResult(new Vector3d(0, 0, 0), 1);

    //we are able to compute ideal place...
    if (targetLocation != null) {


        // A vector from the bot to the target location.
        targetLocation = new Location(targetLocation.x,targetLocation.y, botself.getLocation().z);
        Vector3d vectorToTarget = targetLocation.sub(botself.getLocation()).asVector3d();

        double distFromTarget = vectorToTarget.length();
        nextVelocity.setMult(distFromTarget / 100);
        if (distFromTarget < KMinimalDistance) {
            wantsToStop.setValue(true);
            return new SteeringResult(new Vector3d(0, 0, 0), 1);
        }

        double attractiveForce = KDefaultAttraction;//* (distFromTarget / KDefaultAttractionDistance);

        vectorToTarget.normalize();
        vectorToTarget.scale(attractiveForce);
        nextVelocity.add((Tuple3d) vectorToTarget);
    }else
    {
        nextVelocity.setMult(1);
    }

    //no need to scale, scaling is done within method attraction(...)
    int botAttractiveForce = KDefaultAttraction / 6;

    Vector3d attractionFromFst = attraction(botself, properties.getFstBot(), 1.3);
    Vector3d attractionFromSnd = attraction(botself, properties.getSndBot(), 1.3);

    attractionFromFst.scale(botAttractiveForce);
    nextVelocity.add((Tuple3d) attractionFromFst);

    attractionFromSnd.scale(botAttractiveForce);
    nextVelocity.add((Tuple3d) attractionFromSnd);



    wantsToGoFaster.setValue(false);
    return nextVelocity;
}

public Box3D(Tuple3d p1, Tuple3d p2) {
    this(p1.getX(), p2.getX(), p1.getY(), p2.getY(), p1.getZ(), p2.getZ());
}

/**
 * Instances of Point3D are directly added, other Point are converted to
 * Point3D with the same location.
 */
public PointSet3D(Tuple3d[] points) {
    for (Tuple3d element : points)
        this.points.add(new Point3D(element));
}

public Point3D(Tuple3d point) {
    this(point.getX(), point.getY(), point.getZ());
}

public DebugGen( List<Tuple3d> todebug2, Tweed tweed ) {
    super( "foo", tweed );
    this.cubes = todebug2;
}

public static Point2d to2( Tuple3d loc ) {
    return new Point2d(loc.x, loc.z);
}

public static void convertToMini( Iterable<File> bigObj, File outfile, Matrix4d transform ) {

        outfile.mkdirs();

        ObjDump src = new ObjDump( bigObj );//.iterator().next() );

        src.centerVerts();
        src.transform (transform);

        long count = src.material2Face.entrySet().stream().mapToInt( x -> x.getValue().size() ).sum();
        double[] bounds = src.orderVert.stream().collect( new InaxPoint3dCollector() );
        long targetCount = 5000;

        double volume = ( bounds[ 1 ] - bounds[ 0 ] ) * ( bounds[ 3 ] - bounds[ 2 ] ) * ( bounds[ 5 ] - bounds[ 4 ] );

        double edgeLength = Math.pow( volume / ( count / targetCount ), 0.3333 );

        int 
                xc = (int) Math.ceil( ( bounds[ 1 ] - bounds[ 0 ] ) / edgeLength ), 
                yc = (int) Math.ceil( ( bounds[ 3 ] - bounds[ 2 ] ) / edgeLength ), 
                zc = (int) Math.ceil( ( bounds[ 5 ] - bounds[ 4 ] ) / edgeLength );

        Set<Face>[][][] faces = new Set[xc][yc][zc];

        for ( Entry<Material, List<Face>> e : src.material2Face.entrySet() )
            for ( Face f : e.getValue() ) {

                Tuple3d vt = src.orderVert.get( f.vtIndexes.get( 0 ) );

                int ix = (int) ( ( vt.x - bounds[ 0 ] ) / edgeLength );
                int iy = (int) ( ( vt.y - bounds[ 2 ] ) / edgeLength );
                int iz = (int) ( ( vt.z - bounds[ 4 ] ) / edgeLength );

                if ( faces[ ix ][ iy ][ iz ] == null )
                    faces[ ix ][ iy ][ iz ] = new HashSet();

                faces[ ix ][ iy ][ iz ].add( f );
            }

        int dir = 0;
        MiniTransform mt = new MiniTransform();

        for ( int x = 0; x < xc; x++ )
            for ( int y = 0; y < yc; y++ )
                for ( int z = 0; z < zc; z++ ) {

                    Set<Face> miniF = faces[ x ][ y ][ z ];
                    if ( miniF == null )
                        continue;

                    Matrix4d trans = new Matrix4d();
                    trans.setIdentity();
                    trans.setScale( edgeLength / 255 );
                    trans.setTranslation( new Vector3d(
                            x * edgeLength + bounds[0],
                            y * edgeLength + bounds[2],
                            z * edgeLength + bounds[4]
                            ) );

                    Matrix4d pack = new Matrix4d( trans );
                    pack.invert();

                    ObjDump mini = new ObjDump();
                    miniF.stream().forEach( f -> mini.addFaceFrom( f, src ) );
                    mini.transform( pack );

                    mini.dump( new File( new File( outfile, "" + dir ), OBJ ) );
                    mt.index.put( dir, trans );

                    dir++;
                }

        try {
            new XStream().toXML( mt, new FileWriter( new File( outfile, INDEX ) ) );
        } catch ( IOException e1 ) {
            e1.printStackTrace();
        }

        System.out.println( "wrote " + count + " faces to " + dir + " meshes" );

    }

public Triangle(Tuple3d a, Tuple3d b, Tuple3d c) {
    this.a = new Point3f(a);
    this.b = new Point3f(b);
    this.c = new Point3f(c);
}

public void setA(Tuple3d a) {
    this.a = new Point3f(a);
}

public void setB(Tuple3d b) {
    this.b = new Point3f(b);
}

public void setC(Tuple3d c) {
    this.c = new Point3f(c);
}

public static Vector3d vectorSubtract(Tuple3d t0, Tuple3d t1){
    return new Vector3d(t0.x - t1.x, t0.y- t1.y, t0.z - t1.z);
}

public static Point3d pointAdd(Tuple3d t0, Tuple3d t1){
    return new Point3d(t0.x + t1.x, t0.y+ t1.y, t0.z + t1.z);
}

public void addOutputSideTo( Tuple3d a, Tuple3d b, Edge... edges )
{
    addOutputSideTo( false, a, b, edges );
}

public Corner( Tuple3d in ) {
    super( in );
}

private void cornerEdgeCollision( Corner corner, Edge edge )
    {
            // check for the uphill vector of both edges being too similar (parallel edges)
            // also rejects e == corner.nextL or corner.prevL
            // updated to take into account vertical edges - will always have same uphill! - (so we check edge direction too)
            if (
                    (edge.uphill.angle( corner.prevL.uphill ) < 0.0001 && edge.direction().angle(  corner.prevL.uphill) < 0.0001 ) ||
                    (edge.uphill.angle( corner.nextL.uphill ) < 0.0001 && edge.direction().angle(  corner.nextL.uphill) < 0.0001 ))
                return;


            Tuple3d res = null;
            try
            {
                // sometimes locks up here if edge.linear form has NaN components.
                if ( corner.prevL.linearForm.hasNaN() || corner.nextL.linearForm.hasNaN() || edge.linearForm.hasNaN())
                    throw new Error();
                res =edge.linearForm.collide( corner.prevL.linearForm, corner.nextL.linearForm );
            }
            catch ( Throwable f )
            {
                //trying to collide parallel-ish faces, don't bother
//                System.err.println( "didn't like colliding " + edge + " and " + corner.prevL + " and " + corner.nextL );
                return;
            }

            if ( res != null )
            {
                // cheap reject: if collision is equal or below (not the correct place to check) the corner, don't bother with it
                if ( res.z < corner.z || res.z < edge.start.z )
                    return;

                EdgeCollision ec =
                        new EdgeCollision( new Point3d(res),
                        corner.prevL,
                        corner.nextL,
                        edge);


                if (! skel.seen.contains( ec ))
                    faceEvents.offer( ec );
            }
    }

public static com.jme3.math.Vector3f toJmeVec( Tuple3d a ) {
    return new com.jme3.math.Vector3f( (float) a.x, (float) a.y, (float) a.z );
}

public static Vector3f to( Tuple3d l ) {
    return new Vector3f( (float) l.x, (float) l.y, (float) l.z );
}

public static Point3d convert (Tuple3d in)
{
    Point3d out = new Point3d(in);
    transform.transform( out );
    return out;
}

public static Vector3f toF (Tuple3d in)
{
    return new Vector3f ((float)in.x,(float)in.y,(float)in.z);
}

public final static Vector3d sub(Tuple3d v, Tuple3d v1) {
    return new Vector3d(v.x - v1.x, v.y - v1.y, v.z - v1.z);
}

static double dot(Tuple3d v0, Tuple3d v1) {
    return (v0.x*v1.x + v0.y*v1.y + v0.z*v1.z);
}