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

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();
}
 

protected void conditionalChange() {
	if(lock.isLocked()) return;
	lock.lock();
		
	Vector3d oldValue = e.get(); 
	Vector3d newValue = new Vector3d(
		getField(0,oldValue.x),
		getField(1,oldValue.y),
		getField(2,oldValue.z)
	);

	Vector3d diff = new Vector3d();
	diff.sub(newValue,oldValue);
	
	if(diff.lengthSquared()>1e-6) {
		ro.undoableEditHappened(new UndoableEditEvent(this,new ActionChangeVector3d(e, newValue) ) );
	}
	
	lock.unlock();
}
 

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

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

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;
  }
}
 

private static double getAngle(IAtom atom1, IAtom atom2, IAtom atom3) {

        Vector3d centerAtom = new Vector3d();
        centerAtom.x = atom1.getPoint3d().x;
        centerAtom.y = atom1.getPoint3d().y;
        centerAtom.z = atom1.getPoint3d().z;
        Vector3d firstAtom = new Vector3d();
        Vector3d secondAtom = new Vector3d();

        firstAtom.x = atom2.getPoint3d().x;
        firstAtom.y = atom2.getPoint3d().y;
        firstAtom.z = atom2.getPoint3d().z;

        secondAtom.x = atom3.getPoint3d().x;
        secondAtom.y = atom3.getPoint3d().y;
        secondAtom.z = atom3.getPoint3d().z;

        firstAtom.sub(centerAtom);
        secondAtom.sub(centerAtom);

        return firstAtom.angle(secondAtom);
    }
 

private double internalGetAzimuth(final EarthVector loc) { // Calculate the True North unit vector
  final EarthVector locNorth = new EarthVector(this);
  final double radInc = Math.max(RAD_1, Math.abs(loc.getLatitude() - getLatitude()));
  final boolean calcNorth = (latitude < loc.getLatitude());
  if (calcNorth) {
    locNorth.setLatitude(locNorth.getLatitude() + radInc);
  } else {
    locNorth.setLatitude(locNorth.getLatitude() - radInc);
  }
  final Vector3d vecNorth = locNorth.getVector();
  vecNorth.sub(ecfVector);

  // Calculate the azimuth between this and loc
  final Vector3d vecTemp = new Vector3d(loc.getVector());
  vecTemp.sub(ecfVector);

  vecNorth.normalize();
  vecTemp.normalize();
  double azimuth = Math.acos(vecNorth.dot(vecTemp));
  if (!calcNorth) {
    azimuth = RAD_180 - azimuth;
  }
  final double deltaLon = Math.abs(loc.getLongitude() - longitude);
  if (((loc.getLongitude() < longitude) && (deltaLon < RAD_180))
      || ((loc.getLongitude() > longitude) && (deltaLon > RAD_180))) {
    // normalize azimuth to 0-360 degrees
    azimuth = RAD_360 - azimuth;
  }

  return azimuth;
}
 

/**
 * from position on detector, work out pixel coordinates
 * 
 * @param p
 *            position vector
 * @param t
 *            output vector (x and y components are pixel coordinates)
 */
public void pixelCoords(final Vector3d p, Vector3d t) {
	t.sub(p, origin);
	if (orientation != null)
		orientation.transform(t);
	t.x /= -hPxSize;
	t.x += sx;
	t.y /= -vPxSize;
	t.y += sy;
}
 

/**
 * Compute the vector from this coord to the input coord.
 */
public Vector3d getVector(final EarthVector loc) {
  final Vector3d vecTemp = new Vector3d(loc.getVector());
  vecTemp.sub(ecfVector);

  return vecTemp;
}
 

@Override
public Vector3d getAcc() {
    Vector3d accBody = new Vector3d(object.getAcceleration());
    accBody.sub(object.getWorld().getEnvironment().getG());
    Matrix3d rot = new Matrix3d(object.getRotation());
    rot.transpose();
    rot.transform(accBody);
    return accBody;
}
 

/**
 * transform a world-space point to the ball's current frame of reference
 * @param pointInWorldSpace the world space point
 * @return the transformed Vector3d
 */
public Vector3d getPickPointInFOR(Vector3d pointInWorldSpace,Matrix4d frameOfReference) {
	Matrix4d iMe = new Matrix4d(frameOfReference);
	iMe.m30=iMe.m31=iMe.m32=0;
	iMe.invert();
	Vector3d pickPointInBallSpace = new Vector3d(pointInWorldSpace);
	pickPointInBallSpace.sub(this.getPosition());
	iMe.transform(pickPointInBallSpace);
	return pickPointInBallSpace;
}
 

@Override
public void update(long t) {
    double progress = Math.min(1.0, Math.max(0.0, (double) (t - timeStart) / (double) (timeFinish - timeStart)));
    Vector3d vec = new Vector3d();
    vec.sub(positionFinish, positionStart);
    position.scaleAdd(progress, vec, positionStart);
    if (progress > 0.0 && progress < 1.0) {
        velocity.scale(1000.0 / (timeFinish - timeStart), vec);
    } else {
        velocity.set(0.0, 0.0, 0.0);
    }
}
 

