Java Vector3d.normalize方法代码示例

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
/**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;
}
 

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
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;
}
 

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
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;
}
 

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
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);	
}
 

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
/** 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;
}
 

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
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 ) );
	}
 

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
private static void randomizeUnitVector(Vector3d v) {
  Random ptr = new Random();

  // obtain a random vector with 0.001 <= length^2 <= 1.0, normalize
  // the vector to obtain a random vector of length 1.0.
  double l;
  do {
    v.set(ptr.nextFloat() - 0.5, ptr.nextFloat() - 0.5, ptr.nextFloat() - 0.5);
    l = v.lengthSquared();
  } while ((l > 1.0) || (l < 1e-4));
  v.normalize();
}
 

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
private static Polygon outline(Polyhedron polyhedron,Vector3d random){	   
  random.normalize();
  double dot=Double.NEGATIVE_INFINITY;
  Vertex start=null;
  for(Vertex vertex:polyhedron.vertices){
   double e=random.dot(new Vector3d(vertex.pnt.x,vertex.pnt.y,vertex.pnt.z));
   if (e>dot) {
	   dot=e;
   start=vertex;
   }
  }
  Vertex current=start;
  Vertex next=new Vertex(null, 0);
  LineString exterior=new LineString();
  exterior.addPoint(start.pnt);
  while (next!=start){
   dot=Double.NEGATIVE_INFINITY;		   
 for(Edge edge:current.edges){
  Vertex another=edge.anotherVertex(current);
  Vector3d v=GeometryUtils.vectorSubtract(another.pnt, current.pnt);
  v.normalize();
  double d=v.dot(random);
  if(d>dot){
	  dot=d;
	  next=another;
  }
 }
 exterior.addPoint(next.pnt);
 current=next;
}
  Polygon polygon=new Polygon(exterior);
  return polygon;	   
 }
 

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
private Vector3d antiPodalDirection(Edge e1,Edge e2){
	Vector3d e1v1=e1.getNFaces().get(0).getNormal();
	Vector3d e1v2=e1.getNFaces().get(1).getNormal();
	Vector3d e2v1=e2.getNFaces().get(0).getNormal();
	Vector3d e2v2=e2.getNFaces().get(1).getNormal();
	Matrix3d m=new Matrix3d();
	m.setColumn(0, GeometryUtils.vectorSubtract(e1v1, e1v2));
	m.setColumn(1, e2v1);
	m.setColumn(2, GeometryUtils.vectorSubtract(e2v2, e2v1));
	try{
	m.invert();
	}catch (Exception e){
	return null;
	}
	double t=m.m00*e1v1.x+m.m01*e1v1.y+m.m02*e1v1.z;
	double c=m.m10*e1v1.x+m.m11*e1v1.y+m.m12*e1v1.z;
	double cu=m.m20*e1v1.x+m.m21*e1v1.y+m.m22*e1v1.z;
	double u=cu/c;
	if (c<0&&t>=0-EPS&&t<=1+EPS&&u>=0-EPS&&u<=1+EPS){
		Vector3d n=new Vector3d();
		Vector3d vv=GeometryUtils.vectorSubtract(e1v2, e1v1);
		vv.scale(t);
		n.add(e1v1,vv);
		n.normalize();
		return n;
	}
	return null;
}
 

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
public Vector3d getNormal(){
	if(this.normal!=null){
		return normal;
	}
	Vector3d v1=GeometryUtils.vectorSubtract(p1, p0);
	Vector3d v2=GeometryUtils.vectorSubtract(p2, p1);
	Vector3d u=new Vector3d();
	u.cross(v1, v2);
	u.normalize();
	this.normal=u;
	return normal;
}
 

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
private Vector3d goRoundPartner(Player player) {        
    Vector3d result = new Vector3d(0,0,0);
    Location myActualLocation = botself.getLocation();
    Vector3d myVelocity = botself.getVelocity().getVector3d();
    Location hisActualLocation = player.getLocation();
    Vector3d hisVelocity = player.getVelocity().getVector3d();
    Location myNextLocation = null;
    Location hisNextLocation = null;
    double collisionTime = -1;
    for(int t=0;t <= projection*TICK_PARTS;t++){
        double time = ((double)t)/TICK_PARTS;
        myNextLocation = getLocationAfterTime(myActualLocation, myVelocity, time);
        hisNextLocation = getLocationAfterTime(hisActualLocation, hisVelocity, time);
        if (myNextLocation.getDistance(hisNextLocation) <= distanceFromOtherPeople) {
            collisionTime = time;
            break;
        }
    }
    if (collisionTime != -1) {  //Za dobu collisionTime bychom se přiblížili příliš blízko.
        double ourNextDistance = myNextLocation.getDistance(hisNextLocation);
        Vector3d myNextLocationToHis = new Vector3d(hisNextLocation.x - myNextLocation.x, hisNextLocation.y - myNextLocation.y, hisNextLocation.z - myNextLocation.z);
        double ourNextAngle = myNextLocationToHis.angle(myVelocity);

        Vector3d turningVector;
        double koefA, koefB;
        boolean turnLeft;
        
        /*Teď podle toho, zda bude v danou chvíli druhý bot od nás napravo či nalevo, zatočíme na danou stranu.
         A podle toho, jak dalekood sebe budeme a za jak dlouho to je, bude síla velká.*/
        if (ourNextAngle == 0) {
            turnLeft = random.nextBoolean();
            if (SteeringManager.DEBUG) {
                System.out.println("Partner exactly front collision. "+turnLeft);
            }
            koefA = 1;
            koefB = getKoefB(collisionTime);
        } else {
            koefA = getKoefA(ourNextAngle, ourNextDistance);
            koefB = getKoefB(collisionTime);
            turnLeft = !SteeringTools.pointIsLeftFromTheVector(myVelocity, myNextLocationToHis);
            if (SteeringManager.DEBUG) System.out.println("Partner nearly front collision. " + turnLeft);
            if (SteeringManager.DEBUG) System.out.println("Distance " + ourNextDistance + " koefA " + koefA + " koefB " + koefB);
        }
        turningVector = SteeringTools.getTurningVector2(botself.getVelocity().getVector3d(), turnLeft);
        turningVector.normalize();
        turningVector.scale(2*repulsiveForce * koefA * koefB);
        if (SteeringManager.DEBUG) System.out.println("Turning vector " + turningVector.length());
        result.add(turningVector);
    }        
    return result;
}
 

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
@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;
}
 

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
public static Prof buildProfile( ObjRead mesh, Line3d oLine, Point3d cen, 
			double minH, double maxH,
			double minD, double maxD,
			Tweed tweed, Node dbg ) {

		Prof monotonic = buildProfile( oLine, cen );
		
		Vector3d dir = oLine.dir();
		dir.normalize();
		Vector3d sliceNormal = new Vector3d( dir.x, 0, dir.z );
		
		LinearForm3D lf = new LinearForm3D( sliceNormal, cen );
		
		List<Line3d> lines = ObjSlice.sliceTri( mesh, lf, 0.5,
				new Vector3d( -dir.z, 0, dir.x ), Math.PI / 2 +  0.1 );
		
//		dbg.attachChild( Jme3z.lines( tweed, lines, ColorRGBA.Blue, 2 ) );
		
		Line3d first = null;
		double closestStart = Double.MAX_VALUE;

		for ( Line3d l : lines ) {
			
			if ( l.start.y > l.end.y )
				l.reverse();
			
			double dist = l.distanceSquared( cen );
			
			if ( dist < closestStart ) {
				closestStart = dist;
				first = l;
			}
		}

		if ( first == null ) {
			return null;
//			lines.clear();
//			monotonic.add( cen );
//			monotonic.add( new Point3d( cen.x, cen.y - 500, cen.z ) );
		} else {
			climb( lines, first, monotonic, maxH, true );
			climb( lines, first, monotonic, minH, false );
		}

		{
			double tol = 0.2;

			minD -= tol;
			maxD += tol;
			
			LinearForm min = new LinearForm( Mathz.UP ).findC( new Point2d(minD,0) );
			LinearForm max = new LinearForm( Mathz.UP ).findC( new Point2d(maxD,0) );
			
			for (int i = 0; i < monotonic.size()-1; i ++) {
				Point2d a = monotonic.get(i), b = monotonic.get(i+1);
				
				if (a.x < minD && b.x < minD) {
					monotonic.remove(i);
					i--;
				} else if (a.x < minD) {
					monotonic.set(i, new LinearForm ( new Line(a,b) ).intersect( min ) );
				} else if (b.x < minD) {
					monotonic.set(i+1, new LinearForm ( new Line(a,b) ).intersect( min ) );
					b.x = minD + Math.ulp(minD);
				}
				
				if (a.x > maxD && b.x > maxD) {
					monotonic.remove(i);
					i--;
				} else if (a.x > maxD) {
					monotonic.set(i, new LinearForm ( new Line(a,b) ).intersect( max ) );
				} else if (b.x > maxD) {
					monotonic.set(i+1, new LinearForm ( new Line(a,b) ).intersect( max ) );
					b.x = maxD - Math.ulp(maxD);
				}
			}
		}
		
		return monotonic; 
	}
 

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
public static void tube (MeshBuilder out, 
			Collection<LinearForm3D> before, Collection<LinearForm3D> after, 
			Line3d line, LinearForm3D left, LinearForm3D right, CrossGen gen ) {
		
		if (angle ( before, line) < 0.1 || angle ( after, line ) < 0.1 )
			return; // too pointy to touch
		
		Point3d middle = line.fromPPram( 0.5 );
		
		Vector3d along = line.dir();
		along.normalize();
		Vector3d nAlong = new Vector3d (along);
		nAlong.negate();
		
		Vector3d o1 = left.normal(), u1 = new Vector3d();
		u1.cross( along, o1 );
		
		Frame frame = Mathz.buildFrame ( o1, u1, along, middle);
		
		Vector3d u2 = right.normal();
		u2.cross( u2, along );
//		u2.add( middle );
		
		Vector2d leftDir = Mathz.toXY ( frame, u1 );
		Vector2d rightDir = Mathz.toXY ( frame, u2 );
		
		List<Point3d> profilePts = gen.gen( leftDir, rightDir ).stream().
				map( p -> Mathz.fromXY( frame, p ) ).collect( Collectors.toList() );
		
		List<LinearForm3D> dummy = new ArrayList<>();
		
		for (Pair <Point3d, Point3d> pair : new ConsecutivePairs<Point3d>( profilePts, true ) ) {
			
			Point3d 
					f1 = clip ( pair.first (), along , after , dummy ),
					f2 = clip ( pair.second(), along , after , dummy ),
					b1 = clip ( pair.first (), nAlong, before, dummy ),
					b2 = clip ( pair.second(), nAlong, before, dummy );

			out.add (f2, f1, b1, b2);
		}
		
//		cap( out, after ,  along, profilePts, true  );
//		cap( out, before, nAlong, profilePts, false );
	}
 

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
protected void moulding( Matrix4d to3d, DRectangle rect, MeshBuilder mb ) {
	
	double hh = rect.height/2;
	
	Point3d start = new Point3d (rect.x, 0, rect.y+hh), end = new Point3d (rect.getMaxX(), 0, rect.y+hh);
	
	to3d.transform( start );
	to3d.transform( end   );
	
	Line3d line= new Line3d(start, end);
	
	Vector3d dir = line.dir();
	dir.normalize();
	Vector3d nDir = new Vector3d( dir );
	nDir.scale( -1 );
	
	LinearForm3D left = new LinearForm3D( nDir, start ), right = new LinearForm3D( dir, end);
	
	LinearForm3D wall = new LinearForm3D( to3d.m01,to3d.m11,to3d.m21 );
	wall.findD(start);
	
	Tube.tube( mb, Collections.singleton( left ), Collections.singleton( right ), 
			line, wall, wall, new CrossGen() {
				
				@Override
				public List<Point2d> gen( Vector2d down, Vector2d up ) {
					
					Vector2d d = new Vector2d(down);
					d.normalize();
					
					Vector2d dP = new Vector2d(d.y, -d.x );
					
					List<Point2d> out = new ArrayList();
					
					for (double[] coords : new double[][] {
						{1.00, 0.00},
						{1.00, 0.05},
						{0.66, 0.05},
						{0.66, 0.10},
						{0.33, 0.10},
						{0.33, 0.17},
						{0.00, 0.17},
						{0.00, 0.00},
						} ) {
							Point2d tmp = new Point2d(d);
							tmp.scale (coords[0] * rect.height - hh);
							Point2d tmp2 = new Point2d( dP );
							tmp2.scale (coords[1]);
							tmp.add(tmp2);
						
							out.add(tmp);
					}
					
					return out;
				}
			} );
	
}