private static RigidBodyTransform createTransformToMatchSurfaceNormalPreserveX(Vector3D surfaceNormal) { Vector3D xAxis = new Vector3D(); Vector3D yAxis = new Vector3D(0.0, 1.0, 0.0); xAxis.cross(yAxis, surfaceNormal); xAxis.normalize(); yAxis.cross(surfaceNormal, xAxis); RotationMatrix rotationMatrix = new RotationMatrix(); rotationMatrix.setColumns(xAxis, yAxis, surfaceNormal); RigidBodyTransform transformToReturn = new RigidBodyTransform(); transformToReturn.setRotation(rotationMatrix); return transformToReturn; } }
@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); } }
public static void computeQuaternionFromYawAndZNormal(double yaw, Vector3DReadOnly zNormal, QuaternionBasics quaternionToPack) { double Cx = 1.0; double Cy = Math.tan(yaw); if (Math.abs(zNormal.getZ()) < 1e-9) { quaternionToPack.setToNaN(); return; } double Cz = -1.0 * (zNormal.getX() + Cy * zNormal.getY()) / zNormal.getZ(); double CT = Math.sqrt(Cx * Cx + Cy * Cy + Cz * Cz); if (CT < 1e-9) throw new RuntimeException("Error calculating Quaternion"); Vector3D xAxis = new Vector3D(Cx / CT, Cy / CT, Cz / CT); if (xAxis.getX() * Math.cos(yaw) + xAxis.getY() * Math.sin(yaw) < 0.0) { xAxis.negate(); } Vector3D yAxis = new Vector3D(); Vector3DReadOnly zAxis = zNormal; yAxis.cross(zAxis, xAxis); RotationMatrix rotationMatrix = rotationMatrixForQuaternionFromYawAndZNormal.get(); rotationMatrix.setColumns(xAxis, yAxis, zAxis); quaternionToPack.set(rotationMatrix); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { eX.sub(p2, p1); eX.normalize(); eY.sub(p3, p1); // temp only eZ.cross(eX, eY); eZ.normalize(); eY.cross(eZ, eX); rotation.setColumns(eX, eY, eZ); transformToParent.setRotationAndZeroTranslation(rotation); transformToParent.setTranslation(p1.getX(), p1.getY(), p1.getZ()); } }
public boolean isInVoronoiRegionOfFace(Point3D facePointOne, Point3D facePointTwo, Point3D facePointThree, Point3D otherPoint) { tempVector1.sub(facePointTwo, facePointOne); tempVector2.sub(facePointThree, facePointOne); tempNormalVector1.cross(tempVector1, tempVector2); tempVector1.set(facePointOne); tempVector1.scale(-1.0); double dot1 = tempVector1.dot(tempNormalVector1); tempVector3.sub(otherPoint, facePointOne); double dot2 = tempVector3.dot(tempNormalVector1); // TODO: Magic delta here. Figure out robustness to deltas. if (Math.abs(dot2) < 1e-10)//1e-6) return true; // Other point is in the same plane. Just project to the plane then. return (dot1 * dot2 < 0.0); // return (dot1 * dot2 < 0.0 + 1e-8); }
/** * Creates a calculator for the camera rotation. * @param up indicates which way is up. * @param forward indicates which is forward. * @throws RuntimeException if {@code up} and {@code forward} are not orthogonal. */ public CameraRotationCalculator(Vector3D up, Vector3D forward) { Vector3D left = new Vector3D(); left.cross(up, forward); if (!MathTools.epsilonEquals(left.length(), 1.0, Epsilons.ONE_HUNDRED_THOUSANDTH)) throw new RuntimeException("The vectors up and forward must be orthogonal. Received: up = " + up + ", forward = " + forward); this.up.set(up); this.forward.set(forward); down.setAndNegate(up); computeOffset(); }
private void computeOffset() { Vector3D cameraZAxis = new Vector3D(forward); Vector3D cameraYAxis = new Vector3D(down); Vector3D cameraXAxis = new Vector3D(); cameraXAxis.cross(cameraYAxis, cameraZAxis); RotationMatrix rotationOffset = new RotationMatrix(); rotationOffset.setColumns(cameraXAxis, cameraYAxis, cameraZAxis); JavaFXTools.convertRotationMatrixToAffine(rotationOffset, offset); }
private static RotationMatrix computeRotationFromNormal(Vector3D normal) { // Won't work if normal is 1,0,0 Vector3D zAxis = new Vector3D(normal); zAxis.normalize(); Vector3D xAxis = new Vector3D(1.0, 0.0, 0.0); Vector3D yAxis = new Vector3D(); yAxis.cross(normal, xAxis); yAxis.normalize(); xAxis.cross(yAxis, zAxis); xAxis.normalize(); zAxis.cross(xAxis, yAxis); zAxis.normalize(); RotationMatrix rotation = new RotationMatrix(xAxis.getX(), xAxis.getY(), xAxis.getZ(), yAxis.getX(), yAxis.getY(), yAxis.getZ(), zAxis.getX(), zAxis.getY(), zAxis.getZ()); return rotation; } }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { originVector.set(origin); xAxis.set(positionToPointAt); xAxis.sub(originVector); double norm = xAxis.length(); if (norm > 1e-12) { xAxis.scale(1.0 / norm); } else { xAxis.set(1.0, 0.0, 0.0); } zAxis.set(0.0, 0.0, 1.0); yAxis.cross(zAxis, xAxis); yAxis.normalize(); zAxis.cross(xAxis, yAxis); rotationMatrix.setColumns(xAxis, yAxis, zAxis); transformToParent.setRotationAndZeroTranslation(rotationMatrix); transformToParent.setTranslation(originVector); } }
@Override protected void computeRotationTranslation(AffineTransform transform3D) { transform3D.setIdentity(); z_rot.set(vector); double length = z_rot.length(); if (length < 1e-7) z_rot.set(0.0, 0.0, 1.0); else z_rot.normalize(); if (Math.abs(z_rot.getX()) <= 0.99) x_rot.set(1.0, 0.0, 0.0); else x_rot.set(0.0, 1.0, 0.0); y_rot.cross(z_rot, x_rot); y_rot.normalize(); x_rot.cross(y_rot, z_rot); x_rot.normalize(); rotMatrix.setColumns(x_rot, y_rot, z_rot); translationVector.set(base); transform3D.setScale(lineThickness, lineThickness, length); transform3D.setTranslation(translationVector); transform3D.setRotation(rotMatrix); }
public static double angleMinusPiToPi(Vector2DReadOnly startVector, Vector2DReadOnly endVector) { double absoluteAngle = Math.acos(startVector.dot(endVector) / startVector.length() / endVector.length()); Vector3D start3d = new Vector3D(startVector.getX(), startVector.getY(), 0.0); Vector3D end3d = new Vector3D(endVector.getX(), endVector.getY(), 0.0); Vector3D crossProduct = new Vector3D(); crossProduct.cross(start3d, end3d); if (crossProduct.getZ() >= 0.0) { return absoluteAngle; } else { return -absoluteAngle; } }
public double computeTripleProductIfTetragon() { if (pointFour == null) return Double.NaN; Vector3D vectorAB = new Vector3D(); Vector3D vectorAC = new Vector3D(); Vector3D vectorAD = new Vector3D(); Vector3D normalVector = new Vector3D(); vectorAB.sub(pointTwo, pointOne); vectorAC.sub(pointThree, pointOne); vectorAD.sub(pointFour, pointOne); normalVector.cross(vectorAB, vectorAC); double tripleProduct = vectorAD.dot(normalVector); return tripleProduct; }
/** * assumes that the GeometricJacobian.compute() method has already been called */ public void compute() { translation.set(point); int angularPartStartRow = 0; int linearPartStartRow = SpatialVector.SIZE / 2; for (int i = 0; i < geometricJacobian.getNumberOfColumns(); i++) { DenseMatrix64F geometricJacobianMatrix = geometricJacobian.getJacobianMatrix(); // angular part: tempVector.set(angularPartStartRow, i, geometricJacobianMatrix); tempJacobianColumn.cross(tempVector, translation); tempVector.set(linearPartStartRow, i, geometricJacobianMatrix); // linear part tempJacobianColumn.add(tempVector); tempJacobianColumn.get(angularPartStartRow, i, jacobianMatrix); } }
public boolean isInVoronoiRegionOfEdge(Point3D edgePointOne, Point3D edgePointTwo, Point3D otherPoint) { // TODO: Redundancies from other checks that be reused. Or is this just bad, due to epsilon problems. // If we are here, then we know we failed one or more of the Vertex Voronoi region checks (but we do not know which ones...). //So maybe these should be removed? tempVector1.set(edgePointOne); tempVector1.scale(-1.0); tempVector2.sub(edgePointTwo, edgePointOne); if (!(tempVector1.dot(tempVector2) >= 0.0)) return false; tempVector1.set(edgePointTwo); tempVector1.scale(-1.0); tempVector2.sub(edgePointOne, edgePointTwo); if (!(tempVector1.dot(tempVector2) >= 0.0)) return false; //////// tempVector1.sub(edgePointTwo, edgePointOne); tempVector2.sub(otherPoint, edgePointOne); tempNormalVector1.cross(tempVector1, tempVector2); tempVector2.cross(tempVector1, tempNormalVector1); tempVector3.set(edgePointOne); tempVector3.scale(-1.0); return (tempVector3.dot(tempVector2) >= 0.0); }
/** * X-axis of hand is located toward Sky. * Z-axis of hand is located toward (wallNormalVector). */ public static Pose3D computeCuttingWallTrajectory(double time, double trajectoryTime, double cuttingRadius, boolean cuttingDirectionCW, Point3D cuttingCenterPosition, Vector3D wallNormalVector) { Vector3D xAxisRotationMatrix = new Vector3D(0, 0, 1); Vector3D yAxisRotationMatrix = new Vector3D(); Vector3D zAxisRotationMatrix = new Vector3D(wallNormalVector); zAxisRotationMatrix.normalize(); yAxisRotationMatrix.cross(zAxisRotationMatrix, xAxisRotationMatrix); RotationMatrix cuttingRotationMatrix = new RotationMatrix(); cuttingRotationMatrix.setColumns(xAxisRotationMatrix, yAxisRotationMatrix, zAxisRotationMatrix); RigidBodyTransform handControl = new RigidBodyTransform(cuttingRotationMatrix, cuttingCenterPosition); double phase = time / trajectoryTime; handControl.appendYawRotation(cuttingDirectionCW ? -phase * 2 * Math.PI : phase * 2 * Math.PI); handControl.appendTranslation(0, cuttingRadius, 0); handControl.appendYawRotation(cuttingDirectionCW ? phase * 2 * Math.PI : -phase * 2 * Math.PI); return new Pose3D(handControl); }
/** * Valkyrie is able to complete only half rotating motion. */ public static Pose3D computeClosingValveTrajectory(double time, double trajectoryTime, double radius, boolean closingDirectionCW, double closingAngle, Point3D valveCenterPosition, Vector3D valveNormalVector) { Vector3D xAxisHandFrame = new Vector3D(valveNormalVector); Vector3D yAxisHandFrame = new Vector3D(); Vector3D zAxisHandFrame = new Vector3D(0, 0, 1); xAxisHandFrame.negate(); xAxisHandFrame.normalize(); yAxisHandFrame.cross(zAxisHandFrame, xAxisHandFrame); RotationMatrix handFrame = new RotationMatrix(); handFrame.setColumns(xAxisHandFrame, yAxisHandFrame, zAxisHandFrame); handFrame.appendRollRotation(closingDirectionCW ? -0.5 * Math.PI : 0.5 * Math.PI); RigidBodyTransform handControl = new RigidBodyTransform(handFrame, valveCenterPosition); double phase = time / trajectoryTime; handControl.appendRollRotation(closingDirectionCW ? phase * closingAngle : -phase * closingAngle); handControl.appendTranslation(0, -radius, 0); return new Pose3D(handControl); }
@Override public void computeTransform(RigidBodyTransform currXform) { update(); CameraMountInterface cameraMount = getCameraMount(); if (isMounted() && (cameraMount != null)) { cameraMount.getTransformToCamera(currXform); return; } positionOffset.set(getCamX(), getCamY(), getCamZ()); xAxis.set(getFixX(), getFixY(), getFixZ()); fixPointNode.translateTo(getFixX(), getFixY(), getFixZ()); xAxis.sub(positionOffset); xAxis.normalize(); zAxis.set(0.0, 0.0, 1.0); yAxis.cross(zAxis, xAxis); yAxis.normalize(); zAxis.cross(xAxis, yAxis); rotationMatrix.setColumns(xAxis, yAxis, zAxis); currXform.setRotationAndZeroTranslation(rotationMatrix); currXform.setTranslation(positionOffset); currXform.normalizeRotationPart(); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { //TODO: Combine with RotationTools.removePitchAndRollFromTransform(). origin.getReferenceFrame().getTransformToDesiredFrame(nonZUpToWorld, worldFrame); nonZUpToWorld.getRotation(nonZUpToWorldRotation); double yAxisX = nonZUpToWorldRotation.getM01(); double yAxisY = nonZUpToWorldRotation.getM11(); yAxis.set(yAxisX, yAxisY, 0.0); yAxis.normalize(); zAxis.set(0.0, 0.0, 1.0); xAxis.cross(yAxis, zAxis); zUpToWorldRotation.setColumns(xAxis, yAxis, zAxis); transformToParent.setRotation(zUpToWorldRotation); originPoint3d.set(origin); nonZUpToWorld.transform(originPoint3d); translation.set(originPoint3d); transformToParent.setTranslation(translation); } }
@Test public void testSetToTildeForm() { Random random = new Random(646L); Matrix3D tildeMatrix = new Matrix3D(); Vector3D vector, vectorCopy; Vector3D vector2, vector2Copy; Vector3D vector3 = new Vector3D(); Vector3D expectedVector3 = new Vector3D(); for (int k = 0; k < ITERATIONS; k++) { vector = vectorCopy = EuclidCoreRandomTools.nextVector3D(random); vector2 = vector2Copy = EuclidCoreRandomTools.nextVector3D(random); vector3.cross(vector, vector2); tildeMatrix.set(vector.getX(), vector.getY(), vector.getZ(), vector2.getX(), vector2.getY(), vector2.getZ(), vector3.getX(), vector3.getY(), vector3.getZ()); tildeMatrix.setToTildeForm(vector); tildeMatrix.transform(vector2, expectedVector3); EuclidCoreTestTools.assertRotationVectorGeometricallyEquals(vector, vectorCopy, EPS); EuclidCoreTestTools.assertRotationVectorGeometricallyEquals(vector2, vector2Copy, EPS); EuclidCoreTestTools.assertRotationVectorGeometricallyEquals(vector3, expectedVector3, EPS); } }
@Override protected void updateTwistRelativeToParent(Twist twistRelativeToParentToPack) { twistRelativeToParentToPack.setToZero(this, getParent(), this); TwistReadOnly twistOfMidFootFrame = midFootZUpGroundFrame.getTwistOfFrame(); angularVelocity.setIncludingFrame(twistOfMidFootFrame.getAngularPart()); linearVelocity.setIncludingFrame(twistOfMidFootFrame.getLinearPart()); pelvisFrame.getTwistRelativeToOther(midFootZUpGroundFrame, twistOfPelvisRelativeToMidFootFrame); linearPelvisVelocity.setIncludingFrame(twistOfPelvisRelativeToMidFootFrame.getLinearPart()); linearPelvisVelocity.changeFrame(this); angularVelocity.changeFrame(this); linearVelocity.changeFrame(this); linearVelocity.add(linearPelvisVelocity.getX(), 0.0, 0.0); offset.set(pelvisPosition.getX(), 0.0, 0.0); linearVelocityOffset.cross(angularVelocity, offset); linearVelocity.add(linearVelocityOffset); twistRelativeToParentToPack.getAngularPart().set(angularVelocity); twistRelativeToParentToPack.getLinearPart().set(linearVelocity); } }