void Teleport( Vector3d newpos ) {
	// move a robot to a new position, update all joints.
	newpos.sub( body.pos );
	body.pos.add( newpos );

	int i;
	for( i = 0; i < 6; ++i ) {
		SpideeLeg leg = legs[i];
	    leg.pan_joint.pos.add( newpos );
	    leg.tilt_joint.pos.add( newpos );
	    leg.knee_joint.pos.add( newpos );
	    leg.ankle_joint.pos.add( newpos );
	  }
}
 

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);
    }
  }
}
 

/**
 * Set beam centre (where beam intersects detector)
 * @param coords in image
 */
public void setBeamCentreCoords(double[] coords) {
	try {
		Vector3d oc = getBeamCentrePosition(); // old beam centre
		oc.sub(pixelPosition(coords[0], coords[1]));
		origin.add(oc); // shift origin accordingly
	} catch (IllegalStateException e) {
		// do nothing
	}
	// Tell listeners
	fireDetectorPropertyListeners(new DetectorPropertyEvent(this, EventType.BEAM_CENTRE));
}
 

/**
 * 
 * @param planePoint point on plane
 * @param planeNormal normal of plane
 * @param rayPoint origin of ray
 * @param rayNormal direction of ray
 * @return Double.POSITIVE_INFINITY if no collision (orthogonal).  otherwise, distance to plane.
 */
static public double rayPlaneIntersection(final Vector3d planePoint,final Vector3d planeNormal,final Vector3d rayPoint,final Vector3d rayNormal) {
	Vector3d dp = new Vector3d(planePoint);
	dp.sub(rayPoint);

	double denominator = rayNormal.dot(planeNormal);
	if(denominator==0) {
		// rays are orthogonal, never collide.
		return Double.POSITIVE_INFINITY;
	} else {
		double numerator = dp.dot(planeNormal);
		return numerator/denominator;
	}
}
 

/**
 * interpolate from a to b
 * @param a
 * @param b
 * @param t [0...1]
 * @return
 */
static public Vector3d interpolate(Vector3d a,Vector3d b,double t) {
	Vector3d n = new Vector3d(b);
	n.sub(a);
	n.scale((float)t);
	n.add(a);
	
	return n;
}
 

public void renderTranslation(GL2 gl2) {
	// camera forward is -z axis 
	RobotOverlord ro = (RobotOverlord)getRoot();
	PoseEntity camera = ro.viewport.getAttachedTo();
	Vector3d lookAtVector = subject.getPosition();
	lookAtVector.sub(camera.getPosition());
	lookAtVector.normalize();

	float r = (majorAxis==Axis.X) ? 1 : 0.5f;
	float g = (majorAxis==Axis.Y) ? 1 : 0.5f;
	float b = (majorAxis==Axis.Z) ? 1 : 0.5f;

	Vector3d fx = MatrixHelper.getXAxis(FOR);
	Vector3d fy = MatrixHelper.getYAxis(FOR);
	Vector3d fz = MatrixHelper.getZAxis(FOR);
	// should we hide an axis if it points almost the same direction as the camera?
	boolean drawX = (Math.abs(lookAtVector.dot(fx))<0.95);
	boolean drawY = (Math.abs(lookAtVector.dot(fy))<0.95);
	boolean drawZ = (Math.abs(lookAtVector.dot(fz))<0.95);

	if(drawX) {
		gl2.glColor3f(r,0,0);
		renderTranslationHandle(gl2,new Vector3d(1,0,0));
	}
	if(drawY) {
		gl2.glColor3f(0,g,0);
		renderTranslationHandle(gl2,new Vector3d(0,1,0));
	}
	if(drawZ) {
		gl2.glColor3f(0,0,b);
		renderTranslationHandle(gl2,new Vector3d(0,0,1));
	}
	
	if(isActivelyMoving) {
		// the distance moved.
		gl2.glBegin(GL2.GL_LINES);
		gl2.glColor3f(255,255,255);
		gl2.glVertex3d(0,0,0);
		gl2.glVertex3d(
				(startMatrix.m03-resultMatrix.m03)/ballSize.get(),
				(startMatrix.m13-resultMatrix.m13)/ballSize.get(),
				(startMatrix.m23-resultMatrix.m23)/ballSize.get());
		gl2.glEnd();
	}
}
 

/**
 * 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
}
 

public Vector3d getNormalizedEarthTangentVector(final double azimuth) {
  // TODO: rewrite this to use real math instead of this silly difference

  final EarthVector nextEV = findPoint(1, azimuth);

  final Vector3d deltaVec = new Vector3d();
  deltaVec.sub(nextEV.getVector(), getVector());

  deltaVec.normalize();

  return deltaVec;
}