public static void pitchAboutPoint(FramePoint3DReadOnly pointToTransform, FramePoint3DReadOnly pointToPitchAbout, double pitch, FramePoint3D resultToPack) { pointToTransform.checkReferenceFrameMatch(pointToPitchAbout); double tempX = pointToTransform.getX() - pointToPitchAbout.getX(); double tempY = pointToTransform.getY() - pointToPitchAbout.getY(); double tempZ = pointToTransform.getZ() - pointToPitchAbout.getZ(); double cosAngle = Math.cos(pitch); double sinAngle = Math.sin(pitch); double x = cosAngle * tempX + sinAngle * tempZ; tempZ = -sinAngle * tempX + cosAngle * tempZ; tempX = x; resultToPack.setIncludingFrame(pointToPitchAbout); resultToPack.add(tempX, tempY, tempZ); }
/** * yawAboutPoint * * @param pointToYawAbout FramePoint * @param yaw double * @return CartesianPositionFootstep */ public static void yawAboutPoint(FramePoint3DReadOnly pointToTransform, FramePoint3DReadOnly pointToYawAbout, double yaw, FramePoint3D resultToPack) { pointToTransform.checkReferenceFrameMatch(pointToYawAbout); double tempX = pointToTransform.getX() - pointToYawAbout.getX(); double tempY = pointToTransform.getY() - pointToYawAbout.getY(); double tempZ = pointToTransform.getZ() - pointToYawAbout.getZ(); double cosAngle = Math.cos(yaw); double sinAngle = Math.sin(yaw); double x = cosAngle * tempX + -sinAngle * tempY; tempY = sinAngle * tempX + cosAngle * tempY; tempX = x; resultToPack.setIncludingFrame(pointToYawAbout); resultToPack.add(tempX, tempY, tempZ); }
/** * Sets the next ball to the given location, and gives it the given appearance. If all the balls * have been set, then does nothing. * * @param location FramePoint to set the next ball to. * @param appearance Appearance to give the next ball. */ public void setBall(FramePoint3DReadOnly location, AppearanceDefinition appearance, int ballIndex) { //TODO: PDN, note that with current implementation of JME, you can only "set" the appearance once. After that, it will ignore all appearance sets if (!location.getReferenceFrame().isWorldFrame()) throw new RuntimeException(location + " must be in a World Frame!"); setBall(location.getX(), location.getY(), location.getZ(), appearance, ballIndex); }
intersectionToPack.changeFrame(firstPointOnLine1.getReferenceFrame()); double pointOnLine1x = firstPointOnLine1.getX(); double pointOnLine1y = firstPointOnLine1.getY(); double lineDirection1x = secondPointOnLine1.getX() - firstPointOnLine1.getX(); double lineDirection1y = secondPointOnLine1.getY() - firstPointOnLine1.getY(); double pointOnLine2x = firstPointOnLine2.getX(); double pointOnLine2y = firstPointOnLine2.getY(); double lineDirection2x = secondPointOnLine2.getX() - firstPointOnLine2.getX(); double lineDirection2y = secondPointOnLine2.getY() - firstPointOnLine2.getY();