Java Code Examples for javax.vecmath.Vector3d#length()

static public double CPADistance(Vector3d a,Vector3d b,Vector3d da,Vector3d db) {
	// find CPA time
	Vector3d dp = new Vector3d(b);
	dp.sub(a);
	Vector3d dv = new Vector3d(db);
	db.sub(da);		
	double t = CPATime(dp,dv);

	// get both points
	Vector3d pa = new Vector3d(da);
	pa.scale(t);
	pa.add(a);
	Vector3d pb = new Vector3d(db);
	pb.scale(t);
	pb.add(b);
	// find difference
	pb.sub(pa);
	return pb.length();
}
 

/**
 * Find all Entities within epsilon mm of pose.
 * TODO Much optimization could be done here to reduce the search time.
 * @param target the center of the cube around which to search.   
 * @param radius the maximum distance to search for entities.
 * @return a list of found PhysicalObjects
 */
public List<PoseEntity> findPhysicalObjectsNear(Vector3d target,double radius) {
	radius/=2;
	
	//Log.message("Finding within "+epsilon+" of " + target);
	List<PoseEntity> found = new ArrayList<PoseEntity>();
	
	// check all children
	for( Entity e : children ) {
		if(e instanceof PoseEntity) {
			// is physical, therefore has position
			PoseEntity po = (PoseEntity)e;
			//Log.message("  Checking "+po.getDisplayName()+" at "+pop);
			Vector3d pop = new Vector3d();
			pop.sub(po.getPosition(),target);
			if(pop.length()<=radius) {
				//Log.message("  in range!");
				// in range!
				found.add(po);
			}
		}
	}
	
	return found;
}
 

void Translate_Body_Towards(Vector3d point,float dt) {
  Vector3d dp = new Vector3d(point);
  dp.sub( body.pos );
  
  double dpl = dp.length();
  if( dpl > dt ) {
    dp.normalize();
    dp.scale( dt );
    if( body.pos.z != 0 || dp.z >= 0 ) {
      body.pos.add(dp);
      int i;
      for( i = 0; i < 6; ++i ) {
        legs[i].pan_joint.pos.add( dp );
      }
    }
  } else {
    body.pos = point;
  }
}
 

/**
 * @return longest vector from beam centre to farthest corner
 */
public Vector3d getLongestVector() {
	Vector3d[] corners = cornerPositions();
	Vector3d longVec = null;
	double length = -Double.MAX_VALUE;
	for (int i = 0; i < 4; i++) {
		Vector3d c = corners[i];
		c.sub(getBeamCentrePosition());
		double l = c.length();
		if (l > length) {
			longVec = c;
			length = l;
		}
	}
	return longVec;
}
 

/**
 * Calculate solid angle subtended by a plane triangle with given vertices
 * @param a
 * @param b
 * @param c
 * @return solid angle
 */
public static double calculatePlaneTriangleSolidAngle(Vector3d a, Vector3d b, Vector3d c) {
	// A. van Oosterom & J. Strackee, "The Solid Angle of a Plane Triangle", IEEE Trans.
	// Biom Eng BME-30(2) pp125-6 (1983)
	// tan(Omega/2) = a . (b x c) / [ a * b * c + (a . b) * c + (b . c) * a + (c . a) *b ]
	double al = a.length();
	double bl = b.length();
	double cl = c.length();
	double denom = al * bl * cl + a.dot(b)*cl + al * b.dot(c) + a.dot(c) * bl; 
	Vector3d bc = new Vector3d();
	bc.cross(b, c);
	double ang = Math.atan(Math.abs(a.dot(bc))/denom);
	if (ang < 0) {
		ang += Math.PI;
	}
	return 2 * ang;
}
 

void Stand_Up(double dt) {
  int i;
  int onfloor = 0;
  float scale = 2.0f;

  // touch the feet to the floor
  for(i=0;i<6;++i) {
    if(legs[i].ankle_joint.pos.z>0) legs[i].ankle_joint.pos.z-=4*scale*dt;
    else ++onfloor;

    // contract - put feet closer to shoulders
    Vector3d df = new Vector3d(legs[i].ankle_joint.pos);
    df.sub(body.pos);
    df.z=0;
    if(df.length()>standing_radius) {
      df.normalize();
      df.scale(6*scale*dt);
      legs[i].ankle_joint.pos.sub(df);
    }
  }

  if(onfloor==6) {
    // we've planted all feet, raise the body a bit
    if( body.pos.z < standing_height ) body.pos.z+=2*scale*dt;

    for(i=0;i<6;++i) {
      Vector3d ds = new Vector3d( legs[i].pan_joint.pos );
      ds.sub( body.pos );
      ds.normalize();
      ds.scale(standing_radius);
      legs[i].npoc.set(body.pos.x+ds.x,
    		  			body.pos.y+ds.y,
    		  			0);
    }
  }
}
 

