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

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

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

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

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

/**
 * Calculate point of intersection of vector from lab frame origin with detector
 * 
 * @param v
 *            vector (does not have to be a unit vector)
 * @param p
 *            position vector of intersection
 */
public void intersect(final Vector3d v, Vector3d p) {
	double t = normal.dot(v);
	if (t == 0) {
		throw new IllegalArgumentException("No intersection possible as vector is parallel to detector");
	}
	t = normal.dot(origin) / t;
	p.scale(t, v);
}
 

/**
 * Set distance from sample to detector
 * @param distance
 */
public void setDetectorDistance(double distance) {
	Vector3d b = new Vector3d(normal);
	b.scale(getDetectorDistance()-distance);
	origin.add(b);
	fireDetectorPropertyListeners(new DetectorPropertyEvent(this, EventType.ORIGIN));
}
 

/**
 * Set distance from sample to beam centre.
 * <p>
 * Can throw an exception if direct beam does not intersect detector.
 * @param distance
 */
public void setBeamCentreDistance(double distance) {
	distance -= getBeamCentrePosition().length();
	Vector3d b = new Vector3d(beamVector);
	b.scale(distance);
	origin.add(b);
	fireDetectorPropertyListeners(new DetectorPropertyEvent(this, EventType.ORIGIN));
}
 

public double distanceToTarget() {
	Matrix4d currentMatrix = endEffector.getPoseWorld();
	
	// linear difference in centers
	Vector3d c0 = new Vector3d();
	Vector3d c1 = new Vector3d();
	currentMatrix.get(c0);
	targetMatrix.get(c1);
	c1.sub(c0);
	double dC = c1.lengthSquared();
	
	// linear difference in X handles
	Vector3d x0 = MatrixHelper.getXAxis(targetMatrix);
	Vector3d x1 = MatrixHelper.getXAxis(currentMatrix);
	x1.scale(CORRECTIVE_FACTOR);
	x0.scale(CORRECTIVE_FACTOR);
	x1.sub(x0);
	double dX = x1.lengthSquared();
	
	// linear difference in Y handles
	Vector3d y0 = MatrixHelper.getYAxis(targetMatrix);
	Vector3d y1 = MatrixHelper.getYAxis(currentMatrix);
	y1.scale(CORRECTIVE_FACTOR);
	y0.scale(CORRECTIVE_FACTOR);
	y1.sub(y0);
	double dY = y1.lengthSquared();		

    // now sum these to get the error term.
	return dC+dX+dY;
}
 

/**
 * 
 * @param mStart matrix of start pose
 * @param mEnd matrix of end pose
 * @param dt time scale, seconds
 * @param cartesianForce 6 doubles that will be filled with the XYZ translation and UVW rotation.
 * @return true if successful
 */
protected boolean getCartesianForceBetweenTwoPoses(Matrix4d mStart,Matrix4d mEnd,double dt,double[] cartesianForce) {
	Vector3d p0 = new Vector3d();
	Vector3d p1 = new Vector3d();
	Vector3d dp = new Vector3d();
	mStart.get(p0);
	mEnd.get(p1);
	dp.sub(p1,p0);
	dp.scale(1.0/dt);

	mStart.setTranslation(new Vector3d(0,0,0));
	mEnd.setTranslation(new Vector3d(0,0,0));
	// get the rotation force
	Quat4d q0 = new Quat4d();
	Quat4d q1 = new Quat4d();
	Quat4d dq = new Quat4d();
	q0.set(mStart);
	q1.set(mEnd);
	dq.sub(q1,q0);
	dq.scale(2/dt);
	Quat4d w = new Quat4d();
	w.mulInverse(dq,q0);
	
	cartesianForce[0]=dp.x;
	cartesianForce[1]=dp.y;
	cartesianForce[2]=dp.z;
	cartesianForce[3]=-w.x;
	cartesianForce[4]=-w.y;
	cartesianForce[5]=-w.z;
	
	return true;
}
 

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

void Update_Gait_Leg(int leg_index,double step,double dt) {
  SpideeLeg leg=legs[leg_index];
  double step_adj = ( step <= 0.5f ) ? step : 1 - step;
  step_adj = Math.sin( step_adj * Math.PI );

  // if we do nothing else, robot will march in place.

  Vector3d dp = new Vector3d( leg.npoc );
  dp.sub( leg.ankle_joint.pos );
  dp.z=0;
  dp.scale( step );

  leg.ankle_joint.pos.add( dp );
  leg.ankle_joint.pos.z = step_adj * stride_height;
}
 

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

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

void Translate_Body(double dt) {
	// IK test - moving body

	float a=(float)buttons[BUTTONS_X_NEG]
			-(float)buttons[BUTTONS_X_POS];
	float b=(float)buttons[BUTTONS_Y_NEG]
			-(float)buttons[BUTTONS_Y_POS];
	float c=(float)buttons[BUTTONS_Z_NEG]
			-(float)buttons[BUTTONS_Z_POS];
	float a1=Math.max(Math.min(a,MAX_VEL),-MAX_VEL);  // sideways
	float b1=Math.max(Math.min(b,MAX_VEL),-MAX_VEL);  // forward/back
	float c1=Math.max(Math.min(c,MAX_VEL),-MAX_VEL);  // raise/lower body


	Vector3d forward = new Vector3d( body.forward );
	forward.scale(b1);
	Vector3d t2 = new Vector3d(body.left);
	t2.scale(a1);
	forward.sub(t2);

	body.pos.z-=c1;
	if(body.pos.z>0) {
		body.pos.add( forward );
		int i;
		for(i=0;i<6;++i) {
			legs[i].pan_joint.pos.add( forward );
			legs[i].pan_joint.pos.z += c1;
		}
	}
}
 

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

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

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

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

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