@Test public void testTransformVector4D() throws Exception { Random random = new Random(435L); Vector4D actual = new Vector4D(); Vector4D expected = new Vector4D(); for (int i = 0; i < NUMBER_OF_ITERATIONS; i++) { RotationScaleMatrix matrix = EuclidCoreRandomTools.nextRotationScaleMatrix(random, 10.0); Vector4D original = EuclidCoreRandomTools.nextVector4D(random); matrix.transform(original, expected); actual.set(original); matrix.transform(actual); EuclidCoreTestTools.assertTuple4DEquals(expected, actual, EPS); actual.setToNaN(); matrix.transform(original, actual); EuclidCoreTestTools.assertTuple4DEquals(expected, actual, EPS); } }
public void computeAngularAcceleration(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector4DReadOnly qDDot, Vector3DBasics angularAccelerationToPack) { qConj.setAndConjugate(q); qDotConj.set(-qDot.getX(), -qDot.getY(), -qDot.getZ(), qDot.getS()); multiply(qDot, qDotConj, intermediateAngularAcceleration); multiply(qDDot, qConj, angularAccelerationToPack); angularAccelerationToPack.add(intermediateAngularAcceleration); angularAccelerationToPack.scale(2.0); }
vectorOriginal = EuclidCoreRandomTools.nextVector4D(random); Vector4D vectorOriginalCopy = new Vector4D(vectorOriginal); vectorExpected.set(0.0, 0.0, 0.0, vectorOriginal.getS()); Matrix3DTools.transform(matrix, vectorOriginal, vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); vectorExpected.set(vectorOriginal); Matrix3DTools.transform(matrix, vectorOriginal, vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); vectorActual.set(vectorOriginal); Matrix3DTools.transform(matrix, vectorActual, vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS);
@Test public void testTransformWithVector4D() throws Exception { Random random = new Random(5634L); RigidBodyTransform transform = EuclidCoreRandomTools.nextRigidBodyTransform(random); Vector4D vectorOriginal = EuclidCoreRandomTools.nextVector4D(random); Vector4D vectorExpected = new Vector4D(); Vector4D vectorActual = new Vector4D(); Vector3D vector3D = new Vector3D(vectorOriginal.getX(), vectorOriginal.getY(), vectorOriginal.getZ()); transform.transform(vector3D); vectorExpected.set(vector3D); vectorExpected.setS(vectorOriginal.getS()); vectorExpected.addX(vectorExpected.getS() * transform.getM03()); vectorExpected.addY(vectorExpected.getS() * transform.getM13()); vectorExpected.addZ(vectorExpected.getS() * transform.getM23()); transform.transform(vectorOriginal, vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); vectorActual.set(vectorOriginal); transform.transform(vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); // Try with dense-matrix DenseMatrix64F transformDenseMatrix = new DenseMatrix64F(4, 4); transform.get(transformDenseMatrix); DenseMatrix64F vectorOriginalDenseMatrix = new DenseMatrix64F(4, 1); vectorOriginal.get(vectorOriginalDenseMatrix); DenseMatrix64F vectorTransformedDenseMatrix = new DenseMatrix64F(4, 1); CommonOps.mult(transformDenseMatrix, vectorOriginalDenseMatrix, vectorTransformedDenseMatrix); vectorExpected.set(vectorTransformedDenseMatrix); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); }
@Test public void testTransformVector4D() throws Exception { Random random = new Random(34534L); RotationScaleMatrix rotationScaleMatrix = EuclidCoreRandomTools.nextRotationScaleMatrix(random, 10.0); Vector4D originalVector = EuclidCoreRandomTools.nextVector4D(random); Vector4D actualVector = new Vector4D(); Vector4D expectedVector = new Vector4D(); expectedVector.set(originalVector); expectedVector.scale(rotationScaleMatrix.getScaleX(), rotationScaleMatrix.getScaleY(), rotationScaleMatrix.getScaleZ(), 1.0); rotationScaleMatrix.getRotationMatrix().transform(expectedVector, expectedVector); rotationScaleMatrix.transform(originalVector, actualVector); EuclidCoreTestTools.assertTuple4DEquals(expectedVector, actualVector, EPS); }
@Test public void testTransformWithVector4D() throws Exception { Random random = new Random(5634L); AffineTransform transform = EuclidCoreRandomTools.nextAffineTransform(random); Vector4D vectorOriginal = EuclidCoreRandomTools.nextVector4D(random); Vector4D vectorExpected = new Vector4D(); Vector4D vectorActual = new Vector4D(); Vector3D vector3D = new Vector3D(vectorOriginal.getX(), vectorOriginal.getY(), vectorOriginal.getZ()); transform.transform(vector3D); vectorExpected.set(vector3D); vectorExpected.setS(vectorOriginal.getS()); vectorExpected.addX(vectorExpected.getS() * transform.getM03()); vectorExpected.addY(vectorExpected.getS() * transform.getM13()); vectorExpected.addZ(vectorExpected.getS() * transform.getM23()); transform.transform(vectorOriginal, vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); vectorActual.set(vectorOriginal); transform.transform(vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); // Try with dense-matrix DenseMatrix64F transformDenseMatrix = new DenseMatrix64F(4, 4); transform.get(transformDenseMatrix); DenseMatrix64F vectorOriginalDenseMatrix = new DenseMatrix64F(4, 1); vectorOriginal.get(vectorOriginalDenseMatrix); DenseMatrix64F vectorTransformedDenseMatrix = new DenseMatrix64F(4, 1); CommonOps.mult(transformDenseMatrix, vectorOriginalDenseMatrix, vectorTransformedDenseMatrix); vectorExpected.set(vectorTransformedDenseMatrix); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); }
@Test public void testInverseTransformVector4D() throws Exception { Random random = new Random(435L); Vector4D actual = new Vector4D(); Vector4D expected = new Vector4D(); for (int i = 0; i < NUMBER_OF_ITERATIONS; i++) { RotationScaleMatrix matrix = EuclidCoreRandomTools.nextRotationScaleMatrix(random, 10.0); Vector4D original = EuclidCoreRandomTools.nextVector4D(random); matrix.inverseTransform(original, expected); actual.set(original); matrix.inverseTransform(actual); EuclidCoreTestTools.assertTuple4DEquals(expected, actual, EPS); actual.setToNaN(); matrix.inverseTransform(original, actual); EuclidCoreTestTools.assertTuple4DEquals(expected, actual, EPS); } }
@Test public void testInverseTransformWithVector4D() throws Exception { Random random = new Random(3454L); T transform = createRandomTransform(random); { // Test inverseTransform(Vector4DBasics vectorToTransform) Vector4D expected = EuclidCoreRandomTools.nextVector4D(random); Vector4D actual = new Vector4D(); actual.set(expected); transform.transform(actual); transform.inverseTransform(actual); EuclidCoreTestTools.assertTuple4DEquals(expected, actual, EPS); } { // Test inverseTransform(Vector4DReadOnly vectorOriginal, Vector4DBasics vectorTransformed) Vector4D expected = EuclidCoreRandomTools.nextVector4D(random); Vector4D actual = new Vector4D(); transform.inverseTransform(expected, actual); transform.transform(actual); EuclidCoreTestTools.assertTuple4DEquals(expected, actual, EPS); } }
Vector3D vector3D = new Vector3D(vectorOriginal.getX(), vectorOriginal.getY(), vectorOriginal.getZ()); QuaternionTools.transform(quaternion, vector3D, vector3D); vectorExpected.set(vector3D); vectorExpected.setS(vectorOriginal.getS()); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPSILON); vectorActual.set(vectorOriginal); QuaternionTools.transform(quaternion, vectorActual, vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPSILON);
@Test public void testTransformVector4D() throws Exception { Random random = new Random(435L); Vector4D actual = new Vector4D(); Vector4D expected = new Vector4D(); for (int i = 0; i < ITERATIONS; i++) { RotationMatrix matrix = EuclidCoreRandomTools.nextRotationMatrix(random); Vector4D original = EuclidCoreRandomTools.nextVector4D(random); matrix.transform(original, expected); actual.set(original); matrix.transform(actual); EuclidCoreTestTools.assertTuple4DEquals(expected, actual, EPS); actual.setToNaN(); matrix.transform(original, actual); EuclidCoreTestTools.assertTuple4DEquals(expected, actual, EPS); } }
Quaternion qResult = new Quaternion(); QuaternionTools.multiply(q1, q2, qResult); vectorExpected.set(qResult); Quaternion qResult = new Quaternion(); QuaternionTools.multiplyConjugateLeft(q1, q2, qResult); vectorExpected.set(qResult); Quaternion qResult = new Quaternion(); QuaternionTools.multiplyConjugateRight(q1, q2, qResult); vectorExpected.set(qResult);
d1B1Dot.set(d1); d2B2Dot.set(d2); d3B3Dot.set(d3); d1B1Dot.scale(0.5 * cumulativeBeziersDot[1].getDoubleValue()); d2B2Dot.scale(0.5 * cumulativeBeziersDot[2].getDoubleValue()); d3B3Dot.scale(0.5 * cumulativeBeziersDot[3].getDoubleValue()); d1B1DDot.set(d1); d2B2DDot.set(d2); d3B3DDot.set(d3); d1B1DDot.scale(0.5 * cumulativeBeziersDDot[1].getDoubleValue()); d2B2DDot.scale(0.5 * cumulativeBeziersDDot[2].getDoubleValue()); QuaternionTools.multiply(expD1B1_expD2B2, qDDot2, qDDotTemp); QuaternionTools.multiply(qDDotTemp, expD3B3, qDDotTemp); qDDot2.set(qDDotTemp); QuaternionTools.multiply(d1B1Dot, qDot2, qDDotTemp); qDDot2.add(qDDotTemp);
pureQuaternion.set(tupleOriginal.getX(), tupleOriginal.getY(), tupleOriginal.getZ(), 0.0);