public static void integrateAngularVelocity(Vector3DReadOnly angularVelocityToIntegrate, double integrationTime, AxisAngleBasics orientationResultToPack) { Vector3D angularVelocityIntegrated = angularVelocityForIntegrator.get(); angularVelocityIntegrated.set(angularVelocityToIntegrate); angularVelocityIntegrated.scale(integrationTime); AxisAngleConversion.convertRotationVectorToAxisAngle(angularVelocityIntegrated, orientationResultToPack); }
private void computeDrift(double time, double alphaDecay, YoFrameVector3D angularVelocity, Quaternion driftToPack) { tempAngularVelocityForDrift.set(angularVelocity); tempAngularVelocity.scale(alphaDecay); RotationTools.integrateAngularVelocity(tempAngularVelocityForDrift, time, driftToPack); }
private Vector3D getDirection(Point3D a, Point3D b) { Vector3D direction = new Vector3D(b); direction.sub(a); direction.scale(0.5); return direction; }
public void bounceAwayAfterCollision() { this.launched = false; velocityVector.scale(-1.0); final double zVelocityAfterBounce = -10.0; velocityVector.setZ(zVelocityAfterBounce); setVelocity(velocityVector); }
private static Vector3D attachParentJointToDistalEndOfCylinder(Vector3D cylinderZAxisInWorld, double length) { Vector3D parentJointOffsetFromCoM = new Vector3D(cylinderZAxisInWorld); parentJointOffsetFromCoM.normalize(); parentJointOffsetFromCoM.scale(-length / 2.0); return parentJointOffsetFromCoM; }
private void addPointsContribution(Point3D pointToContribute, Point3D pointOnAToPack, Point3D pointOnBToPack) { double lambda = lambdas.get(pointToContribute); tempVector1.set(simplexPointToPolytopePointA.get(pointToContribute)); tempVector1.scale(lambda); pointOnAToPack.add(tempVector1); tempVector1.set(simplexPointToPolytopePointB.get(pointToContribute)); tempVector1.scale(lambda); pointOnBToPack.add(tempVector1); }
@Override public void doControl() { double mass = robot.computeCOMMomentum(tempCenterOfMassPoint, tempCenterOfMassVelocity, tempAngularMomentum); exactCenterOfMassPosition.set(tempCenterOfMassPoint); tempCenterOfMassVelocity.scale(1.0 / mass); exactCenterOfMassVelocity.set(tempCenterOfMassVelocity); exactCenterOfMassAcceleration.update(); }
private Vector3D getPerpendicularVectorOfLength(double width, Vector3D direction) { Vector3D toSide = new Vector3D(-direction.getY(), direction.getX(), 0); toSide.normalize(); toSide.scale(width/2.0); return toSide; }
private void updatePointsAndVectors(Point3DReadOnly initialPosition, Point3DReadOnly finalPosition, double velocityMagnitude) { this.finalPosition.set(finalPosition); directionVector.set(finalPosition); directionVector.sub(initialPosition); directionVector.normalize(); velocityVector.set(directionVector); velocityVector.scale(velocityMagnitude); }
private static Vector3D axisAngleToVector(AxisAngle axisAngle) { Vector3D ret = new Vector3D(axisAngle.getX(), axisAngle.getY(), axisAngle.getZ()); ret.scale(axisAngle.getAngle()); return ret; }
public boolean isInVoronoiRegionOfVertex(Point3D pointToCheck, Point3D otherPoint1, Point3D otherPoint2) { tempVector1.set(pointToCheck); tempVector1.scale(-1.0); tempVector2.sub(otherPoint1, pointToCheck); if (!(tempVector1.dot(tempVector2) <= 0.0)) return false; tempVector2.sub(otherPoint2, pointToCheck); if (!(tempVector1.dot(tempVector2) <= 0.0)) return false; return true; }
public void computeSupportPointOnMinkowskiDifference(ConvexPolytope cubeOne, ConvexPolytope cubeTwo, Vector3D supportDirection, Point3D supportPoint) { // Because everything is linear and convex, the support point on the Minkowski difference is s_{a minkowskidiff b}(d) = s_a(d) - s_b(-d) Point3D supportingVertexOne = cubeOne.getSupportingVertex(supportDirection); negativeSupportDirection.set(supportDirection); negativeSupportDirection.scale(-1.0); Point3D supportingVertexTwo = cubeTwo.getSupportingVertex(negativeSupportDirection); supportPoint.set(supportingVertexOne); supportPoint.sub(supportingVertexTwo); }
private static void setTranslationSettingZAndPreservingXAndY(Point3D highestVertex, RigidBodyTransform transformToReturn) { Vector3D newTranslation = new Vector3D(highestVertex.getX(), highestVertex.getY(), 0.0); transformToReturn.transform(newTranslation); newTranslation.scale(-1.0); newTranslation.add(highestVertex); transformToReturn.setTranslation(newTranslation); }
@Override public void perturb(Vector3D direction) { Vector3D force = new Vector3D(direction); if (direction.lengthSquared() > 0.0) { force.normalize(); force.scale(disturbanceMagnitude.getDoubleValue()); forcePerturbable.setForcePerturbance(force, disturbanceDuration.getDoubleValue()); } }
public void packAngularVelocityTermForOrientation(DenseMatrix64F block, QuaternionReadOnly orientation, Vector3DReadOnly linearVelocity, double dt) { orientation.get(rotationMatrix); tempLinearVelocity1.set(linearVelocity); tempLinearVelocity1.scale(dt); packJacobianOfExponentialMap(result, tempLinearVelocity1); result.preMultiply(rotationMatrix); result.scale(dt); result.get(block); }
public BodyPathMeshViewer(Messager messager) { isExecutorServiceProvided = executorService == null; bodyPathMeshView.setMouseTransparent(true); bodyPathMeshView.setMaterial(new PhongMaterial(Color.YELLOW)); Vector3D defaultSize = new Vector3D(1.0, 1.0, 1.0); defaultSize.scale(1.5 * BODYPATH_LINE_THICKNESS); show = messager.createInput(FootstepPlannerMessagerAPI.ShowBodyPath, true); messager.registerTopicListener(FootstepPlannerMessagerAPI.BodyPathDataTopic, this::processBodyPathOnThread); messager.registerTopicListener(FootstepPlannerMessagerAPI.ComputePathTopic, data -> reset.set(true)); root.getChildren().addAll(bodyPathMeshView); }
private void visualizeNormalVector(Plane3D plane) { this.linkGraphics.identity(); linkGraphics.translate(new Vector3D(plane.getPointCopy())); linkGraphics.addSphere(0.005, YoAppearance.Black()); Vector3D normalCopy = plane.getNormalCopy(); normalCopy.scale(0.01); linkGraphics.translate(normalCopy); linkGraphics.addSphere(0.005, YoAppearance.Blue()); }
@Override public void updateAllGroundContactPointVelocities() { RigidBodyTransform sliderJointTransform = new RigidBodyTransform(); RigidBodyTransform newButtonPose = new RigidBodyTransform(); buttonPushVector.scale(buttonSliderJoint.getQYoVariable().getDoubleValue()); sliderJointTransform.setTranslationAndIdentityRotation(buttonPushVector); buttonPushVector.normalize(); newButtonPose.set(originalButtonTransform); newButtonPose.multiply(sliderJointTransform); buttonFrame.setPoseAndUpdate(newButtonPose); super.updateAllGroundContactPointVelocities(); }