public static Vector3D nextOrthogonalVector3D(Random random, Vector3DReadOnly vectorToBeOrthogonalTo, boolean normalize) { Vector3D v1 = new Vector3D(vectorToBeOrthogonalTo.getY(), -vectorToBeOrthogonalTo.getX(), 0.0); Vector3D v2 = new Vector3D(-vectorToBeOrthogonalTo.getZ(), 0.0, vectorToBeOrthogonalTo.getX()); Vector3D randomPerpendicular = new Vector3D(); double a = RandomNumbers.nextDouble(random, 1.0); double b = RandomNumbers.nextDouble(random, 1.0); randomPerpendicular.scaleAdd(a, v1, randomPerpendicular); randomPerpendicular.scaleAdd(b, v2, randomPerpendicular); if (normalize) randomPerpendicular.normalize(); return randomPerpendicular; }
public static Vector3D generateRandomOrthogonalVector3D(Random random, Vector3DReadOnly vectorToBeOrthogonalTo, boolean normalize) { Vector3D v1 = new Vector3D(vectorToBeOrthogonalTo.getY(), - vectorToBeOrthogonalTo.getX(), 0.0); Vector3D v2 = new Vector3D(- vectorToBeOrthogonalTo.getZ(), 0.0, vectorToBeOrthogonalTo.getX()); Vector3D randomPerpendicular = new Vector3D(); double a = JOctoMapRandomTools.generateRandomDouble(random, 1.0); double b = JOctoMapRandomTools.generateRandomDouble(random, 1.0); randomPerpendicular.scaleAdd(a, v1, randomPerpendicular); randomPerpendicular.scaleAdd(b, v2, randomPerpendicular); if (normalize) randomPerpendicular.normalize(); return randomPerpendicular; }
deltaPosition.scaleAdd(half_dt_dt, linearAcceleration, deltaPosition);
rotationVector.scaleAdd(half_dt_dt, angularAcceleration, rotationVector); integrated.setRotationVector(rotationVector); finalOrientation.set(initialOrientation);
public void update(Vector3DReadOnly inputAngularVelocity, Vector3DReadOnly inputLinearAcceleration, Vector3DReadOnly inputMagneticVector) { if (!hasBeenInitialized.getValue()) { initialize(inputLinearAcceleration, inputMagneticVector); return; } boolean success = computeOrientationError((QuaternionReadOnly) estimatedOrientation, inputLinearAcceleration, inputMagneticVector, quaternionUpdate); if (success) { quaternionUpdate.getRotationVector(totalError); yoErrorTerm.set(totalError); integralTerm.scaleAdd(integralGain.getValue() * updateDT, yoErrorTerm, yoIntegralTerm); yoIntegralTerm.set(integralTerm); angularVelocityTerm.scaleAdd(proportionalGain.getValue(), totalError, inputAngularVelocity); angularVelocityTerm.add(integralTerm); } else { yoErrorTerm.setToZero(); angularVelocityTerm.set(inputAngularVelocity); } rotationUpdate.setAndScale(updateDT, angularVelocityTerm); quaternionUpdate.setRotationVector(rotationUpdate); estimatedOrientation.multiply(quaternionUpdate); if (estimatedAngularVelocity != null) estimatedAngularVelocity.add(inputAngularVelocity, integralTerm); }
/** * Computes the moment of inertia matrix for a solid cylinder. * * @param mass the cylinder mass. * @param radius the radius of the cylinder. * @param height the height, or length, of the cylinder. * @param axisOfCylinder the revolution axis of the cylinder. Not modified. * @return the moment of inertia of the cylinder. */ public static Matrix3D solidCylinder(double mass, double radius, double height, Vector3DReadOnly axisOfCylinder) { checkMassAndDimensions(mass, radius, height); double IalongAxis = 0.5 * mass * radius * radius; double IcrossAxis = mass * (3.0 * radius * radius + height * height) / 12.0; Vector3D principalInertia = new Vector3D(1.0, 1.0, 1.0); principalInertia.sub(axisOfCylinder); principalInertia.scale(IcrossAxis); principalInertia.scaleAdd(IalongAxis, axisOfCylinder, principalInertia); Matrix3D momentOfInertia = new Matrix3D(); momentOfInertia.setM00(principalInertia.getX()); momentOfInertia.setM11(principalInertia.getY()); momentOfInertia.setM22(principalInertia.getZ()); return momentOfInertia; }
offset.scaleAdd(((double) j) / pathPoints, point);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeInterpolationForTranslation() throws Exception { RotationMatrix maxtrixIdentity = new RotationMatrix(); maxtrixIdentity.setIdentity(); Vector3D vector1 = new Vector3D(0.0, 0.0, 0.0); Vector3D vector2 = new Vector3D(5.0, 8.0, 10.0); RigidBodyTransform t1 = new RigidBodyTransform(maxtrixIdentity, vector1); RigidBodyTransform t2 = new RigidBodyTransform(maxtrixIdentity, vector2); RigidBodyTransform t3 = new RigidBodyTransform(); transformInterpolationCalculator.computeInterpolation(t1, t2, t3, 0.0); Vector3D interpolatedVector = new Vector3D(); t3.getTranslation(interpolatedVector); assertTrue(vector1.epsilonEquals(interpolatedVector, 1e-8)); transformInterpolationCalculator.computeInterpolation(t1, t2, t3, 1.0); interpolatedVector = new Vector3D(); t3.getTranslation(interpolatedVector); assertTrue(vector2.epsilonEquals(interpolatedVector, 1e-8)); double alpha = 0.25; transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); interpolatedVector = new Vector3D(); t3.getTranslation(interpolatedVector); Vector3D expectedVector = new Vector3D(); expectedVector.scaleAdd((1- alpha), vector1, expectedVector); expectedVector.scaleAdd(alpha, vector2, expectedVector); assertTrue(expectedVector.epsilonEquals(interpolatedVector, 1e-8)); }
Vector3D excursionVector = new Vector3D(alongDirectionOne); excursionVector.scale(excursionVector2d.getX()); excursionVector.scaleAdd(excursionVector2d.getY(), alongDirectionTwo, excursionVector);
/** * Computes and sets the camera rotation for given camera position and a given point to focus on. * @param cameraPosition desired camera position. Not modified. * @param focusPoint desired focus position. Not modified. * @param cameraRoll desired camera roll. */ public void setRotationFromCameraAndFocusPositions(Point3D cameraPosition, Point3D focusPoint, double cameraRoll) { Vector3D fromFocusToCamera = new Vector3D(); fromFocusToCamera.sub(cameraPosition, focusPoint); fromFocusToCamera.normalize(); Vector3D fromCameraToFocus = new Vector3D(); fromCameraToFocus.setAndNegate(fromFocusToCamera); // We remove the component along up to be able to compute the longitude fromCameraToFocus.scaleAdd(-fromCameraToFocus.dot(up), up, fromCameraToFocus); double newLatitude = Math.PI / 2.0 - fromFocusToCamera.angle(up); double newLongitude = fromCameraToFocus.angle(forward); Vector3D cross = new Vector3D(); cross.cross(fromCameraToFocus, forward); if (cross.dot(up) > 0.0) newLongitude = -newLongitude; latitude.set(newLatitude); longitude.set(newLongitude); roll.set(cameraRoll); updateRotation(); }