How to use

Vector4d

in

javax.vecmath

/**
* Transform vector by multiplying it by this transform and put result back
* into vector.
*
* @param vector
*/
public final void transform(Vector4d vector)
{
 if (vector.getW() != 1.0)
 {
   throw new RuntimeException("Final element of vector must be 1.");
 }
 double x = mat00 * vector.getX() + mat01 * vector.getY() + mat02 * vector.getZ() + mat03;
 double y = mat10 * vector.getX() + mat11 * vector.getY() + mat12 * vector.getZ() + mat13;
 vector.setZ(mat20 * vector.getX() + mat21 * vector.getY() + mat22 * vector.getZ() + mat23);
 vector.setX(x);
 vector.setY(y);
 vector.setW(1.0);
}

/** 
 *   Returns the (4-space) angle in radians between this vector and 
 *   the vector parameter; the return value is constrained to the 
 *   range [0,PI]. 
 *   @param v1    the other vector 
 *   @return   the angle in radians in the range [0,PI] 
 */   
 public final double angle(Vector4d v1) 
 { 
  double vDot = this.dot(v1) / ( this.length()*v1.length() );
  if( vDot < -1.0) vDot = -1.0;
  if( vDot >  1.0) vDot =  1.0;
  return((double) (Math.acos( vDot )));
 } 

public static void assertVector4dEquals(String message, Vector4d expected, Vector4d actual, double delta)
{
 assertEquals(message + " [X component]",expected.getX(), actual.getX(),delta);
 assertEquals(message + " [Y component]",expected.getY(), actual.getY(),delta);
 assertEquals(message + " [Z component]",expected.getZ(), actual.getZ(),delta);
 assertEquals(message + " [W component]",expected.getW(), actual.getW(),delta);
}

public static Vector4d generateRandomVector4d(Random random, Tuple4d lowerBound, Tuple4d upperBound)
{
 Vector4d ret = new Vector4d();
 ret.setX(generateRandomDouble(random, lowerBound.getX(), upperBound.getX()));
 ret.setY(generateRandomDouble(random, lowerBound.getY(), upperBound.getY()));
 ret.setZ(generateRandomDouble(random, lowerBound.getZ(), upperBound.getZ()));
 ret.setW(generateRandomDouble(random, lowerBound.getW(), upperBound.getW()));
 return ret;
}

  public WheelData(double backLeftX, double backLeftZ, double frontRightX, double frontRightZ) {
    bl = new Vector2d(backLeftX, backLeftZ);
    br = new Vector2d(frontRightX, backLeftZ);
    fl = new Vector2d(backLeftX, frontRightZ);
    fr = new Vector2d(frontRightX, frontRightZ);
    carVector = new Vector4d(backLeftX, backLeftZ, frontRightX, frontRightZ);
  }
}

/**
* Transform vectorIn using this transform and store result in vectorOut.
*
* @param vectorIn
* @param vectorOut
*/
public final void transform(Vector4d vectorIn, Vector4d vectorOut)
{
 if (vectorIn != vectorOut)
 {
   vectorOut.setX(mat00 * vectorIn.getX() + mat01 * vectorIn.getY() + mat02 * vectorIn.getZ() + mat03);
   vectorOut.setY(mat10 * vectorIn.getX() + mat11 * vectorIn.getY() + mat12 * vectorIn.getZ() + mat13);
   vectorOut.setZ(mat20 * vectorIn.getX() + mat21 * vectorIn.getY() + mat22 * vectorIn.getZ() + mat23);
   vectorOut.setW(1.0);
 }
 else
 {
   transform(vectorIn);
 }
}

/**
* Multiply a 4x4 matrix by a 4x1 vector. Since result is stored in vector, the matrix must be 4x4.
* @param matrix
* @param vector
*/
public static void mult(DenseMatrix64F matrix, Vector4d vector)
{
 if (matrix.numCols != 4 || matrix.numRows != 4)
 {
   throw new RuntimeException("Improperly sized matrices.");
 }
 double x = vector.getX();
 double y = vector.getY();
 double z = vector.getZ();
 double w = vector.getW();
 vector.setX(matrix.get(0, 0) * x + matrix.get(0, 1) * y + matrix.get(0, 2) * z + matrix.get(0, 3) * w);
 vector.setY(matrix.get(1, 0) * x + matrix.get(1, 1) * y + matrix.get(1, 2) * z + matrix.get(1, 3) * w);
 vector.setZ(matrix.get(2, 0) * x + matrix.get(2, 1) * y + matrix.get(2, 2) * z + matrix.get(2, 3) * w);
 vector.setW(matrix.get(3, 0) * x + matrix.get(3, 1) * y + matrix.get(3, 2) * z + matrix.get(3, 3) * w);
}