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

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

Vector3d getWorldCoordinatesFor(Vector3d in) {
	Vector3d out = new Vector3d(motionFuture.base);
	
	Vector3d tempx = new Vector3d(motionFuture.base_forward);
	tempx.scale(in.x);
	out.add(tempx);

	Vector3d tempy = new Vector3d(motionFuture.base_right);
	tempy.scale(-in.y);
	out.add(tempy);

	Vector3d tempz = new Vector3d(motionFuture.base_up);
	tempz.scale(in.z);
	out.add(tempz);
			
	return out;
}
 

Vector3d getWorldCoordinatesFor(Vector3d in) {
	Vector3d out = new Vector3d(motionFuture.base);

	Vector3d tempx = new Vector3d(motionFuture.baseForward);
	tempx.scale(in.x);
	out.add(tempx);

	Vector3d tempy = new Vector3d(motionFuture.baseRight);
	tempy.scale(-in.y);
	out.add(tempy);

	Vector3d tempz = new Vector3d(motionFuture.baseUp);
	tempz.scale(in.z);
	out.add(tempz);

	return out;
}
 

@Override
protected Vector3d getTorque() {
    int n = getRotorsNum();
    Vector3d torque = new Vector3d();
    Vector3d m = new Vector3d();
    Vector3d t = new Vector3d();
    for (int i = 0; i < n; i++) {
        // Roll / pitch
        t.z = -rotors[i].getThrust();
        m.cross(getRotorPosition(i), t);
        // Yaw
        m.z -= rotors[i].getTorque();
        torque.add(m);
    }
    Vector3d airRotationRate = new Vector3d(rotationRate);
    airRotationRate.scale(-1.0);
    torque.add(getAirFlowTorque(airRotationRate));
    return torque;
}
 

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

void Center_Body_Around_Feet(double dt) {
	  // center the body around the feet
	  Vector3d p = new Vector3d(0,0,0);
	  Vector3d r = new Vector3d(0,0,0);
	  int i;
	  for(i=0;i<6;++i) {
	    if(legs[i].ankle_joint.pos.z<=0) {
	      p.add(legs[i].ankle_joint.pos);
	    } else {
	      p.add(legs[i].ankle_joint.pos);
	    }
	    if(i<3) r.sub(legs[i].ankle_joint.pos);
	    else    r.add(legs[i].ankle_joint.pos);
	  }

	  p.scale(1.0f/6.0f);
	  r.scale(1.0f/6.0f);
	  Vector3d dp = new Vector3d( p.x - body.pos.x, p.y-body.pos.y,0 );
	  dp.scale(0.5f);
	  body.pos.add(dp);
	  // zero body height
	  body.pos.z += ( standing_height - body.pos.z ) * dt;

	  // zero the body orientation
	  target.left = r;
	  target.left.normalize();
	  target.up.set(0,0,1);
	  target.forward.cross(target.left, target.up);
}
 

/**
 * from image coordinates, work out position of pixel's top-left corner
 */
public void pixelPosition(final double x, final double y, Vector3d p) {
	p.set(-hPxSize * (x - sx), -vPxSize * (y - sy), 0);
	if (invOrientation != null)
		invOrientation.transform(p);
	p.add(origin);
}
 

/**
 * Calculate solid angle subtended by pixel
 * @param x
 * @param y
 * @return solid angle
 */
public double calculateSolidAngle(final int x, final int y) {
	Vector3d a = pixelPosition(x, y);
	Vector3d ab = getPixelRow();
	Vector3d ac = getPixelColumn();

	Vector3d b = new Vector3d();
	Vector3d c = new Vector3d();
	b.add(a, ab);
	c.add(a, ac);
	double s = calculatePlaneTriangleSolidAngle(a, b, c);

	a.add(b, ac);
	return s + calculatePlaneTriangleSolidAngle(b, a, c); // order is important
}
 

public void update(long t) {
    double dt = lastTime == 0 ? 0.0 : (t - lastTime) / 1000.0;
    lastTime = t;
    Vector3d r = new Vector3d(random.nextGaussian() * windDeviation, random.nextGaussian() * windDeviation, 0.0);
    Vector3d dev = new Vector3d(wind);
    dev.sub(windCurrent);
    dev.scale(1.0 / windT);
    r.add(dev);
    r.scale(dt);
    windCurrent.add(r);
}
 

@Override
protected Vector3d getForce() {
    int n = getRotorsNum();
    Vector3d f = new Vector3d();
    for (int i = 0; i < n; i++) {
        f.z -= rotors[i].getThrust();
    }
    rotation.transform(f);
    Vector3d airSpeed = new Vector3d(getVelocity());
    airSpeed.scale(-1.0);
    airSpeed.add(getWorld().getEnvironment().getWind(position));
    f.add(getAirFlowForce(airSpeed));
    return f;
}
 

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
}
 

