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

The following examples show how to use javax.vecmath.Vector3d#normalize() . You can vote up the ones you like or vote down the ones you don't like, and go to the original project or source file by following the links above each example. You may check out the related API usage on the sidebar.
Example 1
Source File: Spidee.java    From Robot-Overlord-App with GNU General Public License v2.0 6 votes vote down vote up
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;
  }
}
 
Example 2
Source File: EarthVector.java    From geowave with Apache License 2.0 6 votes vote down vote up
/** Vector3d (unit vector)/elev constructor */
public EarthVector(final Vector3d vec, final double inelev) {
  final Vector3d norm = vec;
  norm.normalize();

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

  final double cosa = norm.x / coslat;
  final double sina = norm.y / coslat;
  double vra;

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

  longitude = vra;

  elevation = inelev;

  initVector();
}
 
Example 3
Source File: Spidee.java    From Robot-Overlord-App with GNU General Public License v2.0 5 votes vote down vote up
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);
    }
  }
}
 
Example 4
Source File: QuatSymmetrySubunits.java    From biojava with GNU Lesser General Public License v2.1 5 votes vote down vote up
private void calcCenters() {
	for (Point3d p : originalCenters) {
		Point3d c = new Point3d(p);
		c.sub(centroid);
		centers.add(c);
		Vector3d v = new Vector3d(c);
		v.normalize();
		unitVectors.add(v);
	}
}
 
Example 5
Source File: EarthVector.java    From geowave with Apache License 2.0 5 votes vote down vote up
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;
}
 
Example 6
Source File: EarthVector.java    From geowave with Apache License 2.0 5 votes vote down vote up
/**
 * Rotates this coordinate about the input vector through the input angle (radians - because we
 * usually use this internally)
 *
 * @param rotAxis The axis of rotation
 * @param angle The angle of rotation (in radians)
 */
public Vector3d rotate(final Vector3d rotAxis, final double angle) {
  final Vector3d thisVec = new Vector3d(ecfVector);
  final Vector3d axis = new Vector3d(rotAxis);
  axis.normalize();

  final Matrix3d trans = new Matrix3d();
  trans.set(new AxisAngle4d(axis, angle));

  trans.transform(thisVec);

  return thisVec;
}
 
Example 7
Source File: EarthVector.java    From geowave with Apache License 2.0 5 votes vote down vote up
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);
}
 
Example 8
Source File: EarthVector.java    From geowave with Apache License 2.0 5 votes vote down vote up
/**
 * 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);
}
 
Example 9
Source File: CavesExporter.java    From WorldPainter with GNU General Public License v3.0 5 votes vote down vote up
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;
        }
    }
}
 
Example 10
Source File: EarthVector.java    From geowave with Apache License 2.0 5 votes vote down vote up
/**
 * 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();
}
 
Example 11
Source File: Spidee.java    From Robot-Overlord-App with GNU General Public License v2.0 5 votes vote down vote up
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;
}
 
Example 12
Source File: DetectorProperties.java    From dawnsci with Eclipse Public License 1.0 4 votes vote down vote up
/**
 * @return scattering angle (two-theta) associated with pixel
 */
public double pixelScatteringAngle(final double x, final double y) {
	Vector3d p = pixelPosition(x, y);
	p.normalize();
	return Math.acos(p.dot(beamVector));
}
 
Example 13
Source File: DetectorProperties.java    From dawnsci with Eclipse Public License 1.0 4 votes vote down vote up
/**
 * @param beamVector
 *            The beam vector to set.
 */
public void setBeamVector(Vector3d beamVector) {
	this.beamVector = beamVector;
	beamVector.normalize();
}
 
