public void setUsingArm(ReferenceFrame expressedInFrame, Vector3d linearPart, Vector3d arm) { this.expressedInFrame = expressedInFrame; this.linearPart.set(linearPart); this.angularPart.cross(arm, linearPart); }
private static void getRawNormal(Point3d ptA, Point3d ptB, Point3d ptC, Vector3d normal, Vector3d vcAB, Vector3d vcAC) { // make A->B and A->C vcAB.sub(ptB, ptA); vcAC.sub(ptC, ptA); // make the normal to this normal.cross(vcAB, vcAC); }
private static Vector3d getNonColinearVector(Vector3d ab) { Vector3d cr = new Vector3d(); cr.cross(ab, XV); if (cr.length() > 0.00001) { return XV; } else { return YV; } } }
/** * Gets the nonColinearVector attribute of the AtomLigandPlacer3D class * *@param ab Description of the Parameter *@return The nonColinearVector value */ private Vector3d getNonColinearVector(Vector3d ab) { Vector3d cr = new Vector3d(); cr.cross(ab, XV); if (cr.length() > 0.00001) { return XV; } else { return YV; } }
/** * Construct using linear part and arm */ public static SpatialForceVector createUsingArm(ReferenceFrame expressedInFrame, Vector3d linearPart, Vector3d arm) { SpatialForceVector ret = new SpatialForceVector(expressedInFrame); ret.setLinearPart(linearPart); ret.getAngularPart().cross(arm, linearPart); return ret; }
public void setIncludingFrame(FrameVector force, FramePoint pointOfApplication) { force.checkReferenceFrameMatch(pointOfApplication); expressedInFrame = force.getReferenceFrame(); force.get(linearPart); pointOfApplication.get(tempVector); angularPart.cross(tempVector, linearPart); }
protected final void makeNormal() { normal = new Vector3d(); normal.cross(row, column); normal.normalize(); normalArray = new double[3]; normal.get(normalArray); // depends of vector system (right/left-handed system): normalArray[2] = normalArray[2] * -1 normal = new Vector3d(normalArray); }
private static Vector3d calcNormal(Point3d p1, Point3d p2, Point3d p3) { Vector3d n = new Vector3d(p2); n.sub(p1); Vector3d n2 = new Vector3d(p3); n2.sub(p2); n.cross(n, n2); n.normalize(); return n; }
// 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 void setIncludingFrame(FrameVector force, FrameVector moment, FramePoint pointOfApplication) { force.checkReferenceFrameMatch(pointOfApplication); expressedInFrame = force.getReferenceFrame(); force.get(linearPart); pointOfApplication.get(tempVector); angularPart.cross(tempVector, linearPart); angularPart.add(moment.getVector()); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { positionToPointAt.get(xAxis); xAxis.setZ(0.0); xAxis.normalize(); yAxis.cross(zAxis, xAxis); rotationMatrix.setColumn(0, xAxis); rotationMatrix.setColumn(1, yAxis); rotationMatrix.setColumn(2, zAxis); transformToParent.setRotation(rotationMatrix); } }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { x.set(xAxis.getX(), xAxis.getY(), 0.0); z.set(0.0, 0.0, 1.0); y.cross(z, x); rotation.setColumn(0, x); rotation.setColumn(1, y); rotation.setColumn(2, z); transformToParent.setRotationAndZeroTranslation(rotation); } }
public void setScrew(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, ReferenceFrame expressedInFrame, double angularVelocityMagnitude, double linearVelocityMagnitude, Vector3d axisOfRotation, Vector3d offset) { axisOfRotation.normalize(); this.bodyFrame = bodyFrame; this.baseFrame = baseFrame; this.expressedInFrame = expressedInFrame; this.freeVector.cross(offset, axisOfRotation); this.freeVector.scale(angularVelocityMagnitude); this.linearPart = new Vector3d(axisOfRotation); this.linearPart.scale(linearVelocityMagnitude); this.linearPart.add(freeVector); this.angularPart = new Vector3d(axisOfRotation); this.angularPart.scale(angularVelocityMagnitude); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { x.set(xAxis.getX(), xAxis.getY(), 0.0); z.set(0.0, 0.0, 1.0); y.cross(z, x); rotation.setColumn(0, x); rotation.setColumn(1, y); rotation.setColumn(2, z); transformToParent.setRotationAndZeroTranslation(rotation); } }
private void computeOffset() { Vector3d cameraZAxis = new Vector3d(forward); Vector3d cameraYAxis = new Vector3d(down); Vector3d cameraXAxis = new Vector3d(); cameraXAxis.cross(cameraYAxis, cameraZAxis); Matrix3d rotationOffset = new Matrix3d(); rotationOffset.setColumn(0, cameraXAxis); rotationOffset.setColumn(1, cameraYAxis); rotationOffset.setColumn(2, cameraZAxis); JavaFXTools.convertRotationMatrixToAffine(rotationOffset, offset); }
private Vector3d calcFaceNormal(LineSegment2d edge, double heightFactor) { Vector2d edgeVector = Vector2dUtil.fromTo(edge.getBegin(), edge.getEnd()); edgeVector.normalize(); Vector2d edgeOrthogonal = Vector2dUtil.orthogonalLeft(edgeVector); Vector3d v1 = new Vector3d(edgeVector.x, 0, -edgeVector.y); Vector3d v2 = new Vector3d(edgeOrthogonal.x, heightFactor, -edgeOrthogonal.y); v1.cross(v1, v2); v1.normalize(); return v1; }
protected void doCommonConstructorStuff() { validateDirectionCosines(localizerRow, localizerColumn); localizerNormal = new Vector3d(); localizerNormal.cross(localizerRow, localizerColumn); // the normal to the plane is the cross product of the row // and column localizerVoxelSpacingArray = new double[3]; localizerVoxelSpacing.get(localizerVoxelSpacingArray); localizerDimensionsArray = new double[3]; localizerDimensions.get(localizerDimensionsArray); rotateIntoLocalizerSpace = new Matrix3d(); rotateIntoLocalizerSpace.setRow(0, localizerRow); rotateIntoLocalizerSpace.setRow(1, localizerColumn); rotateIntoLocalizerSpace.setRow(2, localizerNormal); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { eX.sub(p2.getPoint(), p1.getPoint()); eX.normalize(); eY.sub(p3.getPoint(), p1.getPoint()); // temp only eZ.cross(eX, eY); eZ.normalize(); eY.cross(eZ, eX); rotation.setColumn(0, eX); rotation.setColumn(1, eY); rotation.setColumn(2, eZ); transformToParent.setRotationAndZeroTranslation(rotation); transformToParent.setTranslation(p1.getX(), p1.getY(), p1.getZ()); } }
public void setBasedOnOriginAcceleration(FrameVector angularAcceleration, FrameVector originAcceleration, Twist twistOfBodyWithRespectToBase) { bodyFrame.checkReferenceFrameMatch(expressedInFrame); twistOfBodyWithRespectToBase.getBodyFrame().checkReferenceFrameMatch(bodyFrame); twistOfBodyWithRespectToBase.getBaseFrame().checkReferenceFrameMatch(baseFrame); angularAcceleration.changeFrame(bodyFrame); angularPart.set(angularAcceleration.getVector()); originAcceleration.changeFrame(bodyFrame); twistOfBodyWithRespectToBase.changeFrame(bodyFrame); linearPart.cross(twistOfBodyWithRespectToBase.getAngularPart(), twistOfBodyWithRespectToBase.getLinearPart()); linearPart.sub(originAcceleration.getVector(), linearPart); }
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()); } }