public static Vector3D stringToNormalizedVector3d(String vector) { Vector3D vector3d = stringToVector3d(vector); vector3d.normalize(); return vector3d; }
default public void packVector(List<Point2D> contactPoints, int contactIdx, int vectors, int vectorIdx, double friction, Vector3D directionToPack) { double angleOffset = computeYawOffset(contactPoints, contactIdx, vectors, vectorIdx); double angle = angleOffset + 2.0 * Math.PI * vectorIdx / vectors; double x = Math.cos(angle) * friction; double y = Math.sin(angle) * friction; directionToPack.set(x, y, 1.0); directionToPack.normalize(); }
private int addVertex(Point3D point) { Vector3D vector = new Vector3D(point); vector.normalize(); point = new Point3D(vector); geometry.positions.add(point); return index++; }
public void getRay(Vector3D rayToPack, int rayIndex) { MathTools.checkIntervalContains(rayIndex, 0, numberOfRays - 1); rayToPack.sub(sphereOrigin, pointsOnSphere[rayIndex]); rayToPack.normalize(); }
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 generateRandomVector3D(Random random, double magnitude) { Vector3D ret = generateRandomVector3D(random, 1.0, 1.0, 1.0); ret.normalize(); ret.scale(magnitude); return ret; }
public static Vector3D nextVector3D(Random random, double length) { Vector3D ret = nextVector3D(random); ret.normalize(); ret.scale(length); return ret; }
public Color getNormalBasedColor(Vector3D normal, boolean isNormalSet) { Color color = DEFAULT_COLOR; if (isNormalSet) { Vector3D zUp = new Vector3D(0.0, 0.0, 1.0); normal.normalize(); double angle = Math.abs(zUp.dot(normal)); double hue = 120.0 * angle; color = Color.hsb(hue, 1.0, 1.0); } return color; }
@Override public GroundProfile3D getGroundProfile() { Vector3D surfaceNormal = new Vector3D(0.03, 0.27, 1.0); surfaceNormal.normalize(); Point3D intersectionPoint = new Point3D(0.22, 2.2, -0.4); double maxXY = 10.0; return new SlopedPlaneGroundProfile(surfaceNormal, intersectionPoint, maxXY); }
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); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { xAxis.set(positionToPointAt); xAxis.setZ(0.0); xAxis.normalize(); yAxis.cross(zAxis, xAxis); rotationMatrix.setColumns(xAxis, yAxis, zAxis); transformToParent.setRotation(rotationMatrix); } }
@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); } }
@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()); } }
private QuadTreeTestHelper testOnASlope(Point3D center, Vector3D normal, double halfWidth, double resolution, boolean visualize) { normal.normalize(); BoundingBox2D boundingBox = new BoundingBox2D(center.getX() - halfWidth, center.getY() - halfWidth, center.getX() + halfWidth, center.getY() + halfWidth); Plane3D plane3d = new Plane3D(center, normal); ArrayList<Point3D> points = generatePointsForSlope(plane3d, halfWidth, resolution); int pointsPerBallUpdate = 1; return testOnAListOfPoints(points, pointsPerBallUpdate, boundingBox, resolution, visualize); }
private void testOnAStaircase(Point3D center, Vector3D normal, double halfWidth, double resolution, double stairSeparation, double oneStairLandingHeight) { normal.normalize(); BoundingBox2D boundingBox = new BoundingBox2D(center.getX() - halfWidth, center.getY() - halfWidth, center.getX() + halfWidth, center.getY() + halfWidth); Plane3D plane3d = new Plane3D(center, normal); ArrayList<Point3D> points = generatePointsForStairs(plane3d, halfWidth, resolution, stairSeparation, oneStairLandingHeight); // Collections.shuffle(points); testOnAListOfPoints(points, boundingBox, resolution); }
private void testOnASlope(Point3D center, Vector3D normal, double halfWidth, double resolution) { normal.normalize(); BoundingBox2D boundingBox = new BoundingBox2D(center.getX() - halfWidth, center.getY() - halfWidth, center.getX() + halfWidth, center.getY() + halfWidth); Plane3D plane3d = new Plane3D(center, normal); ArrayList<Point3D> points = generatePointsForSlope(plane3d, halfWidth, resolution); testOnAListOfPoints(points, boundingBox, resolution); }
private void testSurfaceNormalsForAnyRampFace(RotatableRampTerrainObject ramp, Vector3D expectedRampSurfaceNormal, Point3D[] pointsOnRamp) { expectedRampSurfaceNormal.normalize(); for (int i = 0; i < pointsOnRamp.length; i++) { Vector3D normal = new Vector3D(); ramp.surfaceNormalAt(pointsOnRamp[i].getX(), pointsOnRamp[i].getY(), pointsOnRamp[i].getZ(), normal); String message = "Normal for point " + pointsOnRamp[i].getX() + " " + pointsOnRamp[i].getY() + " " + pointsOnRamp[i].getZ(); EuclidCoreTestTools.assertTuple3DEquals(message, expectedRampSurfaceNormal, normal, epsilon); } }
@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(); }
public static RigidBodyTransform opencvTRtoRigidBodyTransform(Mat rvec, Mat tvec) { Vector3D translation = new Vector3D(tvec.get(0, 0)[0], tvec.get(1, 0)[0], tvec.get(2, 0)[0]); Vector3D axis = new Vector3D(rvec.get(0, 0)[0], rvec.get(1, 0)[0], rvec.get(2, 0)[0]); double angle = axis.length(); axis.normalize(); AxisAngle rotation = new AxisAngle(axis, angle); RigidBodyTransform transform = new RigidBodyTransform(rotation, translation); return transform; }