vec.scale(strength);
public boolean isSolvable(double trajectoryTime, Vector3d omegaA, Vector3d omegaB) { omegaA.scale(trajectoryTime / 3); boolean omegaALength = omegaA.length() < 2 * Math.PI; omegaB.scale(trajectoryTime / 3); boolean omegaBLength = (omegaB.length() < 2 * Math.PI); omegaB.scale(-1); boolean opposite = omegaA.epsilonEquals(omegaB, 1e-2); return omegaALength && omegaBLength && !opposite; }
public static void integrateAngularVelocity(Vector3d angularVelocityToIntegrate, double integrationTime, AxisAngle4d orientationResultToPack) { Vector3d angularVelocityIntegrated = angularVelocityForIntegrator.get(); angularVelocityIntegrated.set(angularVelocityToIntegrate); angularVelocityIntegrated.scale(integrationTime); convertRotationVectorToAxisAngle(angularVelocityIntegrated, orientationResultToPack); }
public static Vector3d generateRandomVector(Random random, double length) { Vector3d ret = generateRandomVector(random); ret.normalize(); ret.scale(length); return ret; }
public void setCenterOfMassOffset(FramePoint centerOfMassOffset) { expressedInframe.checkReferenceFrameMatch(centerOfMassOffset); centerOfMassOffset.get(crossPart); crossPart.scale(-mass); // c = - m * comOffset }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { tempTranslation.set(axis); tempTranslation.scale(position); transformToParent.setTranslationAndIdentityRotation(tempTranslation); } }
private void computeBezierQuaternionCurveTerm(int i, Quat4d resultToPack) { double cumulativeBernsteinCoefficient = cumulativeBeziers[i].getDoubleValue(); controlAngularVelocities[i].get(tempAngularVelocity); tempAngularVelocity.scale(cumulativeBernsteinCoefficient); quaternionCalculus.exp(tempAngularVelocity, resultToPack); }
@Override public Vector3d getDimension() { run(); Vector3d dimension = new Vector3d(); dimension.sub(maxBoundary, minBoundary); dimension.scale(0.5); return dimension; }
public void limitLinearPartMagnitude(double maximumMagnitude) { if (maximumMagnitude < 1e-7) linearPart.set(0.0, 0.0, 0.0); if (linearPart.lengthSquared() > maximumMagnitude * maximumMagnitude) { linearPart.scale(maximumMagnitude/linearPart.length()); } }
private void computeOrientationStateMatrixBlock() { angularVelocityPort.getData().get(angularVelocity); angularVelocity.scale(-0.5); MatrixTools.toTildeForm(tempMatrix3d, angularVelocity); MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix3d, stateMatrixBlocks.get(orientationPort)); }
public void limitAngularPartMagnitude(double maximumMagnitude) { if (maximumMagnitude < 1e-7) angularPart.set(0.0, 0.0, 0.0); if (angularPart.lengthSquared() > maximumMagnitude * maximumMagnitude) { angularPart.scale(maximumMagnitude/angularPart.length()); } }
@Override protected void orthogonalProjectionShapeFrame(Point3d point) { temporaryVector.set(point); double distance = temporaryVector.length(); if (distance >= Epsilons.ONE_TRILLIONTH) { temporaryVector.normalize(); temporaryVector.scale(radius); point.set(temporaryVector); } }
@Override protected boolean checkIfInsideShapeFrame(Point3d pointInWorldToCheck, Point3d closestPointToPack, Vector3d normalToPack) { boolean isInside = isInsideOrOnSurfaceShapeFrame(pointInWorldToCheck, Epsilons.ONE_TRILLIONTH); surfaceNormalAt(pointInWorldToCheck, normalToPack); temporaryVector.set(normalToPack); temporaryVector.scale(radius); closestPointToPack.set(temporaryVector); return isInside; }
@Override protected void orthogonalProjectionShapeFrame(Point3d pointToCheckAndPack) { computeCompositeVectorsForPoint(originToRadiusTemporaryVector, tubeCenterToPointTemporaryVector, pointToCheckAndPack); if (tubeCenterToPointTemporaryVector.length() > tubeRadius) tubeCenterToPointTemporaryVector.scale(tubeRadius / tubeCenterToPointTemporaryVector.length()); tubeCenterToPointTemporaryVector.add(originToRadiusTemporaryVector); pointToCheckAndPack.set(tubeCenterToPointTemporaryVector); }
public void computeAngularAcceleration(Quat4d q, Quat4d qDot, Quat4d qDDot, Vector3d angularAccelerationToPack) { qConj.conjugate(q); qDotConj.conjugate(qDot); multiply(qDot, qDotConj, intermediateAngularAcceleration); multiply(qDDot, qConj, angularAccelerationToPack); angularAccelerationToPack.add(intermediateAngularAcceleration); angularAccelerationToPack.scale(2.0); }
public boolean isPointNearWheel(double x, double y, double z, double fudgeFactor) { tempVector.set(x, y, z); tempVector3.set(1.0, 0.0, 0.0); double dotProduct = tempVector.dot(tempVector3); tempVector2.set(tempVector3); tempVector2.scale(dotProduct); tempVector.sub(tempVector2); double radius = tempVector.length(); return (dotProduct > -BOUNDING_WHEEL_NEAR_X) && (dotProduct < BOUNDING_WHEEL_FAR_X) && (radius < BOUNDING_OUTER_WHEEL_RADIUS - fudgeFactor) && (radius > BOUNDING_INNER_WHEEL_RADIUS + fudgeFactor); }