Java Vector3d.add方法代码示例

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包/类
/** 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包/类
public static double pointPlaneAngleRadians(Vector3d a, Vector3d b,
                                            Vector3d c, Vector3d d,
                                            Vector3d v1,Vector3d v2, 
                                            Vector3d norm) {
  v1.sub(b, c);
  v2.sub(b, d);
  norm.cross(v1, v2);
  v2.add(v1);
  v1.sub(b, a);
  double angleA_CD = vectorAngleRadians(v2, v1);
  double angleNorm = vectorAngleRadians(norm, v1);
  if (angleNorm > Math.PI / 2)
    angleNorm = Math.PI - angleNorm;
  return Math.PI / 2.0 + (angleA_CD > Math.PI / 2.0 ? -angleNorm : angleNorm) ;
}
 

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
public static Sphere boundingSphere(Geometry geom) {
		if (geom.isEmpty()) {
			return new Sphere();
		}

		PointsVisitor v = new PointsVisitor();
		geom.accept(v);

		if (v.getPoints().size() == 0) {
			return new Sphere();
		}

		Vector3d c = new Vector3d(0, 0, 0);

		int numPoint = 0;
		while (numPoint < v.getPoints().size()) {
			c.add(v.getPoints().get(numPoint).asPoint3d());
			numPoint++;
		}

		c.scale(1/numPoint);

		// farthest point from centroid
//		Point3d f = c;
		double maxDistanceSq = 0;

		for (int i = 0; i < v.getPoints().size(); i++) {
			Point3d x = v.getPoints().get(i).asPoint3d();
			Vector3d cx = new Vector3d(x.x-c.x,x.y-c.y,x.z-c.z);
			double dSq = cx.length();
			if (dSq > maxDistanceSq) {
//				f = x;
				maxDistanceSq = dSq;
			}
		}

		return new Sphere(Math.sqrt(maxDistanceSq), new Point3d(c.x,c.y,c.z));
	}
 

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

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
protected void createWindow( DRectangle winPanel, Matrix4d to3d, 
		MeshBuilder wall, 
		MeshBuilder window, 
		MeshBuilder glass, 
		double depth,
		float sillDepth, float sillHeight,
		float corniceHeight,
		double panelWidth, double panelHeight ) {
	
	Point2d[] pts = winPanel.points();
	
	Point3d[] ptt = new Point3d[4];
	
	for (int i = 0; i < 4; i++) {
		ptt[i] = Pointz.to3( pts[i] );
		to3d.transform( ptt[i] ); 
	}
	
	Vector3d along = new Vector3d(ptt[3]);
	along.sub(ptt[0]);
	along.normalize();
	
	Vector3d up = new Vector3d(ptt[1]);
	up.sub(ptt[0]);
	up.normalize();

	Vector3d out = new Vector3d();
	out.cross( along, up );
	out.scale(-1/out.length());
	
	Vector3d loc = new Vector3d();
	loc.cross( along, up );
	loc.scale ( -depth / loc.length() );
	loc.add(ptt[0]);
	
	WindowGen.createWindow( window, glass, new Window( Jme3z.to ( loc ), Jme3z.to(along), Jme3z.to(up), 
			winPanel.width, winPanel.height, 0.3, panelWidth, panelHeight ) ); 
	
	Vector3f u = Jme3z.to(up), o = Jme3z.to( out );
	
	wall.addInsideRect( Jme3z.to ( ptt[0] ), o, Jme3z.to(along), u,  
			 (float)depth, (float)winPanel.width,(float) winPanel.height  );
	
	if (sillDepth > 0 && sillHeight > 0)
		window.addCube( Jme3z.to ( ptt[0] ).add( u.mult( -sillHeight + 0.01f ) ).add( o.mult( -sillDepth) ),
			Jme3z.to(out), Jme3z.to(along), Jme3z.to(up),
			(float)depth + sillDepth, (float)winPanel.width,(float) sillHeight  );
	
	if (corniceHeight > 0) 
		moulding( to3d, new DRectangle(winPanel.x, winPanel.getMaxY(), winPanel.width, corniceHeight), wall );
}
 

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
public static double restorativeForceAndTorsionAngleRadians(Vector3d i, Vector3d j,
                                                    Vector3d k, Vector3d l) {
  // This is adapted from http://scidok.sulb.uni-saarland.de/volltexte/2007/1325/pdf/Dissertation_1544_Moll_Andr_2007.pdf
  // Many thanks to Andreas Moll and the BALLView developers for this

  // Bond vectors of the three atoms

  i.sub(j, i);
  j.sub(k, j);
  k.sub(l, k);

  double len_ij = i.length();
  double len_jk = j.length();
  double len_kl = k.length();

  if (isNearZero(len_ij) || isNearZero(len_jk) || isNearZero(len_kl)) {
    i.set(0, 0, 0);
    j.set(0, 0, 0);
    k.set(0, 0, 0);
    l.set(0, 0, 0);
    return 0.0;
  }

  double ang = vectorAngleRadians(i, j);
  double sin_j = Math.sin(ang);
  double cos_j = Math.cos(ang);
  
  ang = vectorAngleRadians(j, k);
  double sin_k = Math.sin(ang);
  double cos_k = Math.cos(ang);

  // normalize the bond vectors:

  i.normalize();
  j.normalize();
  k.normalize();

  // use i, k, and l for temporary variables as well
  i.cross(i, j);  //a
  l.cross(j, k);  //b
  k.cross(i, l);  //c

  double theta = -Math.atan2(
      k.dot(j),  // ((ij x jk) x (jk x kl)) . jk 
      i.dot(l)); //  (ij x jk) . (jk x kl)
      
  i.scale(1. / len_ij / sin_j / sin_j);
 
  l.scale(-1. / len_kl / sin_k / sin_k);

  j.set(i);
  j.scale(-len_ij / len_jk * cos_j - 1.);
  k.set(l);
  k.scale(-len_kl / len_jk * cos_k);
  j.sub(k);
  
  k.set(i);
  k.add(j);
  k.add(l);
  k.scale(-1);

  return theta;
}
 

import javax.vecmath.Vector3d; //导入方法依赖的package包/类
private static LinearForm3D roofTween( SharedEdge a, SharedEdge b, Face f ) {
	
	if (a == null || b == null)
		return null;
	
	Vector3d aD = a.dir(f);
	
	if (aD == null)
		return null;
	
	aD.normalize();
	
	Vector3d bD = b.dir(f);
	
	if (bD == null)
		return null;
	
	bD.normalize();
	
	aD.add(bD);
	
	aD.normalize();
	
	Point3d pt = a.getEnd( f );
	
	return new LinearForm3D( new Vector3d(aD.x, aD.z, aD.y), new Point3d(pt.x, pt.z, pt.y)  );
}