Java Vector3d.length方法代碼示例

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

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包/類
static double distancePointSegment3D(Point3d pt, Segment seg) throws GeometryException {
	Vector3d diff = GeometryUtils.vectorSubtract(pt, seg.p0.asPoint3d());
	Vector3d segvec = GeometryUtils.vectorSubtract(seg.p1.asPoint3d(), seg.p0.asPoint3d());
	double d = diff.dot(segvec);
	if (d <= 0)
		return (diff.length());
	if ((d * diff.length()) > (segvec.length()))
		return distancePointPoint3D(pt, seg.p1.asPoint3d());

	Vector3d cr = new Vector3d();
			cr.cross(segvec,diff);
	return cr.length() / (segvec.length());
}
 

import javax.vecmath.Vector3d; //導入方法依賴的package包/類
public static double area( Triangle g )
{
    Vector3d v1=GeometryUtils.vectorSubtract(g.p1.asPoint3d(),g.p0.asPoint3d());
    Vector3d v2=GeometryUtils.vectorSubtract(g.p2.asPoint3d(),g.p0.asPoint3d());
    Vector3d cross=new Vector3d();
    cross.cross(v1, v2);
    return cross.length()/2;
}
 

import javax.vecmath.Vector3d; //導入方法依賴的package包/類
/**
 */
