Java Vector3d.sub方法代码示例

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