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