private void calcCenters() { for (Point3d p : originalCenters) { Point3d c = new Point3d(p); c.sub(centroid); centers.add(c); Vector3d v = new Vector3d(c); v.normalize(); unitVectors.add(v); } }
public double[][] getInertiaTensor() { Point3d p = new Point3d(); double[][] tensor = new double[3][3]; // calculate the inertia tensor at center of mass Point3d com = getCenterOfMass(); for (int i = 0, n = points.size(); i < n; i++) { double mass = masses.get(i); p.sub(points.get(i), com); tensor[0][0] += mass * (p.y * p.y + p.z * p.z); tensor[1][1] += mass * (p.x * p.x + p.z * p.z); tensor[2][2] += mass * (p.x * p.x + p.y * p.y); tensor[0][1] -= mass * p.x * p.y; tensor[0][2] -= mass * p.x * p.z; tensor[1][2] -= mass * p.y * p.z; } tensor[1][0] = tensor[0][1]; tensor[2][0] = tensor[0][2]; tensor[2][1] = tensor[1][2]; return tensor; }
/** * <p> * Transform a point into the "viewport" defined by the localizer that we are an instance of. * </p> * * @param point * a 3D point to be transformed * @return a new, transformed point */ protected Point3d transformPointFromSourceSpaceIntoLocalizerSpace(Point3d point) { Point3d newPoint = new Point3d(point); // do not overwrite the supplied point newPoint.sub(localizerTLHC); // move everything to origin of the target localizer rotateIntoLocalizerSpace.transform(newPoint); return newPoint; }
/** * Translates the template ring system to new coordinates. * *@param originalCoord original coordinates of the placed ring atom from template *@param newCoord new coordinates from branch placement *@param ac AtomContainer contains atoms of ring system */ private void translateStructure(Point3d originalCoord, Point3d newCoord, IAtomContainer ac) { Point3d transVector = new Point3d(originalCoord); transVector.sub(newCoord); for (int i = 0; i < ac.getAtomCount(); i++) { if (!(ac.getAtom(i).getFlag(CDKConstants.ISPLACED))) { ac.getAtom(i).getPoint3d().sub(transVector); //ac.getAtomAt(i).setFlag(CDKConstants.ISPLACED, true); } } }
/** * Rotate the viewfrustum around the y-axis (according to the view direction). roll * * @param delta */ public void rotateY( double delta ) { rotate( right, up, delta ); rotate( backward, up, delta ); lookingAt = new Point3d( eye ); lookingAt.sub( backward ); setCameraParams( eye, lookingAt, up ); }
public static BoundingBox3d createUsingCenterAndPlusMinusVector(Point3d center, Tuple3d plusMinusVector, double epsilon) { Point3d minimumPoint = new Point3d(center); Point3d maximumPoint = new Point3d(center); minimumPoint.sub(plusMinusVector); maximumPoint.add(plusMinusVector); return new BoundingBox3d(minimumPoint, maximumPoint, epsilon); }
/** * Rotate the viewfrustum around the x-axis (according to the view direction). pitch * * @param delta */ public void rotateX( double delta ) { rotate( up, right, delta ); rotate( backward, right, delta ); lookingAt = new Point3d( eye ); lookingAt.sub( backward ); setCameraParams( eye, lookingAt, up ); }
/** * Returns the vertices of an n-fold polygon of given radius and center * @param n * @param radius * @param center * @return */ @Override public Point3d[] getVertices() { double x = getSideLengthFromCircumscribedRadius(circumscribedRadius)/2; double z = x/Math.sqrt(2); Point3d[] tetrahedron = new Point3d[4]; tetrahedron[0] = new Point3d(-x, 0, -z); tetrahedron[1] = new Point3d( x, 0, -z); tetrahedron[2] = new Point3d( 0, -x, z); tetrahedron[3] = new Point3d( 0, x, z); Point3d centroid = CalcPoint.centroid(tetrahedron); // rotate tetrahedron to align one vertex with the +z axis Matrix3d m = new Matrix3d(); m.rotX(0.5 * TETRAHEDRAL_ANGLE); for (Point3d p: tetrahedron) { p.sub(centroid); m.transform(p); } return tetrahedron; };
Vector3d ZnearD = new Vector3d( backward ); ZnearD.scale( zNear ); nc.sub( ZnearD ); fc.sub( ZfarD ); ntl = new Point3d( nc ); ntl.add( Ynh ); ntl.sub( Xnw ); ntr = new Point3d( nc ); ntr.add( Ynh ); ntr.add( Xnw ); nbl = new Point3d( nc ); nbl.sub( Ynh ); nbl.sub( Xnw ); nbr = new Point3d( nc ); nbr.sub( Ynh ); nbr.add( Xnw ); ftl = new Point3d( fc ); ftl.add( Yfh ); ftl.sub( Xfw ); ftr = new Point3d( fc ); ftr.add( Yfh ); ftr.add( Xfw ); fbl = new Point3d( fc ); fbl.sub( Yfh ); fbl.sub( Xfw ); fbr = new Point3d( fc );
/** * Returns the vertices of an n-fold polygon of given radius and center * @return */ @Override public Point3d[] getVertices() { Point3d[] polygon = new Point3d[2*n]; Matrix3d m = new Matrix3d(); Point3d center = new Point3d(0, 0, height/2); for (int i = 0; i < n; i++) { polygon[i] = new Point3d(0, circumscribedRadius, 0); m.rotZ(i*2*Math.PI/n); m.transform(polygon[i]); polygon[n+i] = new Point3d(polygon[i]); polygon[i].sub(center); polygon[n+i].add(center); } return polygon; };
for (Point3d point : projectedPoints) point.sub(averagePoint); rotationMatrix.transform(point); if (Math.abs(point.getZ()) > 1e-14)
tempPoint.sub(footstepData.getLocation()); rotationMatrix.transform(tempPoint);
private void computeEstimationLinkToWorldTransform(ReferenceFrame estimationFrame, RigidBodyTransform estimationLinkToWorldToPack, FramePoint centerOfMassWorld, FrameOrientation estimationLinkOrientation) { // r^{estimation} tempCenterOfMassBody.setToZero(estimationFrame); centerOfMassCalculator.getCenterOfMass(tempCenterOfMassBody); tempCenterOfMassBody.changeFrame(estimationFrame); // R_{estimation}^{w} estimationLinkOrientation.changeFrame(worldFrame); estimationLinkOrientation.getTransform3D(estimationLinkToWorldToPack); // R_{estimation}^{w} * r^{estimation} tempCenterOfMassBody.get(tempCenterOfMassBodyVector3d); estimationLinkToWorldToPack.transform(tempCenterOfMassBodyVector3d); // p_{estimation}^{w} = r^{w} - R_{estimation}^{w} r^{estimation} centerOfMassWorld.get(tempEstimationLinkPosition); tempEstimationLinkPosition.sub(tempCenterOfMassBodyVector3d); // H_{estimation}^{w} tempEstimationLinkPositionVector3d.set(tempEstimationLinkPosition); estimationLinkToWorldToPack.setTranslation(tempEstimationLinkPositionVector3d); }
private void computeEstimationLinkTransform(ReferenceFrame estimationFrame, RigidBodyTransform estimationLinkToWorldToPack, FramePoint centerOfMassWorld, FrameOrientation estimationLinkOrientation) { // r^{estimation} tempCenterOfMassBody.setToZero(estimationFrame); centerOfMassCalculator.getCenterOfMass(tempCenterOfMassBody); tempCenterOfMassBody.changeFrame(estimationFrame); // R_{estimation}^{w} estimationLinkOrientation.changeFrame(worldFrame); estimationLinkOrientation.getTransform3D(estimationLinkToWorldToPack); // R_{estimation}^{w} * r^{estimation} tempCenterOfMassBody.get(tempCenterOfMassBodyVector3d); estimationLinkToWorldToPack.transform(tempCenterOfMassBodyVector3d); // p_{estimation}^{w} = r^{w} - R_{estimation}^{w} r^{estimation} centerOfMassWorld.get(tempEstimationLinkPosition); tempEstimationLinkPosition.sub(tempCenterOfMassBodyVector3d); // H_{estimation}^{w} tempEstimationLinkPositionVector3d.set(tempEstimationLinkPosition); estimationLinkToWorldToPack.setTranslation(tempEstimationLinkPositionVector3d); }
temporaryPoint.sub(originToRadiusVectorToPack);
newPoints[1] = new Point3d(aPoint); newPoints[1].add(ax); newPoints[1].sub(baxca);
newPoints[1] = new Point3d(aPoint); newPoints[1].add(ax); newPoints[1].sub(baxca);
/** * Computes the orthogonal projection of a 3D point on a given 3D plane defined by a 3D point and 3D normal. * <p> * Edge cases: * <ul> * <li> if the length of the plane normal is too small, i.e. less than {@link Epsilons#ONE_TRILLIONTH}, * this method fails and returns {@code false}. * </ul> * </p> * * @param pointToProject the point to compute the projection of. Not modified. * @param pointOnPlane a point on the plane. Not modified. * @param planeNormal the normal of the plane. Not modified. * @param projectionToPack point in which the projection of the point onto the plane is stored. Modified. * @return whether the method succeeded or not. */ public static boolean getOrthogonalProjectionOnPlane(Point3d pointToProject, Point3d pointOnPlane, Vector3d planeNormal, Point3d projectionToPack) { double normalMagnitude = planeNormal.length(); if (normalMagnitude < Epsilons.ONE_TRILLIONTH) return false; projectionToPack.sub(pointToProject, pointOnPlane); double signedDistance = projectionToPack.getX() * planeNormal.getX() + projectionToPack.getY() * planeNormal.getY() + projectionToPack.getZ() * planeNormal.getZ(); signedDistance /= (normalMagnitude * normalMagnitude); projectionToPack.setX(pointToProject.getX() - signedDistance * planeNormal.getX()); projectionToPack.setY(pointToProject.getY() - signedDistance * planeNormal.getY()); projectionToPack.setZ(pointToProject.getZ() - signedDistance * planeNormal.getZ()); return true; }
tlhcB.sub(distanceAlongNormal);
bottomArrowheadRight.sub(arrowheadBaseWidthVector);