boolean Sit_Down(double dt) {
  int i;
  int legup=0;
  float scale=1.0f;

  // we've planted all feet, lower the body to the ground
  if( body.pos.z > 0 ) body.pos.z -= 2 * scale * dt;
  else {
    for( i = 0; i < 6; ++i ) {

      // raise feet
      Vector3d ls = new Vector3d( legs[i].ankle_joint.pos );
      ls.sub( legs[i].pan_joint.pos );
      if( ls.length() < 16 ) {
        ls.z=0;
        ls.normalize();
        ls.scale( 4 * scale * dt );
        legs[i].ankle_joint.pos.add( ls );
      } else ++legup;

      if( legs[i].ankle_joint.pos.z-legs[i].pan_joint.pos.z < 5.5 ) legs[i].ankle_joint.pos.z += 4 * scale * dt;
      else ++legup;

      if( legs[i].knee_joint.pos.z-legs[i].pan_joint.pos.z < 5.5 ) legs[i].knee_joint.pos.z += 4 * scale * dt;
      else ++legup;
    }
    if( legup == 6*3 ) return true;
  }

  return false;
}
 

/**
 * Vector3d (ECF or unit vector) constructor If vector is ECF, elevation is derived from it
 * Otherwise, elevation is zero
 */
public EarthVector(final Vector3d vec) {
  final Vector3d norm = new Vector3d(vec);
  norm.normalize();

  final double sinlat = norm.z;
  final double coslat = Math.sqrt(Math.abs(1.0 - (sinlat * sinlat)));
  latitude = Math.atan2(sinlat, coslat);

  double vra;
  // not sure which epsilon value is most appropriate - just chose Y eps.
  // because it's bigger
  if (Math.abs(coslat) <= Y_EPSILON) {
    // this value's kind of arbitrary in this case
    vra = 0.0;
  } else {
    // this unchecked divide by 0 was causing EV's to have NaN's and
    // such
    // sometimes, causing stuff to break, especially for the globe view
    final double cosa = norm.x / coslat;
    final double sina = norm.y / coslat;

    if (Math.abs(cosa) < X_EPSILON) {
      vra = RAD_90 * sign(sina);
    } else {
      vra = Math.atan2(sina, cosa);
    }
  }

  longitude = vra;

  if (vec.length() > getEarthRadiusKM()) {
    elevation = vec.length() - getEarthRadiusKM();
  } else {
    elevation = 0.0;
  }

  initVector();
}
 

/**
 * Locate a coordinate on the line between this one and the "next" coord, at some fraction of the
 * distance between them
 */
public EarthVector findPoint(final EarthVector nextCoord, final double fraction) {
  // check for same point first
  if (equals(nextCoord)) {
    return new EarthVector(this);
  }

  // compute the vector normal to this vector and the input vector
  final Vector3d nextVector = nextCoord.getVector();
  final Vector3d vec = new Vector3d();
  vec.cross(ecfVector, nextVector);

  // compute the fractional angle between this vector and the input vector
  final double phi =
      fraction
          * Math.acos(ecfVector.dot(nextVector) / (ecfVector.length() * nextVector.length()));

  // create the vector rotated through phi about the normal vector
  final Vector3d output = rotate(vec, phi);

  // now scale the output vector by interpolating the magnitudes
  // of this vector and the input vector
  output.normalize();
  final double size =
      ((nextVector.length() - ecfVector.length()) * fraction) + ecfVector.length();
  output.scale(size);

  return new EarthVector(output);
}
 

public EarthVector findPointReverseDirection(final EarthVector nextCoord, final double fraction) {
  // check for same point first
  if (equals(nextCoord)) {
    return new EarthVector(this);
  }

  // compute the vector normal to this vector and the input vector
  final Vector3d nextVector = nextCoord.getVector();
  final Vector3d vec = new Vector3d();
  vec.cross(ecfVector, nextVector);
  vec.negate();

  // compute the fractional angle between this vector and the input vector
  final double phi =
      fraction
          * Math.acos(ecfVector.dot(nextVector) / (ecfVector.length() * nextVector.length()));

  // create the vector rotated through phi about the normal vector
  final Vector3d output = rotate(vec, phi);
  // now scale the output vector by interpolating the magnitudes
  // of this vector and the input vector
  output.normalize();
  final double size =
      ((nextVector.length() - ecfVector.length()) * fraction) + ecfVector.length();
  output.scale(size);

  return new EarthVector(output);
}
 

