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); }
/** * Pack the variance along each principal axis in the given Vector3d. * * @param principalVarianceToPack The variance is stored in the Vector3d as follows: x is the * variance on the principal axis, y on the secondary axis, and z on the third axis. */ public void getVariance(Vector3D principalVarianceToPack) { principalVarianceToPack.setX(variance.getX()); principalVarianceToPack.setY(variance.getY()); principalVarianceToPack.setZ(variance.getZ()); }
private void addBlock(double yaw, Vector3D position, Vector3D dimensions, AppearanceDefinition color) { AxisAngle orientation = new AxisAngle(new Vector3D(0.0, 0.0, 1.0), yaw); RigidBodyTransform blockPose = new RigidBodyTransform(orientation, position); Box3D block = new Box3D(blockPose, dimensions.getX(), dimensions.getY(), dimensions.getZ()); terrainObject.addTerrainObject(new RotatableBoxTerrainObject(block, color)); }
public static void applyTranform(Transform transform, Vector3D vectorToTransform) { javafx.geometry.Point3D temporaryVector = transform.deltaTransform(vectorToTransform.getX(), vectorToTransform.getY(), vectorToTransform.getZ()); vectorToTransform.set(temporaryVector.getX(), temporaryVector.getY(), temporaryVector.getZ()); }
private void computeInitialConstraintError(FramePoint3DReadOnly initialPosition, double initialTime) { trajectory.compute(initialTime); trajectoryFrame.checkReferenceFrameMatch(initialPosition.getReferenceFrame()); trajectory.getPosition(tempPosition); tempPosition.changeFrame(trajectoryFrame); initialConstraintPositionError.set(initialPosition); initialConstraintPositionError.sub(tempPosition); }
@Override public void perturb(Vector3D direction) { Vector3D ballVelocity = new Vector3D(direction); if (ballVelocity.lengthSquared() > 0.0) { ballVelocity.normalize(); ballVelocity.scale(ballVelocityMagnitude); collidable.handleCollision(ballVelocity, getBallMass(), coefficientOfRestitution); } }
protected Footstep createFootstepPlacedAtBipedfootLocation(RobotSide side) { ReferenceFrame soleFrame = soleFrames.get(side); Vector3D translation = new Vector3D(); Quaternion rotation = new Quaternion(); soleFrame.getTransformToWorldFrame().getTranslation(translation); soleFrame.getTransformToWorldFrame().getRotation(rotation); FramePose2D solePose2d = new FramePose2D(soleFrame, new Point2D(translation.getX(), translation.getY()), rotation.getYaw()); Footstep foot = createFootstep(side, solePose2d); return foot; }
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; }
private static Vector3D attachParentJointToDistalEndOfCylinder(Vector3D cylinderZAxisInWorld, double length) { Vector3D parentJointOffsetFromCoM = new Vector3D(cylinderZAxisInWorld); parentJointOffsetFromCoM.normalize(); parentJointOffsetFromCoM.scale(-length / 2.0); return parentJointOffsetFromCoM; }
public static Vector3D nextVector3D(Random random, Tuple3DReadOnly boundary1, Tuple3DReadOnly boundary2) { Vector3D ret = new Vector3D(); ret.setX(RandomNumbers.nextDouble(random, boundary1.getX(), boundary2.getX())); ret.setY(RandomNumbers.nextDouble(random, boundary1.getY(), boundary2.getY())); ret.setZ(RandomNumbers.nextDouble(random, boundary1.getZ(), boundary2.getZ())); return ret; }
protected void setPositionInWorld(HumanoidFloatingRootJointRobot robot) { robot.update(); robot.getRootJointToWorldTransform(rootToWorld); rootToWorld.getTranslation(offset); Vector3D positionInWorld = new Vector3D(); rootToWorld.getTranslation(positionInWorld); GroundContactPoint gc1 = robot.getFootGroundContactPoints(RobotSide.LEFT).get(0); double pelvisToFoot = positionInWorld.getZ() - gc1.getPositionPoint().getZ(); offset.setZ(groundZ + pelvisToFoot); robot.setPositionInWorld(offset); }
private Vector3DReadOnly convertToAngularAcceleration(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector4DReadOnly qDDot) { // w = qDDot * q^-1 + qDot * qDot^-1 QuaternionTools.multiplyConjugateRight(qDDot, q, tempConvertVector4D); tempConvertVector3D.setX(tempConvertVector4D.getX()); tempConvertVector3D.setY(tempConvertVector4D.getY()); tempConvertVector3D.setZ(tempConvertVector4D.getZ()); QuaternionTools.multiplyConjugateRight(qDot, qDot, tempConvertVector4D); tempConvertVector3D.addX(tempConvertVector4D.getX()); tempConvertVector3D.addY(tempConvertVector4D.getY()); tempConvertVector3D.addZ(tempConvertVector4D.getZ()); tempConvertVector3D.scale(2.0); return tempConvertVector3D; }