Java 类javax.vecmath.Matrix3d 实例源码

项目:Pogamut3    文件:PogamutRotation.java   
/**
 * 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;
}
项目:Pogamut3    文件:AffineTransform3D.java   
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);
    }
}
项目:Pogamut3    文件:Rotation.java   
/**
 * 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;
}
项目:Pogamut3    文件:AdvancedLocomotion.java   
/**
 * 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));
}
项目:Pogamut3    文件:AdvancedLocomotion.java   
/**
 * 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));
}
项目:AquaRegia    文件:VectorUtils.java   
/**
 * 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());
}
项目:rct-java    文件:Transform.java   
/**
 * 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);
}
项目:SekC-Physics    文件:MatrixMaths.java   
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));
    }
项目:dawnsci    文件:DetectorProperties.java   
/**
 * 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);
}
项目:dawnsci    文件:DetectorProperties.java   
/**
 * 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);
        }
    }
}
项目:geometria    文件:GJoinAction.java   
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();
}
项目:geometria    文件:GJoinAction.java   
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();
}
项目:jvircam    文件:Painter.java   
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);
            }
        }
    }
}
项目:jvircam    文件:Camera.java   
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;
}
项目:SiJaRayCluster    文件:Rotation.java   
/**
 * 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);
}
项目:SiJaRayCluster    文件:Rotation.java   
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);
}
项目:biojava    文件:Icosahedron.java   
@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;
}
项目:biojava    文件:Tetrahedron.java   
/**
 * 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;
}
项目:biojava    文件:Prism.java   
/**
 * 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;
}
项目:biojava    文件:Prism.java   
@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;
}
项目:biojava    文件:Octahedron.java   
@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;
}
项目:biojava    文件:RectangularPrism.java   
@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;
}
项目:biojava    文件:SpaceGroup.java   
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);
}
项目:jMAVSim    文件:Quadcopter.java   
/**
 * 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);
    }
}
项目:jMAVSim    文件:CameraGimbal2D.java   
@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);
        }
    }
}
项目:Pogamut3    文件:PogamutLocation.java   
/**
 * 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;
}
项目:Pogamut3    文件:PogamutRotation.java   
/**
 * 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;
}
项目:Pogamut3    文件:PogamutRotation.java   
/**
 * 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;
}
项目:Pogamut3    文件:PogamutRotation.java   
/**
 * 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;
}
项目:Pogamut3    文件:AffineTransform3D.java   
public static AffineTransform3D createRotationOx(double theta) {
    Matrix3d matrix3d = new Matrix3d();
    matrix3d.rotX(theta);
    Matrix4d matrix4d = new Matrix4d();
    matrix4d.set(matrix3d);
    return new AffineTransform3D(matrix4d);
}
项目:Pogamut3    文件:AffineTransform3D.java   
public static AffineTransform3D createRotationOy(double theta) {
    Matrix3d matrix3d = new Matrix3d();
    matrix3d.rotY(theta);
    Matrix4d matrix4d = new Matrix4d();
    matrix4d.set(matrix3d);
    return new AffineTransform3D(matrix4d);
}
项目:Pogamut3    文件:AffineTransform3D.java   
public static AffineTransform3D createRotationOz(double theta) {
    Matrix3d matrix3d = new Matrix3d();
    matrix3d.rotZ(theta);
    Matrix4d matrix4d = new Matrix4d();
    matrix4d.set(matrix3d);
    return new AffineTransform3D(matrix4d);
}
项目:Pogamut3    文件:Rotation.java   
/**
 * 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;
}
项目:Pogamut3    文件:Rotation.java   
/**
 * 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;
}
项目:Pogamut3    文件:Rotation.java   
/**
 * 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;
}
项目:Pogamut3    文件:Location.java   
/**
 * 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;
}
项目:Pogamut3    文件:AdvancedLocomotion.java   
/**
 * 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));
}
项目:AquaRegia    文件:VectorUtils.java   
/**
 * 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;
}
项目:BimSPARQL    文件:MVBB.java   
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;
}
项目:rct-java    文件:TransformerCoreDefault.java   
public Vector3d quatRotate(Quat4d r, Vector3d v) {
    Matrix3d rotMat = new Matrix3d();
    rotMat.set(r);
    Vector3d result = new Vector3d();
    rotMat.transform(v, result);
    return result;
}