public void showPickingTest(GL2 gl2) {
	renderChosenProjection(gl2);
	gl2.glPushMatrix();

	Ray r = rayPick();

	double cx=cursorX;
	double cy=cursorY;
       int w = canvasWidth;
       int h = canvasHeight;
       setCursor(0,0);        Ray tl = rayPick();
       setCursor(w,0);        Ray tr = rayPick();
       setCursor(0,h);        Ray bl = rayPick();
       setCursor(w,h);        Ray br = rayPick();
	cursorX=cx;
	cursorY=cy;

       double scale=20;
       tl.direction.scale(scale);
       tr.direction.scale(scale);
       bl.direction.scale(scale);
       br.direction.scale(scale);
       r.direction .scale(scale);
       
       Vector3d tl2 = new Vector3d(tl.direction);
       Vector3d tr2 = new Vector3d(tr.direction);
       Vector3d bl2 = new Vector3d(bl.direction);
       Vector3d br2 = new Vector3d(br.direction);
       Vector3d r2  = new Vector3d(r.direction );
       
       tl2.add(tl.start);
       tr2.add(tr.start);
       bl2.add(bl.start);
       br2.add(br.start);
       r2.add(r.start);
       
       gl2.glDisable(GL2.GL_TEXTURE_2D);
	gl2.glDisable(GL2.GL_LIGHTING);
	
       gl2.glColor3d(1, 0, 0);
	gl2.glBegin(GL2.GL_LINES);
	gl2.glVertex3d(tl.start.x, tl.start.y, tl.start.z);		gl2.glVertex3d(tl2.x, tl2.y, tl2.z);
	gl2.glVertex3d(tr.start.x, tr.start.y, tr.start.z);		gl2.glVertex3d(tr2.x, tr2.y, tr2.z);
	gl2.glVertex3d(bl.start.x, bl.start.y, bl.start.z);		gl2.glVertex3d(bl2.x, bl2.y, bl2.z);
	gl2.glVertex3d(br.start.x, br.start.y, br.start.z);		gl2.glVertex3d(br2.x, br2.y, br2.z);

       gl2.glColor3d(1, 1, 1);
	gl2.glVertex3d(r.start.x, r.start.y, r.start.z);		gl2.glVertex3d(r2.x,r2.y,r2.z);
	gl2.glEnd();
       gl2.glColor3d(0, 1, 0);
	gl2.glBegin(GL2.GL_LINE_LOOP);
	gl2.glVertex3d(tl2.x, tl2.y, tl2.z);
	gl2.glVertex3d(tr2.x, tr2.y, tr2.z);
	gl2.glVertex3d(br2.x, br2.y, br2.z);
	gl2.glVertex3d(bl2.x, bl2.y, bl2.z);
	gl2.glEnd();
       gl2.glColor3d(0, 0, 1);
	gl2.glBegin(GL2.GL_LINE_LOOP);
	gl2.glVertex3d(tl.start.x, tl.start.y, tl.start.z);
	gl2.glVertex3d(tr.start.x, tr.start.y, tr.start.z);
	gl2.glVertex3d(br.start.x, br.start.y, br.start.z);
	gl2.glVertex3d(bl.start.x, bl.start.y, bl.start.z);
	gl2.glEnd();
	
	PrimitiveSolids.drawStar(gl2,r2,5);
	gl2.glPopMatrix();
}
 

public void render(GL2 gl2) {
	RobotOverlord ro = (RobotOverlord)getRoot();
	ViewportEntity cameraView = ro.viewport;
	
	gl2.glClear(GL2.GL_DEPTH_BUFFER_BIT);

	gl2.glEnable(GL2.GL_DEPTH_TEST);
	gl2.glEnable(GL2.GL_CULL_FACE);
	
	gl2.glBlendFunc(GL2.GL_SRC_ALPHA,GL2.GL_ONE_MINUS_SRC_ALPHA);

   	gl2.glMatrixMode(GL2.GL_PROJECTION);
	gl2.glPushMatrix();
	cameraView.renderOrtho(gl2,1);
	gl2.glMatrixMode(GL2.GL_MODELVIEW);
	
	gl2.glPushMatrix();			
		double c = cubeSize.get();			
		PoseEntity camera = cameraView.getAttachedTo();
		Matrix4d m = camera.getPoseWorld();
		Vector3d p = camera.getPosition();
		Vector3d vx = MatrixHelper.getXAxis(m);
		Vector3d vy = MatrixHelper.getYAxis(m);
		Vector3d vz = MatrixHelper.getZAxis(m);
	
		vz.scale(-100);
		vx.scale(cameraView.getCanvasWidth() /10 -c*2);
		vy.scale(cameraView.getCanvasHeight()/10 -c*2);
		p.add(vx);
		p.add(vy);
		p.add(vz);
		
		gl2.glTranslated(p.x, p.y, p.z);
		gl2.glScaled(c,c,c);

		model.render(gl2);

		gl2.glDisable(GL2.GL_LIGHTING);
		gl2.glDisable(GL2.GL_COLOR_MATERIAL);
		gl2.glDisable(GL2.GL_TEXTURE_2D);
		
		// the big lines
		gl2.glLineWidth(4);
		gl2.glPushMatrix();
			gl2.glTranslated(-1.05,-1.05,-0.95);
			gl2.glBegin(GL2.GL_LINES);
			gl2.glColor3d(1, 0, 0);		gl2.glVertex3d(0, 0, 0);		gl2.glVertex3d(2.5, 0, 0);
			gl2.glColor3d(0, 1, 0);		gl2.glVertex3d(0, 0, 0);		gl2.glVertex3d(0, 2.5, 0);
			gl2.glColor3d(0, 0, 1);		gl2.glVertex3d(0, 0, 0);		gl2.glVertex3d(0, 0, 2.5);
			gl2.glEnd();
		gl2.glPopMatrix();
		gl2.glLineWidth(1);
					
	gl2.glPopMatrix();

   	gl2.glMatrixMode(GL2.GL_PROJECTION);
	gl2.glPopMatrix();
	gl2.glMatrixMode(GL2.GL_MODELVIEW);
}
 