private Vector3d notPushPartner(Vector3d botsVelocity, Player player, RefBoolean wantsToGoFaster) {
    Vector3d myVelo = botself.getVelocity().getVector3d();
    Vector3d hisVelo = player.getVelocity().getVector3d();
    Location myLoc = botself.getLocation();
    Location hisLoc = player.getLocation();
    Vector2d vInterSec = SteeringTools.getIntersectionOld(new Vector2d(myLoc.x, myLoc.y), new Vector2d(myVelo.x, myVelo.y), new Vector2d(hisLoc.x, hisLoc.y), new Vector2d(hisVelo.x, hisVelo.y));
    Vector3d result = new Vector3d();
    boolean noForce = true;
    if (vInterSec != null) {    //Zajímají nás jen ty případy, kdy se mají naše budoucí dráhy křížit.
        Location locInterSec = new Location(vInterSec.x, vInterSec.y, myLoc.z);
        double myDist = locInterSec.getDistance(myLoc);
        double hisDist = locInterSec.getDistance(hisLoc);
        double myTime = myDist / myVelo.length();
        double hisTime = hisDist / hisVelo.length();
        double minTime = Math.min(myTime, hisTime);
        Location myNewLoc = new Location(myLoc.x + myVelo.x * minTime, myLoc.y + myVelo.y * minTime, myLoc.z);
        Location hisNewLoc = new Location(hisLoc.x + hisVelo.x * minTime, hisLoc.y + hisVelo.y * minTime, hisLoc.z);
        double newLocsDiff = myNewLoc.getDistance(hisNewLoc);

        //Podle visionInTicks spočítáme okruh, který nás zajímá - a vše co bude dál (než far_distance), budeme igonorvat.
        double far_distance = projection*myVelo.length();
        double far_distance2 = Math.max(far_distance,distanceFromOtherPeople+1); //Aby far_distance2 bylo vyšší než distanceFromOtherPeople.
        if (myDist <= far_distance2 && newLocsDiff < 2*distanceFromOtherPeople) { //Zajímá nás jen to, kdy není průsečík moc daleko a když bychom se na něm měli setkat "společně", tedy že lokace, na které dorazíme za minTime, budou méně vzdálené než je povolená vzdálenost.
            double koefA = (far_distance2 - myDist) / (far_distance2 - distanceFromOtherPeople);
            koefA = Math.min(koefA, 1);
            double koefB = ( 2*distanceFromOtherPeople - newLocsDiff) / (2*distanceFromOtherPeople);
            if (myTime < hisTime && acceleration) {
                if (SteeringManager.DEBUG) System.out.println("We speed up: koefA "+koefA+" koefB "+koefB);
                noForce = false;
                result = getBiggerVelocity(botsVelocity, 3*koefA*koefB, false, wantsToGoFaster);
            } else if (myTime > hisTime && deceleration) {
                if (SteeringManager.DEBUG) System.out.println("We slow down: koefA "+koefA+" koefB "+koefB);
                noForce = false;
                result = getBiggerVelocity(botsVelocity, koefA*koefB, true, wantsToGoFaster);
            } else if (myTime == hisTime) {
                boolean slowDown = random.nextBoolean();
                if (SteeringManager.DEBUG) System.out.println("Random --> We slow down "+slowDown+" koefA "+koefA+" koefB "+koefB);
                noForce = false;
                result = getBiggerVelocity(botsVelocity, 5, slowDown, wantsToGoFaster);
            }
        }
    }
    if (noForce && circumvention) {   //Pokud žádný z nich nezpomaluje ani nezrychluje, může mít smysl, aby se obešli.
        result = goRoundPartner(player);
    }
    if (SteeringManager.DEBUG) System.out.println("pushing force: " + result.length());
    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包/類
@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包/類
private static double vectorAngleRadians(Vector3d v1, Vector3d v2) {
  double l1 = v1.length();
  double l2 = v2.length();
  return (isNearZero(l1) || isNearZero(l2) ? 0 :
    Math.acos(v1.dot(v2) / (l1 * l2)));
}
 

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包/類
static double distanceGeometryCollectionToGeometry3D(Geometry gA, Geometry gB) throws GeometryException {

		if (gA.isEmpty() || gB.isEmpty()) {
			return Double.MAX_VALUE;
		}

		// if bounding spheres (BS) of gB and gAi don't intersect and
		// if the closest point of BS(gAj) is further than the farest
		// point of BS(gAi) there is no need to compute the distance(gAj, gB)
		// since it will be greater than distance(gAi, gB)
		//
		// The aim is not to find the minimal bounding sphere, but a good
		// enough sphere than
		// encloses all points
		List<Integer> noTest = new ArrayList<Integer>();

		List<Sphere> bsA = new ArrayList<Sphere>();

		for (int i = 0; i < gA.numGeometries(); i++) {
			bsA.add(boundingSphere(gA.geometryN(i)));
		}

		Sphere bsB = boundingSphere(gB);

		if (bsB.isEmpty()) {
			return Double.POSITIVE_INFINITY;
		}

		List<Integer> noIntersect = new ArrayList<Integer>();

		for (int i = 0; i < gA.numGeometries(); i++) {
			if (bsA.get(i).isEmpty()) {
				continue;
			}

			double l2 = Math.pow(GeometryUtils.vectorSubtract(bsB.getCenter(), bsA.get(i).getCenter()).length(), 2);

			if (Math.pow(bsB.getRadius() + bsA.get(i).getRadius(), 2) < l2) {
				noIntersect.add(i);
			}
		}

		for (int i = 0; i < noIntersect.size(); i++) {
			Vector3d vi=GeometryUtils.vectorSubtract(bsA.get(noIntersect.get(i)).getCenter(), bsB.getCenter());
			double li = vi.length();

			for (int j = i; j < noIntersect.size(); j++) {
				Vector3d vj=GeometryUtils.vectorSubtract(bsA.get(noIntersect.get(j)).getCenter(), bsB.getCenter());
				double lj = vj.length();

				if (li + bsA.get(noIntersect.get(i)).getRadius() < lj - bsA.get(noIntersect.get(j)).getRadius()) {
					noTest.add(noIntersect.get(j));
				} else if (lj + bsA.get(noIntersect.get(j)).getRadius() < li
						- bsA.get(noIntersect.get(i)).getRadius()) {
					noTest.add(noIntersect.get(i));
				}
			}
		}

		double dMin = Double.POSITIVE_INFINITY;

		for (int i = 0; i < gA.numGeometries(); i++) {
			if (noTest.size() - 1 != noTest.indexOf(i)) {
				continue;
			}

			dMin = Math.min(dMin, distance3D(gA.geometryN(i), gB));
			if(dMin==0){
				return 0;
			}
		}

		return dMin;
	}
 

import javax.vecmath.Vector3d; //導入方法依賴的package包/類
private static double distanceSegmentSegment3D(Segment s1, Segment s2) {
	Vector3d u = GeometryUtils.vectorSubtract(s1.p1.asPoint3d(),s1.p0.asPoint3d());
	Vector3d v = GeometryUtils.vectorSubtract(s2.p1.asPoint3d(),s2.p0.asPoint3d());
	Vector3d w = GeometryUtils.vectorSubtract(s1.p0.asPoint3d(),s2.p0.asPoint3d());
	double a = u.dot(u); 
	double b = u.dot(v);
	double c = v.dot(v); 
	double d = u.dot(w);
	double e = v.dot(w);
	double D = a * c - b * b; 
	double sc, sN, sD = D; 
	double tc, tN, tD = D; 

	// compute the line parameters of the two closest points
	if (D < EPS) { // the lines are almost parallel
		sN = 0.0; // force using point P0 on segment S1
		sD = 1.0; // to prevent possible division by 0.0 later
		tN = e;
		tD = c;
	} else { // get the closest points on the infinite lines
		sN = (b * e - c * d);
		tN = (a * e - b * d);
		if (sN < 0.0) { // sc < 0 => the s=0 edge is visible
			sN = 0.0;
			tN = e;
			tD = c;
		} else if (sN > sD) { // sc > 1 => the s=1 edge is visible
			sN = sD;
			tN = e + b;
			tD = c;
		}
	}

	if (tN < 0.0) { // tc < 0 => the t=0 edge is visible
		tN = 0.0;
		// recompute sc for this edge
		if (-d < 0.0)
			sN = 0.0;
		else if (-d > a)
			sN = sD;
		else {
			sN = -d;
			sD = a;
		}
	} else if (tN > tD) { // tc > 1 => the t=1 edge is visible
		tN = tD;
		// recompute sc for this edge
		if ((-d + b) < 0.0)
			sN = 0;
		else if ((-d + b) > a)
			sN = sD;
		else {
			sN = (-d + b);
			sD = a;
		}
	}
	// finally do the division to get sc and tc
	sc = (Math.abs(sN) < EPS ? 0.0 : sN / sD);
	tc = (Math.abs(tN) < EPS ? 0.0 : tN / tD);

	u.scaleAdd(sc, w);
	v.scale(tc);
	Vector3d dP = GeometryUtils.vectorSubtract(u, v);

	return dP.length(); // return the closest distance
}