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

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

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

public void rotateFinger() {
	Vector3d forward = new Vector3d(HOME_FORWARD_X, HOME_FORWARD_Y, HOME_FORWARD_Z);
	Vector3d right = new Vector3d(HOME_RIGHT_X, HOME_RIGHT_Y, HOME_RIGHT_Z);
	Vector3d up = new Vector3d();

	up.cross(forward, right);

	Vector3d of = new Vector3d(forward);
	Vector3d or = new Vector3d(right);
	Vector3d ou = new Vector3d(up);

	Vector3d result;

	result = MathHelper.rotateAroundAxis(forward, of, Math.toRadians(motionFuture.rotationAngleU)); // TODO
																											// rotating
																											// around
																											// itself
																											// has
																											// no
																											// effect.
	result = MathHelper.rotateAroundAxis(result, or, Math.toRadians(motionFuture.rotationAngleV));
	result = MathHelper.rotateAroundAxis(result, ou, Math.toRadians(motionFuture.rotationAngleW));
	motionFuture.finger_forward.set(result);

	result = MathHelper.rotateAroundAxis(right, of, Math.toRadians(motionFuture.rotationAngleU));
	result = MathHelper.rotateAroundAxis(result, or, Math.toRadians(motionFuture.rotationAngleV));
	result = MathHelper.rotateAroundAxis(result, ou, Math.toRadians(motionFuture.rotationAngleW));
	motionFuture.finger_left.set(result);

	motionFuture.finger_up.cross(motionFuture.finger_forward, motionFuture.finger_left);
}
 

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

void Move_Calculate_Angles() {
  int i,j;
  double x,y;
  for(i=0;i<6;++i) {
    SpideeLeg leg=legs[i];

    // find the pan angle
    Vector3d sf= new Vector3d( leg.pan_joint.pos );
    sf.sub( body.pos );
    sf.normalize();
    Vector3d sl=new Vector3d();
    sl.cross( body.up, sf );

    x = leg.pan_joint.forward.dot(sf);
    y = leg.pan_joint.forward.dot(sl);
    double pan_angle = Math.atan2( y, x );

    // find the tilt angle
    x = leg.tilt_joint.forward.dot(leg.pan_joint.forward);
    y = leg.tilt_joint.forward.dot(leg.pan_joint.up     );
    double tilt_angle = Math.atan2( y, x );

    // find the knee angle
    x = leg.knee_joint.forward.dot(leg.tilt_joint.forward);
    y = leg.knee_joint.forward.dot(leg.tilt_joint.up     );
    double knee_angle = Math.atan2( y, x );

    // translate the angles into the servo range, 0...255 over 0...PI.
    final double scale = ( 255.0f / Math.PI );
    if( i < 3 ) pan_angle = -pan_angle;
    double p = leg.pan_joint .zero - pan_angle  * leg.pan_joint .scale * scale;
    double t = leg.tilt_joint.zero + tilt_angle * leg.tilt_joint.scale * scale;
    double k = leg.knee_joint.zero - knee_angle * leg.knee_joint.scale * scale;
    leg.pan_joint .angle = p;
    leg.tilt_joint.angle = t;
    leg.knee_joint.angle = k;

    // record the history for the graphs
    for( j = 0; j < SpideeJoint.ANGLE_HISTORY_LENGTH-1; ++j ) {
      leg.pan_joint .angle_history[j] = leg.pan_joint .angle_history[j+1];
      leg.tilt_joint.angle_history[j] = leg.tilt_joint.angle_history[j+1];
      leg.knee_joint.angle_history[j] = leg.knee_joint.angle_history[j+1];
    }
    //memcpy( leg.pan_joint .angle_history, leg.pan_joint .angle_history + sizeof(float), ( Joint.ANGLE_HISTORY_LENGTH - 1 ) * sizeof(float) );
    //memcpy( leg.tilt_joint.angle_history, leg.tilt_joint.angle_history + sizeof(float), ( Joint.ANGLE_HISTORY_LENGTH - 1 ) * sizeof(float) );
    //memcpy( leg.knee_joint.angle_history, leg.knee_joint.angle_history + sizeof(float), ( Joint.ANGLE_HISTORY_LENGTH - 1 ) * sizeof(float) );
    leg.pan_joint .angle_history[ SpideeJoint.ANGLE_HISTORY_LENGTH - 1 ] = p - leg.pan_joint .zero;
    leg.tilt_joint.angle_history[ SpideeJoint.ANGLE_HISTORY_LENGTH - 1 ] = t - leg.tilt_joint.zero;
    leg.knee_joint.angle_history[ SpideeJoint.ANGLE_HISTORY_LENGTH - 1 ] = k - leg.knee_joint.zero;

    // @TODO: contrain angles in the model to the limits set in joint::angle_max and joint::angle_min
  }
}
 

@Override
public void render(GL2 gl2) {
	Vector3d fingerRight = new Vector3d();
	fingerRight.cross(finger_forward,finger_up);
	MatrixHelper.drawMatrix(gl2,fingerPosition,finger_forward,fingerRight,finger_up);
}
 

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