Example 14
Source File: EarthVector.java    From geowave with Apache License 2.0 4 votes vote down vote up
/**
 * 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;
}
 
Example 15
Source File: DragBallEntity.java    From Robot-Overlord-App with GNU General Public License v2.0 4 votes vote down vote up
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();
	}
}
 
Example 16
Source File: Spidee.java    From Robot-Overlord-App with GNU General Public License v2.0 4 votes vote down vote up
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
  }
}
 
Example 17
Source File: SystematicSolver.java    From biojava with GNU Lesser General Public License v2.1 4 votes vote down vote up
private boolean evaluatePermutation(List<Integer> permutation) {
	// permutate subunits
	for (int j = 0, n = subunits.getSubunitCount(); j < n; j++) {
		transformedCoords[j].set(originalCoords[permutation.get(j)]);
	}

	int fold = PermutationGroup.getOrder(permutation);

	// TODO implement this piece of code using at origin superposition
	Quat4d quat = UnitQuaternions.relativeOrientation(
			originalCoords, transformedCoords);
	AxisAngle4d axisAngle = new AxisAngle4d();
	Matrix4d transformation = new Matrix4d();

	transformation.set(quat);
	axisAngle.set(quat);

	Vector3d axis = new Vector3d(axisAngle.x, axisAngle.y, axisAngle.z);
	if (axis.lengthSquared() < 1.0E-6) {
		axisAngle.x = 0;
		axisAngle.y = 0;
		axisAngle.z = 1;
		axisAngle.angle = 0;
	} else {
		axis.normalize();
		axisAngle.x = axis.x;
		axisAngle.y = axis.y;
		axisAngle.z = axis.z;
	}

	CalcPoint.transform(transformation, transformedCoords);

	double subunitRmsd = CalcPoint.rmsd(transformedCoords, originalCoords);

	if (subunitRmsd <parameters.getRmsdThreshold()) {
		// transform to original coordinate system
		combineWithTranslation(transformation);
		QuatSymmetryScores scores = QuatSuperpositionScorer.calcScores(subunits, transformation, permutation);
		if (scores.getRmsd() < 0.0 || scores.getRmsd() > parameters.getRmsdThreshold()) {
			return false;
		}

		scores.setRmsdCenters(subunitRmsd);
		Rotation symmetryOperation = createSymmetryOperation(permutation, transformation, axisAngle, fold, scores);
		rotations.addRotation(symmetryOperation);
		return true;
	}
	return false;
}
 
Example 18
Source File: C2RotationSolver.java    From biojava with GNU Lesser General Public License v2.1 4 votes vote down vote up
private void solve() {
		initialize();
		Vector3d trans = new Vector3d(subunits.getCentroid());
		trans.negate();
		List<Point3d[]> traces = subunits.getTraces();

//		Point3d[] x = SuperPosition.clonePoint3dArray(traces.get(0));
//		SuperPosition.center(x);
//		Point3d[] y = SuperPosition.clonePoint3dArray(traces.get(1));
//		SuperPosition.center(y);

		Point3d[] x = CalcPoint.clonePoint3dArray(traces.get(0));
		CalcPoint.translate(trans, x);
		Point3d[] y = CalcPoint.clonePoint3dArray(traces.get(1));
		CalcPoint.translate(trans, y);

		// TODO implement this piece of code using at origin superposition
		Quat4d quat = UnitQuaternions.relativeOrientation(
				x, y);
		AxisAngle4d axisAngle = new AxisAngle4d();
		Matrix4d transformation = new Matrix4d();

		transformation.set(quat);
		axisAngle.set(quat);

		Vector3d axis = new Vector3d(axisAngle.x, axisAngle.y, axisAngle.z);
		if (axis.lengthSquared() < 1.0E-6) {
			axisAngle.x = 0;
			axisAngle.y = 0;
			axisAngle.z = 1;
			axisAngle.angle = 0;
		} else {
			axis.normalize();
			axisAngle.x = axis.x;
			axisAngle.y = axis.y;
			axisAngle.z = axis.z;
		}

		CalcPoint.transform(transformation, y);

		// if rmsd or angle deviation is above threshold, stop
		double angleThresholdRadians = Math.toRadians(parameters.getAngleThreshold());
		double deltaAngle = Math.abs(Math.PI-axisAngle.angle);

		if (deltaAngle > angleThresholdRadians) {
			rotations.setC1(subunits.getSubunitCount());
			return;
		}

		// add unit operation
		addEOperation();

		// add C2 operation
		int fold = 2;
		combineWithTranslation(transformation);
		List<Integer> permutation = Arrays.asList(1,0);
		QuatSymmetryScores scores = QuatSuperpositionScorer.calcScores(subunits, transformation, permutation);
		scores.setRmsdCenters(0.0); // rmsd for superposition of two subunits centers is zero by definition

		if (scores.getRmsd() > parameters.getRmsdThreshold() || deltaAngle > angleThresholdRadians) {
			rotations.setC1(subunits.getSubunitCount());
			return;
		}

		Rotation symmetryOperation = createSymmetryOperation(permutation, transformation, axisAngle, fold, scores);
		rotations.addRotation(symmetryOperation);
	}
 
Example 19
Source File: DynamicObject.java    From jMAVSim with BSD 3-Clause "New" or "Revised" License 4 votes vote down vote 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;
}
 
Example 20
Source File: EarthVector.java    From geowave with Apache License 2.0 3 votes vote down vote up
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;
}