Refine search
public EuclideanTrajectoryPointMessage(EuclideanTrajectoryPointMessage trajectoryPoint) { time = trajectoryPoint.time; if (trajectoryPoint.position != null) position = new Point3d(trajectoryPoint.position); if (trajectoryPoint.linearVelocity != null) linearVelocity = new Vector3d(trajectoryPoint.linearVelocity); }
// Surface 1/3 (x, y, z - one surface for each) Vector3d edge1 = new Vector3d(); Vector3d edge2 = new Vector3d(); Vector3d normal1 = new Vector3d(); // Get the edges, the two edges must be perpendicular to one another. edge1.sub( point0, point1 ); edge2.sub( point0, point4 ); normal1.cross( edge1, edge2 ); normal1.normalize();
public Point3d get3DCoordinatesForSPLigands(IAtom refAtom, IAtomContainer withCoords, double length, double angle) { //logger.debug(" SP Ligands start "+refAtom.getPoint3d()+" "+(withCoords.getAtomAt(0)).getPoint3d()); Vector3d ca = new Vector3d(refAtom.getPoint3d()); ca.sub((withCoords.getAtom(0)).getPoint3d()); ca.normalize(); ca.scale(length); Point3d newPoint = new Point3d(refAtom.getPoint3d()); newPoint.add(ca); return newPoint; }
@Test public void positivePointPlaneDistanceTest() { // the normal for the Y-Z plane is X Vector3d planeNormal = new Vector3d(XAXIS); planeNormal.normalize(); // an arbitrary point in the Y-Z plane Point3d pointInPlane = new Point3d(0, 1, 1); // make a positive point on the X axis = same direction as the normal Point3d pointToMeasurePos = new Point3d(2, 0, 0); double distancePos = StereoTool.signedDistanceToPlane(planeNormal, pointInPlane, pointToMeasurePos); Assert.assertEquals(2.0, distancePos, 0.1); }
private void computeCoMVelocityError() { Point3d comPoint = new Point3d(); Vector3d linearVelocity = new Vector3d(); Vector3d angularMomentum = new Vector3d(); double mass = robot.computeCOMMomentum(comPoint, linearVelocity, angularMomentum); linearVelocity.scale(1.0 / mass); perfectCoMVelocity.set(linearVelocity); orientationEstimator.getEstimatedCoMVelocity(estimatedCoMVelocityFrameVector); Vector3d estimatedCoMVelocity = estimatedCoMVelocityFrameVector.getVectorCopy(); estimatedCoMVelocity.sub(linearVelocity); comVelocityError.set(estimatedCoMVelocity.length()); }
private void computePelvisVelocityError() { Vector3d actualVelocity = new Vector3d(); Vector3d linearVelocityError = new Vector3d(); estimationJoint.physics.getLinearVelocityInWorld(actualVelocity, new Vector3d()); perfectPelvisLinearVelocity.set(actualVelocity); orientationEstimator.getEstimatedPelvisLinearVelocity(estimatedPelvisVelocityFrameVector); estimatedPelvisVelocityFrameVector.get(linearVelocityError); linearVelocityError.sub(actualVelocity); pelvisLinearVelocityError.set(linearVelocityError.length()); } }
/** * Move the viewfrustum to the right (according to the view direction). * * @param delta */ public void moveRight( double delta ) { Vector3d deltaVector = new Vector3d( right ); deltaVector.scale( delta ); eye.add( deltaVector ); lookingAt.add( deltaVector ); setCameraParams( eye, lookingAt, up ); }
@Override protected boolean isInsideOrOnSurfaceShapeFrame(Point3d pointToCheck, double epsilon) { temporaryVector.set(pointToCheck.getX(), pointToCheck.getY(), 0.0); if (temporaryVector.length() < Epsilons.ONE_TRILLIONTH) { return tubeRadius >= radius; } temporaryVector.normalize(); temporaryVector.scale(radius); temporaryPoint.set(temporaryVector); return temporaryPoint.distance(pointToCheck) <= tubeRadius + epsilon; }
public static SquaredUpDRCDemo01RobotAtPlatformsInitialSetup createInitialSetupAtNthWall(int nthWall) { double alpha = ((double) nthWall)/7.0; Vector3d startingLocation = morph(firstPlatform, lastPlatform, alpha); startingLocation.add(new Vector3d(-1.0 - 0.1, 1.0 - 0.1, 0.0)); return new SquaredUpDRCDemo01RobotAtPlatformsInitialSetup(startingLocation, yaw - Math.PI/2.0); }
private void calculateCenters() { localCenter = new Point3d(boxVector); localCenter.scale(0.5); rootCenter = new Point3d(localCenter); forwardTransform.transform(rootCenter); forwardCenterTransform = new Transform3D(forwardTransform); forwardCenterTransform.mul(toTransform(new Vector3d(localCenter))); reverseCenterTransform = new Transform3D(forwardCenterTransform); reverseCenterTransform.invert(); }
private void setColumn(Twist twist, FramePoint comPositionScaledByMass, double subTreeMass, int column) { tempVector.set(comPositionScaledByMass.getPoint()); tempVector.cross(twist.getAngularPart(), tempVector); tempJacobianColumn.set(tempVector); tempVector.set(twist.getLinearPart()); tempVector.scale(subTreeMass); tempJacobianColumn.add(tempVector); jacobianMatrix.set(0, column, tempJacobianColumn.getX()); jacobianMatrix.set(1, column, tempJacobianColumn.getY()); jacobianMatrix.set(2, column, tempJacobianColumn.getZ()); } }
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); }
private double calculateAngleBetweenTwoLines(Vector3d a, Vector3d b, Vector3d c, Vector3d d) { Vector3d firstLine = new Vector3d(); firstLine.sub(a, b); Vector3d secondLine = new Vector3d(); secondLine.sub(c, d); Vector3d firstVec = new Vector3d(firstLine); Vector3d secondVec = new Vector3d(secondLine); return firstVec.angle(secondVec); }
public Vector3d getNormalizedEarthTangentVector(final double azimuth) { // TODO: rewrite this to use real math instead of this silly difference final EarthVector nextEV = findPoint(1, azimuth); final Vector3d deltaVec = new Vector3d(); deltaVec.sub(nextEV.getVector(), getVector()); deltaVec.normalize(); return deltaVec; }