/** * Converts this Rotation into PogamutLocation. * * @param order * order of rotations should the method use * @return converted Rotation into PogamutLocation */ public PogamutLocation toLocation(Order order) { Matrix3d yaw = constructXYRot(getYaw() / 32767 * Math.PI); Matrix3d roll = constructYZRot(getRoll() / 32767 * Math.PI); Matrix3d pitch = constructXZRot(getPitch() / 32767 * Math.PI); PogamutLocation res = new PogamutLocation(1, 0, 0); switch (order) { case YAW_PITCH_ROLL: return res.mul(yaw).mul(pitch).mul(roll); case ROLL_PITCH_YAW: return res.mul(roll).mul(pitch).mul(yaw); case PITCH_YAW_ROLL: return res.mul(pitch).mul(yaw).mul(roll); case PITCH_ROLL_YAW: return res.mul(pitch).mul(roll).mul(yaw); case YAW_ROLL_PITCH: return res.mul(yaw).mul(roll).mul(pitch); case ROLL_YAW_PITCH: return res.mul(roll).mul(yaw).mul(pitch); } return null; }
public AffineTransform3D(double[] coeficients) { matrix = new Matrix4d(); if (coeficients.length==9) { matrix.set(new Matrix3d(coeficients)); } else if (coeficients.length==12) { double[] extendedCoeficients = new double[16]; for (int i=0; i<coeficients.length; ++i) { extendedCoeficients[i] = coeficients[i]; } extendedCoeficients[12] = 0; extendedCoeficients[13] = 0; extendedCoeficients[14] = 0; extendedCoeficients[15] = 1; matrix.set(extendedCoeficients); } }
/** * Converts this Rotation into Location. * * @param order * order of rotations should the method use * @return converted Rotation into Location */ public Location toLocation(Order order) { Matrix3d yaw = constructXYRot(getYaw() / 32767 * Math.PI); Matrix3d roll = constructYZRot(getRoll() / 32767 * Math.PI); Matrix3d pitch = constructXZRot(getPitch() / 32767 * Math.PI); Location res = new Location(1, 0, 0); switch (order) { case YAW_PITCH_ROLL: return res.mul(yaw).mul(pitch).mul(roll); case ROLL_PITCH_YAW: return res.mul(roll).mul(pitch).mul(yaw); case PITCH_YAW_ROLL: return res.mul(pitch).mul(yaw).mul(roll); case PITCH_ROLL_YAW: return res.mul(pitch).mul(roll).mul(yaw); case YAW_ROLL_PITCH: return res.mul(yaw).mul(roll).mul(pitch); case ROLL_YAW_PITCH: return res.mul(roll).mul(yaw).mul(pitch); } return null; }
/** * Dodges from the specified location. * * @param inFrontOfTheBot Location to dodge from. * @param bDouble Whether to perform double dodge. */ public void dodgeBack(ILocated inFrontOfTheBot, boolean bDouble) { ILocated bot = self.getLocation(); Location direction = new Location(bot.getLocation().x - inFrontOfTheBot.getLocation().x, bot.getLocation().y - inFrontOfTheBot.getLocation().y, 0); direction = direction.getNormalized(); double alpha = Math.acos(self.getRotation().toLocation().getNormalized().dot(direction.getNormalized())); double orientation = self.getRotation().toLocation().cross(new Location(0, 0, 1)).dot(direction); if (orientation > 0) { alpha = -alpha; } Matrix3d rot = Rotation.constructXYRot(alpha); direction = new Location(1, 0, 0).mul(rot); this.act.act(new Dodge().setDirection(direction).setDouble(bDouble)); }
/** * Dodges towards the specified location. * * @param inFrontOfTheBot Location to dodge to. * @param bDouble Whether to perform double dodge. */ public void dodgeTo(ILocated inFrontOfTheBot, boolean bDouble) { ILocated bot = self.getLocation(); Location direction = new Location(inFrontOfTheBot.getLocation().x - bot.getLocation().x, inFrontOfTheBot.getLocation().y - bot.getLocation().y, 0); direction = direction.getNormalized(); double alpha = Math.acos(self.getRotation().toLocation().getNormalized().dot(direction.getNormalized())); double orientation = self.getRotation().toLocation().cross(new Location(0, 0, 1)).dot(direction); if (orientation > 0) { alpha = -alpha; } Matrix3d rot = Rotation.constructXYRot(alpha); direction = new Location(1, 0, 0).mul(rot); this.act.act(new Dodge().setDirection(direction).setDouble(bDouble)); }
/** * Rotate an {@link AxisAlignedBB} by the specified rotation matrix. * * @param axisAlignedBB The AABB * @param rotationMatrix The rotation matrix * @param forcePositive If true, set each coordinate of the rotated AABB to it absolute value * @return The rotated AABB */ public static AxisAlignedBB rotateAABB(AxisAlignedBB axisAlignedBB, Matrix3d rotationMatrix, boolean forcePositive) { // Extract the minimum and maximum coordinates of the AABB into vectors final Vector3d minCoords = new Vector3d(axisAlignedBB.minX, axisAlignedBB.minY, axisAlignedBB.minZ); final Vector3d maxCoords = new Vector3d(axisAlignedBB.maxX, axisAlignedBB.maxY, axisAlignedBB.maxZ); // Rotate the vectors in-place rotationMatrix.transform(minCoords); rotationMatrix.transform(maxCoords); if (forcePositive) { // Get the absolute value of the coordinates minCoords.absolute(); maxCoords.absolute(); } // Return an AABB with the new coordinates return new AxisAlignedBB(minCoords.getX(), minCoords.getY(), minCoords.getZ(), maxCoords.getX(), maxCoords.getY(), maxCoords.getZ()); }
/** * Getter for the rotation part of the complete transform as yaw, pitch and * roll angles. * * @return The yaw, pitch and roll angles as Java3D Vecmath {@link Vector3d} */ public Vector3d getRotationYPR() { Matrix3d rot = getRotationMatrix(); // this code is taken from buttel btMatrix3x3 getEulerYPR(). // http://bulletphysics.org/Bullet/BulletFull/btMatrix3x3_8h_source.html // first use the normal calculus double yawOut = Math.atan2(rot.m10, rot.m00); double pitchOut = Math.asin(-rot.m20); double rollOut = Math.atan2(rot.m21, rot.m22); // on pitch = +/-HalfPI if (Math.abs(pitchOut) == Math.PI / 2.0) { if (yawOut > 0) yawOut -= Math.PI; else yawOut += Math.PI; if (pitchOut > 0) pitchOut -= Math.PI; else pitchOut += Math.PI; } return new Vector3d(yawOut, pitchOut, rollOut); }
public static PointD addRotAroundAxis(double x, double y, double z, double axis) { // May be faster to rewrite with your own to reduce the number of calculations Matrix3d rotationMatrix = new Matrix3d(); rotationMatrix.rotZ(z); // Add z rotation Matrix3d yRot = new Matrix3d(); yRot.rotY(y); Matrix3d xRot = new Matrix3d(); xRot.rotX(x); Matrix3d axisRot = new Matrix3d(); axisRot.rotY(axis); rotationMatrix.mul(yRot); rotationMatrix.mul(xRot); rotationMatrix.mul(axisRot); return new PointD(Math.atan2(rotationMatrix.m21, rotationMatrix.m22), Math.atan2(-rotationMatrix.m20,Math.sqrt(rotationMatrix.m21 * rotationMatrix.m21 + rotationMatrix.m22 * rotationMatrix.m22)), Math.atan2(rotationMatrix.m10, rotationMatrix.m00)); }
/** * Produce a Detector properties object populated with sensible default values given image shape. * It produces a detector normal to the beam and centred on the beam with square pixels of 0.1024mm and set 200mm * from the sample. * * @param shape image shape */ public static DetectorProperties getDefaultDetectorProperties(int... shape) { int heightInPixels = shape[0]; int widthInPixels = shape[1]; // Set a few default values double pixelSizeX = 0.1024; double pixelSizeY = 0.1024; double distance = 200.00; // Create identity orientation Matrix3d identityMatrix = new Matrix3d(); identityMatrix.setIdentity(); // Create the detector origin vector based on the above Vector3d dOrigin = new Vector3d((widthInPixels - widthInPixels/2d) * pixelSizeX, (heightInPixels - heightInPixels/2d) * pixelSizeY, distance); return new DetectorProperties(dOrigin, heightInPixels, widthInPixels, pixelSizeX, pixelSizeY, identityMatrix); }
/** * Reset entries that are less than or equal to 1 unit of least precision of * the matrix's scale * @param m */ private static void santise(Matrix3d m) { double scale = m.getScale(); double min = Math.ulp(scale); for (int i = 0; i < 3; i++) { double t; t = Math.abs(m.getElement(i, 0)); if (t > 0 && t <= min) { m.setElement(i, 0, 0); } t = Math.abs(m.getElement(i, 1)); if (t > 0 && t <= min) { m.setElement(i, 1, 0); } t = Math.abs(m.getElement(i, 2)); if (t > 0 && t <= min) { m.setElement(i, 2, 0); } } }
void showMatch(GFace face1, GFace face2, int matchIndex, GSolid solid) { matchLabels1 = new String[3]; matchLabels2 = new String[3]; int sc = face1.sideCount(); boolean flipFace2 = face1.getOrientation(solid1, solid1.getGCenter()) == face2.getOrientation(solid2, solid2.getGCenter()); for (int i = 0; i < 3; i++) { matchLabels1[i] = face1.labelAt((matchIndex + i) % sc); if (flipFace2) matchLabels2[i] = face2.labelAt((sc - i) % sc); else matchLabels2[i] = face2.labelAt(i); } GDocumentHandler documentHandler = GDocumentHandler.getInstance(); GFigure figure1 = document.getFigure(figure1Name); GFigure jointFigure = documentHandler.newFigure(solid); jointFigure.setTransparent(figure1.isTransparent()); jointFigure.setLabelled(figure1.isLabelled()); Matrix3d attitude = new Matrix3d(figure1.getCamera().getAttitude()); jointFigure.getCamera().setAttitude(attitude); Color baseColor = figure1.getBaseColor(); jointFigure.setBaseColor(new Color(baseColor.getRGB())); jointFigureName = jointFigure.getName(); }
void showMatch(GFace face1, GFace face2, int matchIndex, GSolid solid) { matchLabels1 = new String[3]; matchLabels2 = new String[3]; int sc = face1.sideCount(); boolean flipFace2 = face1.getOrientation(solid1, solid1.getGCenter()) == face2.getOrientation(solid2, solid2.getGCenter()); for (int i = 0; i < 3; i++) { matchLabels1[i] = face1.labelAt((matchIndex + i) % sc); if (flipFace2) { matchLabels2[i] = face2.labelAt((sc - i) % sc); } else { matchLabels2[i] = face2.labelAt(i); } } GDocumentHandler documentHandler = GDocumentHandler.getInstance(); GFigure figure1 = document.getFigure(figure1Name); GFigure jointFigure = documentHandler.newFigure(solid); jointFigure.setTransparent(figure1.isTransparent()); jointFigure.setLabelled(figure1.isLabelled()); Matrix3d attitude = new Matrix3d(figure1.getCamera().getAttitude()); jointFigure.getCamera().setAttitude(attitude); Color baseColor = figure1.getBaseColor(); jointFigure.setBaseColor(new Color(baseColor.getRGB())); jointFigureName = jointFigure.getName(); }
public void paintSphericalVanishingPoints(Graphics g) { final double EPS = 0.1; final int R = 1; g.setColor(Constants.Vanish_Color); Matrix3d RZ = new Matrix3d(); Matrix3d RY = new Matrix3d(); for (double rz = 0; rz < 2 * Math.PI; rz += EPS) { RZ.rotZ(rz); for (double ry = -Math.PI / 2; ry < Math.PI / 2; ry += EPS) { RY.rotY(ry); Vector3d v = new Vector3d(1, 0, 0); RY.transform(v); RZ.transform(v); Point2d vp = projection.vanishingPoint(v); if (vp != null) { g.fillOval((int) vp.x - R, (int) vp.y - R, 2 * R, 2 * R); } } } }
public Projection getProjection() { double fx = focalLength * resolution.x / sensorSize.x; double fy = focalLength * resolution.y / sensorSize.y; Matrix3d K = new Matrix3d(fx, 0, center.x, 0, fy, center.y, 0, 0, 1); //K.setIdentity(); Matrix3d R = new Matrix3d(this.rotation); R.transpose(); VirtualCamera.logFrame().log(R.toString()); Vector3d T = new Vector3d(this.position); T.scale(-1); R.transform(T); projection.set(new Projection(K, R, T)); return this.projection; }
/** * Creates a new instance of Translation. * * @param center * @param xAngle * @param yAngle * @param zAngle */ public Rotation(Point3d center, double xAngle, double yAngle, double zAngle) { super(); centerOfRotation = center; double x = Math.toRadians(xAngle); double y = Math.toRadians(yAngle); double z = Math.toRadians(zAngle); Matrix3d xrot = new Matrix3d(); xrot.rotX(x); Matrix3d yrot = new Matrix3d(); yrot.rotY(y); Matrix3d zrot = new Matrix3d(); zrot.rotZ(z); yrot.mul(zrot); xrot.mul(yrot); rotation = xrot; toOrigin = new Translation(-centerOfRotation.x, -centerOfRotation.y, -centerOfRotation.z); toCenter = new Translation(centerOfRotation.x, centerOfRotation.y, centerOfRotation.z); }
public Rotation(long beg, long en, Point3d center, double xAngle, double yAngle, double zAngle) { super(beg, en); centerOfRotation = center; double x = Math.toRadians(xAngle); double y = Math.toRadians(yAngle); double z = Math.toRadians(zAngle); Matrix3d xrot = new Matrix3d(); xrot.rotX(x); Matrix3d yrot = new Matrix3d(); yrot.rotY(y); Matrix3d zrot = new Matrix3d(); zrot.rotZ(z); yrot.mul(zrot); xrot.mul(yrot); rotation = xrot; toOrigin = new Translation(-centerOfRotation.x, -centerOfRotation.y, -centerOfRotation.z); toCenter = new Translation(centerOfRotation.x, centerOfRotation.y, centerOfRotation.z); }
@Override public Matrix3d getViewMatrix(int index) { Matrix3d m = new Matrix3d(); switch (index) { case 0: m.setIdentity(); // front vertex-centered break; case 1: m.rotX(-0.6523581397843639); // back face-centered -0.5535743588970415 m.rotX(Math.toRadians(-26)); break; case 2: m.rotZ(Math.PI/2); Matrix3d m1 = new Matrix3d(); m1.rotX(-1.0172219678978445); m.mul(m1); break; default: throw new IllegalArgumentException("getViewMatrix: index out of range:" + index); } return m; }
/** * 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; }
/** * 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; }
@Override public Matrix3d getViewMatrix(int index) { Matrix3d m = new Matrix3d(); switch (index) { case 0: m.setIdentity(); // front break; case 1: m.rotX(Math.PI/2); // side edge-centered break; case 2: m.rotY(Math.PI/n); // side face-centered Matrix3d m1 = new Matrix3d(); m1.rotX(Math.PI/2); m.mul(m1); break; case 3: m.set(flipX()); // back break; default: throw new IllegalArgumentException("getViewMatrix: index out of range:" + index); } return m; }
@Override public Matrix3d getViewMatrix(int index) { Matrix3d m = new Matrix3d(); switch (index) { case 0: m.setIdentity(); // C4 vertex-centered break; case 1: m.rotX(-0.5 * TETRAHEDRAL_ANGLE); // C3 face-centered 2.0*Math.PI/3 Matrix3d m1 = new Matrix3d(); m1.rotZ(Math.PI/4); m.mul(m1); break; case 2: m.rotY(Math.PI/4); // side face-centered break; default: throw new IllegalArgumentException("getViewMatrix: index out of range:" + index); } return m; }
@Override public Matrix3d getViewMatrix(int index) { Matrix3d m = new Matrix3d(); switch (index) { case 0: m.setIdentity(); // front break; case 1: m.rotY(Math.PI/2); // left break; case 2: m.rotY(Math.PI); // back break; case 3: m.rotY(-Math.PI/2); // right break; case 4: m.rotX(Math.PI/2); // top break; case 5: m.rotX(-Math.PI/2); // bottom break; default: throw new IllegalArgumentException("getViewMatrix: index out of range:" + index); } return m; }
public static Matrix4d getMatrixFromAlgebraic(String transfAlgebraic) { String[] parts = transfAlgebraic.toUpperCase().split(","); double[] xCoef = convertAlgebraicStrToCoefficients(parts[0].trim()); double[] yCoef = convertAlgebraicStrToCoefficients(parts[1].trim()); double[] zCoef = convertAlgebraicStrToCoefficients(parts[2].trim()); Matrix4d mat = new Matrix4d(); mat.setIdentity(); mat.setRotation(new Matrix3d(xCoef[0],xCoef[1],xCoef[2],yCoef[0],yCoef[1],yCoef[2],zCoef[0],zCoef[1],zCoef[2])); mat.setTranslation(new Vector3d(xCoef[3],yCoef[3],zCoef[3])); return mat; //return new Matrix4d(xCoef[0],xCoef[1],xCoef[2],xCoef[3], // yCoef[0],yCoef[1],yCoef[2],yCoef[3], // zCoef[0],zCoef[1],zCoef[2],zCoef[3], // 0,0,0,1); }
/** * Generic quadcopter constructor. * * @param world world where to place the vehicle * @param modelName filename of model to load, in .obj format * @param orientation "x" or "+" * @param armLength length of arm from center * @param rotorThrust full thrust of one rotor * @param rotorTorque torque at full thrust of one rotor * @param rotorTimeConst spin-up time of rotor * @param rotorsOffset rotors positions offset from gravity center * @throws FileNotFoundException if .obj model file not found */ public Quadcopter(World world, String modelName, String orientation, double armLength, double rotorThrust, double rotorTorque, double rotorTimeConst, Vector3d rotorsOffset) throws FileNotFoundException { super(world, modelName); rotorPositions[0] = new Vector3d(0.0, armLength, 0.0); rotorPositions[1] = new Vector3d(0.0, -armLength, 0.0); rotorPositions[2] = new Vector3d(armLength, 0.0, 0.0); rotorPositions[3] = new Vector3d(-armLength, 0.0, 0.0); if (orientation.equals("x") || orientation.equals("X")) { Matrix3d r = new Matrix3d(); r.rotZ(-Math.PI / 4); for (int i = 0; i < rotorsNum; i++) { r.transform(rotorPositions[i]); } } else if (orientation.equals("+")) { } else { throw new RuntimeException("Unknown orientation: " + orientation); } for (int i = 0; i < rotors.length; i++) { rotorPositions[i].add(rotorsOffset); Rotor rotor = rotors[i]; rotor.setFullThrust(rotorThrust); rotor.setFullTorque(i < 2 ? -rotorTorque : rotorTorque); rotor.setTimeConstant(rotorTimeConst); } }
@Override public void update(long t) { this.position = baseObject.position; this.velocity = baseObject.velocity; this.acceleration = baseObject.acceleration; double yaw = Math.atan2(baseObject.rotation.getElement(1, 0), baseObject.rotation.getElement(0, 0)); this.rotation.rotZ(yaw); if (pitchChannel >= 0 && baseObject instanceof AbstractVehicle) { // Control camera pitch List<Double> control = ((AbstractVehicle) baseObject).getControl(); if (control.size() > pitchChannel) { Matrix3d r = new Matrix3d(); r.rotY(control.get(4) * pitchScale); this.rotation.mul(r); } } }
/** * Projects this Location (vector) using matrix from parameter * * @param matrix * projection matrix * @return resulting Location */ public PogamutLocation mul(Matrix3d matrix) { PogamutLocation res = new PogamutLocation( matrix.getM00() * x + matrix.getM10() * y + matrix.getM20() * z, matrix.getM01() * x + matrix.getM11() * y + matrix.getM21() * z, matrix.getM02() * x + matrix.getM12() * y + matrix.getM22() * z ); return res; }
/** * Useful methods from Rotation->PogamutLocation conversions. Constructs rotation * in YZ plane. * * @param angle * @return projection Matrix3d */ public static Matrix3d constructYZRot(double angle) { Matrix3d res = new Matrix3d(); double cos = Math.cos(angle); double sin = Math.sin(angle); res.setM00(1); res.setM11(cos); res.setM21(sin); res.setM12(-sin); res.setM22(cos); return res; }
/** * Useful methods from Rotation->PogamutLocation conversions. Constructs rotation * in XZ plane. * * @param angle * @return projection Matrix3d */ public static Matrix3d constructXZRot(double angle) { Matrix3d res = new Matrix3d(); double cos = Math.cos(angle); double sin = Math.sin(angle); res.setM00(cos); res.setM20(-sin); res.setM11(1); res.setM02(sin); res.setM22(cos); return res; }
/** * Useful methods from Rotation->PogamutLocation conversions. Constructs rotation * in XY plane. * * @param angle * @return projection Matrix3d */ public static Matrix3d constructXYRot(double angle) { Matrix3d res = new Matrix3d(); double cos = Math.cos(angle); double sin = Math.sin(angle); res.setM00(cos); res.setM10(-sin); res.setM01(sin); res.setM11(cos); res.setM22(1); return res; }
public static AffineTransform3D createRotationOx(double theta) { Matrix3d matrix3d = new Matrix3d(); matrix3d.rotX(theta); Matrix4d matrix4d = new Matrix4d(); matrix4d.set(matrix3d); return new AffineTransform3D(matrix4d); }
public static AffineTransform3D createRotationOy(double theta) { Matrix3d matrix3d = new Matrix3d(); matrix3d.rotY(theta); Matrix4d matrix4d = new Matrix4d(); matrix4d.set(matrix3d); return new AffineTransform3D(matrix4d); }
public static AffineTransform3D createRotationOz(double theta) { Matrix3d matrix3d = new Matrix3d(); matrix3d.rotZ(theta); Matrix4d matrix4d = new Matrix4d(); matrix4d.set(matrix3d); return new AffineTransform3D(matrix4d); }
/** * Useful methods from Rotation->Location conversions. Constructs rotation * in YZ plane (~ ROLL). * * @param angle * @return projection Matrix3d */ public static Matrix3d constructYZRot(double angle) { Matrix3d res = new Matrix3d(); double cos = Math.cos(angle); double sin = Math.sin(angle); res.setM00(1); res.setM11(cos); res.setM21(-sin); res.setM12(sin); res.setM22(cos); return res; }
/** * Useful methods from Rotation->Location conversions. Constructs rotation * in XZ plane (~ PITCH). * * @param angle in radians * @return projection Matrix3d */ public static Matrix3d constructXZRot(double angle) { Matrix3d res = new Matrix3d(); double cos = Math.cos(angle); double sin = Math.sin(angle); res.setM00(cos); res.setM20(sin); res.setM11(1); res.setM02(-sin); res.setM22(cos); return res; }
/** * Useful methods from Rotation->Location conversions. Constructs rotation * in XY plane (~ YAW). * * @param angle in radians * @return projection Matrix3d */ public static Matrix3d constructXYRot(double angle) { Matrix3d res = new Matrix3d(); double cos = Math.cos(angle); double sin = Math.sin(angle); res.setM00(cos); res.setM10(-sin); res.setM01(sin); res.setM11(cos); res.setM22(1); return res; }
/** * Projects this Location (vector) using matrix from parameter * * @param matrix * projection matrix * @return resulting Location */ public Location mul(Matrix3d matrix) { // This is a correct way. It also depends on Matrix3d definitions in Rotation class. // This way of multiply is compatibile with current rotation to location conversion. // Before any changes, test that conversion from rotation to location // works well. Location res = new Location( matrix.getM00() * x + matrix.getM10() * y + matrix.getM20() * z, matrix.getM01() * x + matrix.getM11() * y + matrix.getM21() * z, matrix.getM02() * x + matrix.getM12() * y + matrix.getM22() * z ); return res; }
/** * Makes the bot to dodge in the selected direction (this is in fact single * jump that is executed to selected direction). Direction is absolute in * this method. (issues GB DODGE command) * * @param direction Absolute vector (that will be normalized) that specifies * direction of the jump. * @param bDouble Wheter we want to perform double dodge. * @see jump() * @see doubleJump() */ public void dodge(Location direction, boolean bDouble) { direction = direction.getNormalized(); double alpha = Math.acos(self.getRotation().toLocation().getNormalized().dot(direction.getNormalized())); double orientation = self.getRotation().toLocation().cross(new Location(0, 0, 1)).dot(direction); if (orientation > 0) { alpha = -alpha; } Matrix3d rot = Rotation.constructXYRot(alpha); direction = new Location(1, 0, 0).mul(rot); act.act(new Dodge().setDirection(direction).setDouble(bDouble)); }
/** * Create a matrix that rotates around the specified axis by the specified angle. * * @param axis The axis * @param radians The angle in radians * @return The rotation matrix */ public static Matrix3d getRotationMatrix(EnumFacing.Axis axis, double radians) { final Vec3i axisDirectionVector = AXIS_DIRECTION_VECTORS.get(axis); final AxisAngle4d axisAngle = new AxisAngle4d(axisDirectionVector.getX(), axisDirectionVector.getY(), axisDirectionVector.getZ(), radians); final Matrix3d rotationMatrix = new Matrix3d(); rotationMatrix.set(axisAngle); return rotationMatrix; }
private Vector3d antiPodalDirection(Edge e1,Edge e2){ Vector3d e1v1=e1.getNFaces().get(0).getNormal(); Vector3d e1v2=e1.getNFaces().get(1).getNormal(); Vector3d e2v1=e2.getNFaces().get(0).getNormal(); Vector3d e2v2=e2.getNFaces().get(1).getNormal(); Matrix3d m=new Matrix3d(); m.setColumn(0, GeometryUtils.vectorSubtract(e1v1, e1v2)); m.setColumn(1, e2v1); m.setColumn(2, GeometryUtils.vectorSubtract(e2v2, e2v1)); try{ m.invert(); }catch (Exception e){ return null; } double t=m.m00*e1v1.x+m.m01*e1v1.y+m.m02*e1v1.z; double c=m.m10*e1v1.x+m.m11*e1v1.y+m.m12*e1v1.z; double cu=m.m20*e1v1.x+m.m21*e1v1.y+m.m22*e1v1.z; double u=cu/c; if (c<0&&t>=0-EPS&&t<=1+EPS&&u>=0-EPS&&u<=1+EPS){ Vector3d n=new Vector3d(); Vector3d vv=GeometryUtils.vectorSubtract(e1v2, e1v1); vv.scale(t); n.add(e1v1,vv); n.normalize(); return n; } return null; }
public Vector3d quatRotate(Quat4d r, Vector3d v) { Matrix3d rotMat = new Matrix3d(); rotMat.set(r); Vector3d result = new Vector3d(); rotMat.transform(v, result); return result; }