private void createTunnel(MinecraftWorld world, Dimension dimension, Random random, CaveSettings tunnelSettings, boolean surfaceBreaking, int minimumLevel) {
    Point3d location = new Point3d(tunnelSettings.start.x, tunnelSettings.start.y, tunnelSettings.start.z);
    Vector3d direction = getRandomDirection(random);
    final double minRadius = tunnelSettings.minRadius, maxRadius = tunnelSettings.maxRadius,
            radiusChangeSpeed = tunnelSettings.radiusChangeSpeed;
    double length = 0.0, radius = (maxRadius + minRadius) / 2.0, radiusDelta = 0.0;
    final int maxLength = tunnelSettings.length, twistiness = tunnelSettings.twistiness;
    if (logger.isTraceEnabled()) {
        logger.trace("Creating tunnel @ {},{},{} of length {}; radius: {} - {} (variability: {}); twistiness: {}",
                tunnelSettings.start.x, tunnelSettings.start.y, tunnelSettings.start.z, maxLength, tunnelSettings.minRadius, tunnelSettings.maxRadius,
                radiusChangeSpeed, twistiness);
    }
    while (length < maxLength) {
        if ((minimumLevel == 0) && (dimension.getLayerValueAt(Caves.INSTANCE, (int) location.x, (int) location.y) < 1)) {
            // Don't stray into areas where the layer isn't present at all
            return;
        }
        excavate(world, dimension, random, tunnelSettings, location, radius, surfaceBreaking);
        length += direction.length();
        location.add(direction);
        final Vector3d dirChange = getRandomDirection(random);
        dirChange.scale(random.nextDouble() / (5 - twistiness));
        direction.add(dirChange);
        direction.normalize();
        if (radiusChangeSpeed > 0.0) {
            radius = MathUtils.clamp(minRadius, radius + radiusDelta, maxRadius);
            radiusDelta += random.nextDouble() * 2 * radiusChangeSpeed - radiusChangeSpeed;
        }
    }
}
 

/**
 * test intersection of two cylinders.  From http://geomalgorithms.com/a07-_distance.html
 * @param cA cylinder A
 * @param cB cylinder B
 * @return true if intersect
 */
static public boolean cylinderCylinder(Cylinder cA,Cylinder cB) {
    Vector3d   u = new Vector3d(cA.GetP2());  u.sub(cA.GetP1());
    Vector3d   v = new Vector3d(cB.GetP2());  v.sub(cB.GetP1());
    Vector3d   w = new Vector3d(cA.GetP1());  w.sub(cB.GetP1());
    double    a = u.dot(u);         // always >= 0
    double    b = u.dot(v);
    double    c = v.dot(v);         // always >= 0
    double    d = u.dot(w);
    double    e = v.dot(w);
    double    D = a*c - b*b;        // always >= 0
    double    sc, sN, sD = D;       // sc = sN / sD, default sD = D >= 0
    double    tc, tN, tD = D;       // tc = tN / tD, default tD = D >= 0

    // compute the line parameters of the two closest points
    if (D < SMALL_NUM) { // the lines are almost parallel
        sN = 0.0f;         // force using point P0 on segment S1
        sD = 1.0f;         // 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.0f;
            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.0f;
        // recompute sc for this edge
        if (-d < 0.0)
            sN = 0.0f;
        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) < SMALL_NUM ? 0.0f : sN / sD;
    tc = Math.abs(tN) < SMALL_NUM ? 0.0f : tN / tD;

    // get the difference of the two closest points
    //Vector   dP = w + (sc * u) - (tc * v);  // =  L1(sc) - L2(tc)
    u.scale(sc);
    v.scale(tc);
    Vector3d dP = new Vector3d(w);
    dP.add(u);
    dP.sub(v);

    //Log.message(ca.getRadius()+"\t"+cb.getRadius()+"\t("+(ca.getRadius()+cb.getRadius())+") >=\t"+dP.length()+"\n");

    return dP.length() <= (cA.getRadius()+cB.getRadius());   // return the closest distance
}