/**
 * Locate a coordinate at a specific distance (km), elevation angle (radians), and heading
 * (radians) from this one.
 */
public EarthVector findPoint(
    final double distanceKM,
    final double azimuth,
    final double elevAngle) {
  // convert distance to radians
  // final double distR = distanceKM / KMPerDegree() / DPR;
  final double lon = getLongitude();
  final double lat = getLatitude();
  // convert local enu to ecf to get east and north vectors
  // east vector
  final Vector3d eastVec = new Vector3d(1, 0, 0);
  final Vector3d northVec = new Vector3d(0, 1, 0);
  final double sinLon = Math.sin(lon);
  final double cosLon = Math.cos(lon);
  final double sinLat = Math.sin(lat);
  final double cosLat = Math.cos(lat);
  final Matrix3d enuToEcf = new Matrix3d();
  enuToEcf.m00 = -sinLon;
  enuToEcf.m01 = -(sinLat * cosLon);
  enuToEcf.m02 = cosLat * cosLon;
  enuToEcf.m10 = cosLon;
  enuToEcf.m11 = -(sinLat * sinLon);
  enuToEcf.m12 = cosLat * sinLon;
  enuToEcf.m20 = 0;
  enuToEcf.m21 = cosLat;
  enuToEcf.m22 = sinLat;
  enuToEcf.transform(eastVec);
  enuToEcf.transform(northVec);
  eastVec.normalize();
  northVec.normalize();
  northVec.scale(distanceKM);
  final Matrix3d elevTrans = new Matrix3d();
  elevTrans.set(new AxisAngle4d(eastVec, elevAngle));

  elevTrans.transform(northVec);
  final Matrix3d azTrans = new Matrix3d();
  final Vector3d unitEcf = new Vector3d(ecfVector);
  unitEcf.normalize();
  azTrans.set(new AxisAngle4d(unitEcf, azimuth));
  azTrans.transform(northVec);
  final Vector3d transformedEcf = new Vector3d();
  transformedEcf.add(ecfVector, northVec);
  final EarthVector transformedEv = new EarthVector(transformedEcf);
  return transformedEv;
}
 

@Override
public void update(long t) {
    if (lastTime >= 0) {
        double dt = (t - lastTime) / 1000.0;
        if (dt < 0.001) {
            dt = 0.001; // Limit min dt to 1ms
        }
        // Position
        Vector3d dPos = new Vector3d(velocity);
        dPos.scale(dt);
        position.add(dPos);
        // Velocity
        acceleration = getForce();
        acceleration.scale(1.0 / mass);
        acceleration.add(getWorld().getEnvironment().getG());
        if (position.z >= getWorld().getEnvironment().getGroundLevel(position) &&
                velocity.z + acceleration.z * dt >= 0.0) {
            // On ground
            acceleration.x = -velocity.x / dt;
            acceleration.y = -velocity.y / dt;
            acceleration.z = -velocity.z / dt;
            position.z = getWorld().getEnvironment().getGroundLevel(position);
            //rotationRate.set(0.0, 0.0, 0.0);
        }
        Vector3d dVel = new Vector3d(acceleration);
        dVel.scale(dt);
        velocity.add(dVel);
        // Rotation
        if (rotationRate.length() > 0.0) {
            Matrix3d r = new Matrix3d();
            Vector3d rotationAxis = new Vector3d(rotationRate);
            rotationAxis.normalize();
            r.set(new AxisAngle4d(rotationAxis, rotationRate.length() * dt));
            rotation.mulNormalize(r);
        }
        // Rotation rate
        Vector3d Iw = new Vector3d(rotationRate);
        momentOfInertia.transform(Iw);
        Vector3d angularAcc = new Vector3d();
        angularAcc.cross(rotationRate, Iw);
        angularAcc.negate();
        angularAcc.add(getTorque());
        momentOfInertiaInv.transform(angularAcc);
        angularAcc.scale(dt);
        rotationRate.add(angularAcc);
    }
    lastTime = t;
}