private void setVerticalTranslationVector(){ double y = Math.cos(Math.toRadians(this.rotation.y)); double yz = Math.sin(Math.toRadians(this.rotation.y)); translationVectorV = new Vector3d(0, y, yz); translationVectorV.normalize(); }
private void setHorizontalTranslationVector() { double x = Math.cos(Math.toRadians(this.rotation.x)); double xz = Math.sin(Math.toRadians(this.rotation.x)); double y = xz * Math.sin(Math.toRadians(this.rotation.y)); double yz = xz * Math.cos(Math.toRadians(this.rotation.y)); translationVectorH = new Vector3d(x, y, yz); translationVectorH.normalize(); }
/** * Initializes the components of the spatial acceleration vector to zero * @param expressedInFrame the frame in which the spatial acceleration vector is expressed */ public SpatialForceVector(ReferenceFrame expressedInFrame) { this.expressedInFrame = expressedInFrame; this.linearPart = new Vector3d(); this.angularPart = new Vector3d(); }
/** Normalize the ECF vector */ public Vector3d getUnitVector() { final Vector3d unitVec = new Vector3d(ecfVector); unitVec.normalize(); return unitVec; }
public Vector3d getNormalCopy() { Vector3d normalToReturn = new Vector3d(); this.getNormal(normalToReturn); return normalToReturn; }
public Vector3d getLinearVelocityCopy() { Vector3d linearVelocityCopy = new Vector3d(); getLinearVelocity(linearVelocityCopy); return linearVelocityCopy; }
private void calcRotAxesAndAngles() { axisAngles = new AxisAngle4d[multiplicity]; // identity operator (transformId==0) axisAngles[0] = new AxisAngle4d(new Vector3d(0,0,0), 0.0); for (int i=1;i<this.transformations.size();i++){ Matrix3d r = new Matrix3d(transformations.get(i).m00,transformations.get(i).m01,transformations.get(i).m02, transformations.get(i).m10,transformations.get(i).m11,transformations.get(i).m12, transformations.get(i).m20,transformations.get(i).m21,transformations.get(i).m22); axisAngles[i] = getRotAxisAndAngle(r); } }
private void initialize() { // translation to centered coordinate system centroid = new Vector3d(subunits.getCentroid()); // translation back to original coordinate system Vector3d reverse = new Vector3d(centroid); reverse.negate(); centroidInverse.set(reverse); // // On LINUX there seems to be a bug with vecmath, and element m33 is zero. Here we make sure it's 1. centroidInverse.setElement(3, 3, 1.0); }
/** * Compute the vector from this coord to the input coord * * @param loc * @return */ public Vector3d getVector(final EarthVector loc) { final Vector3d vecTemp = new Vector3d(loc.getVector()); vecTemp.sub(ecfVector); return vecTemp; }
public PrismaticJoint(String name, RigidBody predecessor, ReferenceFrame beforeJointFrame, FrameVector jointAxis) { super(name, predecessor, beforeJointFrame, new PrismaticJointReferenceFrame(name, beforeJointFrame, jointAxis)); this.jointAxis = new FrameVector(jointAxis); this.unitJointTwist = new Twist(afterJointFrame, beforeJointFrame, afterJointFrame, jointAxis.getVector(), new Vector3d()); }
@Override public void applyTransform(RigidBodyTransform transform) { checkIsTransformationInPlane(transform); Vector3d xVector = new Vector3d(1.0, 0.0, 0.0); transform.transform(xVector); double deltaYaw = Math.atan2(xVector.getY(), xVector.getX()); if (Double.isNaN(deltaYaw) || Double.isInfinite(deltaYaw)) deltaYaw = 0.0; this.yaw = AngleTools.trimAngleMinusPiToPi(this.yaw + deltaYaw); }
/** * Center a cloud of points. This means subtracting the {@lin * #centroid(Point3d[])} of the cloud to each point. * * @param x * array of points. Point objects will be modified */ public static void center(Point3d[] x) { Point3d center = centroid(x); center.negate(); translate(new Vector3d(center), x); }
@Override public Matrix4d getGeometicCenterTransformation() { run(); Matrix4d geometricCentered = new Matrix4d(reverseTransformationMatrix); geometricCentered.setTranslation(new Vector3d(getGeometricCenter())); return geometricCentered; }
public static RigidBodyTransform transformLocalZ(RigidBodyTransform originalTransform, double magnitude) { RigidBodyTransform transform = new RigidBodyTransform(originalTransform); Vector3d localZTranslation = new Vector3d(0.0, 0.0, magnitude); RigidBodyTransform postTranslation = new RigidBodyTransform(); postTranslation.setTranslationAndIdentityRotation(localZTranslation); transform.multiply(postTranslation); return transform; }
public static RigidBodyTransform transformLocalX(RigidBodyTransform originalTransform, double magnitude) { RigidBodyTransform transform = new RigidBodyTransform(originalTransform); Vector3d localTranslation = new Vector3d(magnitude, 0.0, 0.0); RigidBodyTransform postTranslation = new RigidBodyTransform(); postTranslation.setTranslationAndIdentityRotation(localTranslation); transform.multiply(postTranslation); return transform; }
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; }